;Basicmicro Robo Claw encoder test code. The following code will
;control a 4wheel robot using the encoders to control speed and
;acceleration / deacceleration.

;Packet Mode - SW3 = ON and SW5 = ON.
;Speed and Accel variables to set the command parameters. 
;Try changing the top speed and accel values to to see how
;the commands work.


Speed 	Var Long
Accel 	Var Byte
CRC 	Var Byte
Address con 128
CMD 	Var Byte

ENABLEHSERIAL
SetHSerial H19200,H8DATABITS,HNOPARITY,H1STOPBITS

Clear
Gosub MixStop ;Stop any previous running command and set speed / accel values
	
;Program starts here. Each label specifies what the command does.
;Refer to the data sheet for command specifics.
	
MixForward
	CMD = 36
	CRC = (Address + CMD + Speed.byte3 + Speed.byte2 + Speed.byte1 + Speed.byte0 + Accel) & 0x7f
	Hserout [Address, CMD, Speed.byte3, Speed.byte2, Speed.byte1, Speed.byte0, Accel, CRC]; Drive forward
	Gosub WaitCMD
	Gosub MixStop
	
MixReverse
	CMD = 37
	CRC = (Address + CMD + Speed.byte3 + Speed.byte2 + Speed.byte1 + Speed.byte0 + Accel) & 0x7f
	Hserout [Address, CMD, Speed.byte3, Speed.byte2, Speed.byte1, Speed.byte0, Accel, CRC]; Stop
	Gosub WaitCMD
	Gosub MixStop
	
	
MixTurnRight
	CMD = 38
	CRC = (Address + CMD + Speed.byte3 + Speed.byte2 + Speed.byte1 + Speed.byte0 + Accel) & 0x7f
	Hserout [Address, CMD, Speed.byte3, Speed.byte2, Speed.byte1, Speed.byte0, Accel, CRC]; Stop
	Gosub WaitCMD
	Gosub MixStop
	
MixTurnLeft
	CMD = 39
	CRC = (Address + CMD + Speed.byte3 + Speed.byte2 + Speed.byte1 + Speed.byte0 + Accel) & 0x7f
	Hserout [Address, CMD, Speed.byte3, Speed.byte2, Speed.byte1, Speed.byte0, Accel, CRC]; Stop
	Gosub WaitCMD
	Gosub MixStop
	
End

WaitCMD
	CRC = 0 ;Clear CRC
	Pause 5000 ;Run CMD for 5 seconds
	Return ;Go back to command after gosub
	
MixStop
	Speed = 0 ;Max pulse per second
	Accel = 30 ;ramp up pulse per second
	CRC = (Address + CMD + Speed.byte3 + Speed.byte2 + Speed.byte1 + Speed.byte0 + Accel) & 0x7f
	Hserout [Address, CMD, Speed.byte3, Speed.byte2, Speed.byte1, Speed.byte0, Accel, CRC]; Stop
	Pause 5000
	
SetValues
	Speed = 20000 ;Top speed / max pulse per second
	Accel = 50 ;Ramp up / down pulse per second
	Return