'>Hextor_3.BSx
'==============================================================
'{$stamp BS2Sx}
'==============================================================
'Introduction of sequences held in DATA statements
'and a simple routine to read the moves.
'
'New
'---
'Required knowledge: DATA, read, if then

'Meta Command:	z
'Variable:   	Rptr
'Subroutine:	dosequence
'==============================================================
'Now instead of writing lines of Basic to add commands, the
'commands are held in DATA staements so they are easy to alter.
'The main routine does one sequence and then the other.
'Experiment with changing the commands in the sequences and add
'your own, setting Sptr and then gosub(ing) in the start loop. 
'==============================================================
BOStx		con	0	'pin serial to BOS processor
BOSrx		con	1	'pin serial from BOS processor
epower		con	2	'pin high turns on power to electronics
BOSbusy		var	in4	'pin BOS processor busy => high
Rled		con	8	'pin high turns on right green led
Lled		con	9	'pin high turns on left red led
i96n		con	16624	'9600 baud, 8 bit, no parity, inverted
poweruptime	con	1500	'pause before powering up electronics
BOScmnd		var	byte	'byte to send to BOS processor
Sptr		var	byte	'pointer to DATA routine string

'during downloading the power to the electronics is turned off,
'if it is turned back on again too soon the electronics, 
'especially the ultrasonics do not power up properly,
'so wait poweruptime before turning power on.

init:	output Rled
	output Lled
	high Rled
	high Lled
	pause poweruptime
	high epower			'turn on electronics
	debug cls


'Sequences in DATA statements
'---------------------------
'end sequences with a z indicating end of string

'Stand up, down gripper, step forward, close gripper, step 
'right, step left, open gripper, step back, lift gripper, 
'legs to middle position


wiggle1	DATA "UDFCRLOBPMz"

'Stand up, 3 steps forward, 3steps backward

wiggle2	DATA "UFFFBBBz"


'Program start
'-------------

start:toggle Rled
	Sptr =wiggle1
	gosub dosequence
	Sptr =wiggle2
	gosub dosequence
	toggle Lled
	pause 2000
	goto start			'do it forever

dosequence:
	read SPtr,BOScmnd
	if BOScmnd="z" then doneS   'loop round until BOScmnd="z"
	gosub tellBOS
	Sptr =Sptr+1
	goto dosequence
doneS:return

tellBOS:
	pause 10	'give the BOS time to go into receive mode
	if BOSbusy=1 then tellBOS	'wait until BOS not busy
	serout BOStx,i96n,[BOScmnd]
	return
'------------------------ end program -------------------------