'>Hextor_4.BSx
'==============================================================
'{$stamp BS2Sx}
'==============================================================
'Introduction of setting:- 
'speed, ride-height, leg-lift, stride-length
's=speed, r=ride-height, l=leg-lift,m=half stride-length.
'Introduction of gait modifiers Q and T.
'
'All the settings remain in operation until new values are sent
'or the robot is powered down.
'
'wiggle1 now has the instruction "s",8, setting the spped to 8 
'and the ride-height instruction "r",170 (the default)
'wiggle2 now has the instruction "s",0, setting the spped to 0
'and the ride-height instruction "r",200
'
's = 0 - 15		only the low nibble of higher values is used
'r = 0 - 255	r<150 means the servos have to work very hard 
'l = 0 - 255	but leg height limited to neutral position
'm = 0 - 127	in practice m>40 leads to unstable walking  
'
'Also wiggle1 now has the gait modifier "Q"
'and wiggle3 has the gait modifier "T".
'
'New
'---
'Required knowledge: lookdown 
'
'BOS Commands: Q,T,s,r,l,m 
'Variable:    arg1
'Subroutine:  tellBC1d
'==============================================================
'Experiment with the s,r,m,l,Q and T commands.
'Note the main loop now executes three sequences.
'==============================================================
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 sequence string
arg1		var	byte	'command argument

'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 statments
'---------------------------
wiggle1	DATA "s",8,"r",170,"QUDFBCSRLOBPMz"
wiggle2	DATA "s",0,"r",200,"UFFBBz"
wiggle3	DATA "TUFRRLLBz"


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

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

dosequence:
	read Sptr,BOScmnd		'loop round until BOScmnd="z"
	if BOScmnd="z" then doneS
	arg1 =255
	lookdown BOScmnd,["srml"],arg1
	if arg1=255 then t1   	'BOScmnd not srm or l so jump to t1
	Sptr =Sptr+1		'point to argument
	read Sptr,arg1		'read it
	gosub tellBOS1d		'send them
	goto nxt		'go onto next operation
t1:	gosub tellBOS
nxt:	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

tellBOS1d:		'send command plus one argument in decimal
	pause 10	'give the BOS time to go into receive mode
	if BOSbusy=1 then tellBOS1d	'wait until BOS not busy
	serout BOStx,i96n,50,[BOScmnd,dec arg1,cr]  'pace time 50
	return
'------------------------ end program -------------------------