'>Hextor_0.BSx
'==============================================================
'{$stamp BS2Sx}
'==============================================================
'Setup program for the servos,
'buttons allow:-
'	legs to be set to 
'		Neutral position - legs up
'			the correct position for attaching the legs 
'			and body to the leg servos as in the html 
'			assembly instructions.
'		standUp position - ie legs down
'	servo positions to be adjusted
'		servos 0 - 11 (legs), alter neutral setting in BOS
'		servos 12,13,14	no setting in BOS to alter.
'==============================================================
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
lcdtx		con	6	'pin to send to LCD
lcdrx		con	7	'pin to receive from LCD
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
arg1		var	byte	'command argument
speednib	var	arg1.highnib
servonib	var	arg1.lownib
arg2		var	byte	'command argument
servo		var	nib	'servo number
servospeed	var	nib
lcdbtns	var	byte	'button state
neutrals	var	byte(15)
servspeed	con	4

'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
	pause 1000			'give time for lcd to get ready

	neutrals(12) =127		'lift servo
	neutrals(13) =127		'gripper servo
	neutrals(14) =127		'auxillary servo

start:
	BOScmnd ="r"			'ride-height
	arg1 =200			'over-ride BOS default of 170
	gosub tellBOScd
	gosub lcdmsg1
dobtn:gosub btns
	if lcdbtns.bit0=1 then neutralpos
	if lcdbtns.bit1=1 then standUp
	if lcdbtns.bit2=1 then setneut
	goto dobtn
'----------------------- leaves ---------------
neutralpos:
	gosub lcdmsg2
	BOScmnd ="N"		'set legs to Neutral position
	gosub tellBOSc
	'now set other three servos to midd position
	servospeed =4		'0 - 15 valid
	BOScmnd =0		'set servo positon
	for servo=12 to 14	'lift,gripper,auxilliary
	  servonib =servo
	  speednib =servospeed
	  arg2 =neutrals(servo)	'servo positions 0 - 255 valid
	  gosub tellBOSccc
	 next
	goto start
'------------------------------
standUp:
	gosub lcdmsg3
	BOScmnd ="U"	'stand up at new ride height of 200
	gosub tellBOSc
	goto start
'------------------------------
setneut:BOScmnd ="?"
	arg1 ="n"		'get neutral values
	gosub tellBOScc
	for servo=0 to 11
	  serin BOSrx,i96n,[wait (" "),dec neutrals(servo)]
	 next
	servo =0
	gosub nobtns		'wait until finger off button
	gosub lcdmsg4		'adjust
	gosub lcdmsg5		'keys
doS:	gosub lcdmsg6		'#servo
	gosub lcdmsg7		'#Spos
	BOScmnd =0		'move servo
	speednib =servspeed
	servonib =servo
	arg2 =neutrals(servo)
	gosub tellBOSccc
	gosub btns
	if lcdbtns.bit0=1 then start
	if lcdbtns.bit1=1 then decN
	if lcdbtns.bit2=1 then incN
	if lcdbtns.bit3=1 then incServ
	goto doS

decN:	if neutrals(servo)=0 then doS
	neutrals(servo) =neutrals(servo)-1
	if servo>11 then doS
	BOScmnd ="n"		'update BOS neutral
	arg1 =servo
	arg2 =neutrals(servo)
	gosub tellBOScdd
	goto doS
	 
incN:	if neutrals(servo)=255 then doS
	neutrals(servo) =neutrals(servo) +1
	if servo>11 then doS
	BOScmnd ="n"		'update BOS neutral
	arg1 =servo
	arg2 =neutrals(servo)
	gosub tellBOScdd
	goto doS
	 
incServ:
	servo =servo +1
	if servo<12 then msg6
	if servo<15 then msg8
	servo =0
msg6: gosub lcdmsg4
	gosub lcdmsg6
	gosub nobtns		'wait until finger off button
	goto doS
msg8: gosub lcdmsg8		'position
	gosub nobtns		'wait until finger off button
	goto doS

'------------- subroutines ---------------------
lcdmsg1:serout lcdtx,i96n,[4,16,64,"Assembly Prog.0 "]
	serout lcdtx,i96n,[16,80,  "Neut Up AdjN    "]
	return
'-----------------------
lcdmsg2:serout lcdtx,i96n,[12,16,64,"Neutral"]
	return
'-----------------------
lcdmsg3:serout lcdtx,i96n,[12,16,64,"Up"]
	return
'-----------------------
lcdmsg4:serout lcdtx,i96n,[1,16,64,"Adj Neutral S=  "]
	return
'-----------------------
lcdmsg5:serout lcdtx,i96n,[16,80,"Quit <   > Servo"]
	return
'-----------------------
lcdmsg6:serout lcdtx,i96n,[16,80,18,"2",dec servo,cr]
	return
'-----------------------
lcdmsg7:serout lcdtx,i96n,[16,89,18,"3",dec neutrals(servo),cr]
	return
'-----------------------
lcdmsg8:serout lcdtx,i96n,[1,16,64,"Move        S=  "]
	return
'-----------------------
nobtns:lcdbtns =0
nbtn:	serout lcdtx,i96n,[27,"K0"]	'ask LCD for button status
	serin lcdrx,i96n,200,nolcd,[lcdbtns]	'get byte
	if lcdbtns.lownib<>0 then nbtn
	return
'-----------------------
btns:	serout lcdtx,i96n,[27,"K0"]	'ask LCD for button status
	serin lcdrx,i96n,200,nolcd,[lcdbtns]	'get byte
nolcd:return
'--------------------
tellBOSc:
	pause 10	'give the BOS time to go into receive mode
	if BOSbusy=1 then tellBOSc	'wait until BOS not busy
	serout BOStx,i96n,[BOScmnd]
	return
'--------------------
tellBOScc:
	pause 10	'give the BOS time to go into receive mode
	if BOSbusy=1 then tellBOScc	'wait until BOS not busy
	serout BOStx,i96n,50,[BOScmnd,arg1]
	return
'--------------------
tellBOScdd:		'send command plus two arguments in decimal
	pause 10	'give the BOS time to go into receive mode
	if BOSbusy=1 then tellBOScdd	'wait until BOS not busy
	serout BOStx,i96n,50,[BOScmnd,dec arg1,cr,dec arg2,cr]  
	return
'--------------------
tellBOScd:		'send command plus one argument in decimal
	pause 10	   'give the BOS time to go into receive mode
	if BOSbusy=1 then tellBOScd	   'wait until BOS not busy
	serout BOStx,i96n,50,[BOScmnd,dec arg1,cr]  '50 pace time
	return
'--------------------
tellBOSccc:if BOSbusy=1 then tellBOSccc
	serout BOStx,i96n,1,[BOScmnd,arg1,arg2]	'pace min 1
	return
'------------------------ end program -------------------------