'This program uses the I2C Object 
'to communicate with a Nubotics WC-132
'WheelCommander differential drive controller


Dim wc As New oI2CM
Dim wd As Word
Dim b As Byte
Const SYNC = 46      ' 0x2E
Const ECHO = 69      ' 0x45
Const NAME = 78      ' 0x4E
Const STATUS = 83    ' 0x53
Const SET_CONST = 70 ' 0x46
Const DELTA_DIST = 88' 0x55
Const RUN = 71       ' 0x47

Const ACK = 97       ' 0x61
Const NACK = 110     ' 0x6E

T As oTone(21)


Sub Main()

  wc.Node = 16
  wc.NoInc = cvTrue

  Call Start
  
' synchronize with WC-132
  Call Send1(SYNC)
  b = Get1	' should return SYNC
  If b <> SYNC Then
  	Call Alarm
  End If
  
' set mode to short mode, counter axis rotation, servos
  Call Send3(SET_CONST, 2, 132)
  b = Get1  ' should return ACK
  If b <> ACK Then
  	Call Alarm
  End If

' make it move!
  Call Send4(DELTA_DIST, 0, 1, 0)
  b = Get1 ' should return ACK
  If b <> ACK Then
  	Call Alarm
  End If
  
  Call Send1(RUN)
  b = Get1 ' should return ACK
  If b <> ACK Then
  	Call Alarm
  Else
  	Call Pass
  End If
  
End Sub


Sub Alarm()
  T.Operate.TurnOn
  Do 5 Times
    T.Value = 120
    ooPIC.Delay = 100
    T.Value = 127
    ooPIC.Delay = 100
  Loop
  T.Operate.TurnOff
  Delay = 2000
End Sub

Sub Pass()
  T.Operate.TurnOn
  T.Value = 127
  ooPIC.Delay = 1000
  T.Operate.TurnOff
  Delay = 2000
End Sub

Sub Start()
  T.Operate.TurnOn
  T.Value = 80
  ooPIC.Delay = 1000
  T.Operate.TurnOff
  Delay = 5000
End Sub


Sub Send1(cmd As Byte)
	wc.Mode = cv7Bit
	wc.Width = cv8Bit
	wc.Value = cmd
End Sub

Sub Send2(cmd As Byte, cmd2 As Byte)
	wc.Mode = cv7Bit
	wc.Width = cv16Bit
	wd = cmd + cmd2 * 256
	wc.Value = wd
End Sub

Sub Send3(cmd As Byte, cmd2 As Byte, cmd3 As Byte)
	wc.Mode = cv10Bit
	wc.Width = cv16Bit
	wc.Location = cmd
	wd = cmd2 + cmd3 * 256
	wc.Value = wd
End Sub

Sub Send4(cmd1 As Byte, cmd2 As Byte, cmd3 As Byte, cmd4 As Byte)
	wc.Mode = cv23Bit
	wc.Width = cv16Bit
	wd = cmd1 + cmd2 * 256
	wc.Location = wd
	wd = cmd3 + cmd4 * 256
	wc.Value = wd
End Sub

Function Get1() As Byte
	wc.Mode = cv7Bit
	wc.Width = cv8Bit
	Get1 = wc.Value
End Function

Function Get2() As Word
	wc.Mode = cv7Bit
	wc.Width = cv16Bit
	Get2 = wc.Value
End Function