'-------------------------------------------------------------------------------
Option Explicit
' This module is used for communicating directly with ActivMedia Robots
' like the AmigoBot and the Pioneer over their serial interface

' Input and Output buffer sizes
Private Const InputBufferSize As Integer = 10		' 1-byte buffer.
Private Const OutputBufferSize As Integer = 10	' 1-byte buffer.
' Create input and output buffers
Private InputBuffer(1 To InputBufferSize) As Byte
Private OutputBuffer(1 To OutputBufferSize) As Byte
' Argument Types
Public Const sfARGINT			As Byte = &h3B
Public Const sfARGSTR			As Byte = &h2B
' Communication Syncronization
Public Const ARCMD_SYNC0		As Byte = 0
Public Const ARCMD_SYNC1		As Byte = 1
Public Const ARCMD_SYNC2		As Byte = 2
' Command List
Public Const ARCMD_PULSE		As Byte = 0
Public Const ARCMD_OPEN		As Byte = 1
Public Const ARCMD_CLOSE		As Byte = 2
Public Const ARCMD_POLLING		As Byte = 3
Public Const ARCMD_ENABLE		As Byte = 4
Public Const ARCMD_SETA		As Byte = 5
Public Const ARCMD_SETV		As Byte = 6
Public Const ARCMD_SETO		As Byte = 7
Public Const ARCMD_SETRV		As Byte = 10
Public Const ARCMD_VEL			As Byte = 11
Public Const ARCMD_HEAD		As Byte = 12
Public Const ARCMD_DHEAD		As Byte = 13
Public Const ARCMD_SAY			As Byte = 15
Public Const ARCMD_CONFIG		As Byte = 18
Public Const ARCMD_ENCODER		As Byte = 19
Public Const ARCMD_RVEL		As Byte = 21
Public Const ARCMD_SETRA		As Byte = 23
Public Const ARCMD_SONAR		As Byte = 28
Public Const ARCMD_STOP		As Byte = 29
Public Const ARCMD_DIGOUT		As Byte = 30
Public Const ARCMD_VEL2		As Byte = 32
Public Const ARCMD_GRIPPER		As Byte = 33
Public Const ARCMD_ADSEL		As Byte = 35
Public Const ARCMD_GRIPPERVAL	As Byte = 36
Public Const ARCMD_PTUPOS		As Byte = 41
Public Const ARCMD_TTY2		As Byte = 42
Public Const ARCMD_GETAUX		As Byte = 35
Public Const ARCMD_TCM2		As Byte = 45
Public Const ARCMD_BUMPSTALL	As Byte = 44
Public Const ARCMD_E_STOP		As Byte = 55
Public Const ARCMD_SOUND		As Byte = 90
Public Const ARCMD_SOUNDTOG		As Byte = 92
'-------------------------------------------------------------------------------
Public Sub ArRobotInit(ByVal PortNumber As Byte)
' Initialize communications to robot
	' setup output buffer
    	Call OpenQueue(InputBuffer, InputBufferSize)
	' setup input buffer
	Call OpenQueue(OutputBuffer, OutputBufferSize)
	' open the serial port at 9600,8,N,1
	Call OpenCom(PortNumber, 9600, InputBuffer, OutputBuffer)
End Sub
'-------------------------------------------------------------------------------
Public Sub ArRobotOpenConnection()
' Open the connection with the robot
	ArRobotSendCommand(ARCMD_CLOSE)
	Call Delay(0.1)
	ArRobotSendCommand(ARCMD_SYNC0)
	Call Delay(0.1)
	ArRobotSendCommand(ARCMD_SYNC1)
	Call Delay(0.1)
	ArRobotSendCommand(ARCMD_SYNC2)
	Call Delay(0.1)
End Sub
'-------------------------------------------------------------------------------
Public Sub ArRobotCloseConnection()
' Close the connection with the robot
	ArRobotSendCommand(ARCMD_CLOSE)
End Sub
'-------------------------------------------------------------------------------
Public Sub ArRobotSendCommand(ByVal command As Byte)
' Send a command (with no argument) to the robot
	' send header
	Call ArRobotPutByte(&hFA)
	Call ArRobotPutByte(&hFB)
	' send length
	Call ArRobotPutByte(3)
	' send user data
	Call ArRobotPutByte(command)
	' send checksum
	Call ArRobotPutByte(0)
	Call ArRobotPutByte(command)
	' done
End Sub
'-------------------------------------------------------------------------------
Public Sub ArRobotSendCommandInt(ByVal command As Byte, ByVal argint As Integer)
' Send a command with integer argument to the robot
	Dim checksum As New UnsignedInteger
	' send header
	Call ArRobotPutByte(&hFA)
	Call ArRobotPutByte(&hFB)
	' send length
	Call ArRobotPutByte(6)
	' send user data
	Call ArRobotPutByte(command)
	Call ArRobotPutByte(sfARGINT)
	Call ArRobotPutByte(CByte(CuInt(argint)))
	Call ArRobotPutByte(CByte(CuInt(argint)\256))
	' send checksum
	checksum = 0
	checksum = checksum + CuInt(command*256) + CuInt(sfARGINT)
	checksum = checksum + (CuInt(argint)\256) + ((CuInt(argint) And &H00FF)*256)
	Call ArRobotPutByte(CByte(checksum\256))
	Call ArRobotPutByte(CByte(checksum))
	'Call ArRobotPutByte(command + CByte(CuInt(argint)))
	'Call ArRobotPutByte(sfARGINT + CByte(CuInt(argint)\256))
	' done
End Sub
'-------------------------------------------------------------------------------
Public Sub ArRobotSend(ByVal packetlength As Byte, ByRef packetdata() As Byte)
' Send a custom packet to the robot
	Dim i As Byte
	Dim checksum As New UnsignedInteger
	' send header
	Call ArRobotPutByte(&hFA)
	Call ArRobotPutByte(&hFB)
	' send length
	Call ArRobotPutByte(packetlength+2)
	' send user data
	For i = 1 To (packetlength\2)
		Call ArRobotPutByte(packetdata(i))
		Call ArRobotPutByte(packetdata(i+1))
		checksum = checksum + (CuInt(packetdata(i))*256) + CuInt(packetdata(i+1))
	Next
	' send checksum
	Call ArRobotPutByte(CByte(checksum\256))
	Call ArRobotPutByte(CByte(checksum))
	' done
End Sub
'-------------------------------------------------------------------------------
Private Sub ArRobotPutByte(ByVal data As Byte)
' Send a single byte to the robot
	Call PutQueue(OutputBuffer, data, 1)
End Sub
'-------------------------------------------------------------------------------