'>Hextor15.BSx
'==============================================================
'{$stamp BS2Sx}
'==============================================================
'Introducing the concept of Behaviour Controllers.
'
'As we have seen the program evolve up to Hextor_14.BS2 it has
'developed two areas with distinct modes of operation:- 
'1 - Infra-Red/Pendant buttons Behaviour Controller
'	In this section it is the received infra-red commands 
'	which control program flow, running each sequence as the
'	user desires with an option to jump to section 2.
'2 - Ultrasonic Behaviour Controller
'	In this section it is the information from the ultrasonic
'	sensor which directs program flow depending on the rules
'	in the DATA statements at 'usrules'. Most of the time the
'	program looks to see if there are any infra-red commands
'	whereupon it jumps back to section 1. 
'
'In this program a third Behaviour Controller is
'added
'3 - Direct BOS Control
'	In this section it is the program statements as written 
'	by the user which control the flow. Each time through the
'	loop the program looks to see if there are any infra-red
'	commands whereupon it jumps back to section 1. 
'	This section allows you to use all the BOS commands at
'	will.
'
'New
'---
'Key 8 used to jump to Direct BOS Control.
'
'Required knowledge: commands for direct control of the servos
'
'Constants:	lcdoptions altered
'Variables: Rptr Sptr usRptr all changed to word variables
'Routine:	usdo modified to read both bytes of Sptr
'Subroutine:lcdmenu altered for dm8 
'Sequence:
'LCD text:	dm1 - dm6 altered,dm8
'==============================================================
'Experiment with different BOS commands.
'==============================================================
'Direct Control
'turns on servo power and exits with servo power on
'BOSbusy goes low immediately, doesn't wait for move to finish
'$speed;$servo - high nibble and low nibble respectively
'  	BOScmnd,arg1.arg2
'	0,$speed;$servo,position
'	1,$XX,$servo,X 	->P	actual servo position 
'	2,$XX,$servo,X 	->S	servo status, bit1-on bit0-done 
'	3,X,X	->hiF,then loF	two bytes all servo moved status
'	4,X,X	->hiR,then loR	two bytes all servo on/off status
'	5,$XX;$servo,X		stop pulsing servo
'
'-------- Legs & Logical-Servos --------
'
' Plan view of Hextor	S - Servo(0 - 13)	L - Legs(1 - 6)
'	 S    L   L    S
'	       ( )	gripper 13
'	       | |	lift    12
'	    L---F---R		LEG NUMBERS
'	1,0 | 1   2 | 6,7	1=LF	2=RF	,L - left, R - Right
'	3,2 | 3   4 | 8,9	3=LM	4=RM	,M - mid,  F - Front
'	5,4 | 5   6 | 10,11	5=LB	6=RB	,B - Back
'	    \-------/
'
'-------- Servo values and leg positions -------
'	f - forward	m - middle	b - backward
'Left	Lf=255		Lm=127		Lb=0
'Right	Rf=0		Rm=127		Rb=255
'
'	d - Down	u - Up
'Left	Ld=0		Lu=255
'Right	Rd=255		Ru=0
'
'-------- Arm and Gripper positions -------
'Arm up 255
'Arm down 0
'Gripper open 255
'Gripper closed 0
'==============================================================
'Sequences in DATA statments
'---------------------------
wiggle1		DATA "UDFBCRLOBPMz"
wiggle2		DATA "UFFBBz"
wiggle3		DATA "UFRRLLBz"
setting1	DATA "Qs",4,"r",170,"z"
setting2	DATA "Ts",0,"r",200,"z"	'feet drag if not "T"
tround		DATA "RRRRRRRRRRRRz"
beg		DATA "s",4,"U"	'speed 4, Up
		DATA 0,$43,127	'speed 4, servo 3, leg 3 up
		DATA 0,$49,127	'speed 4, servo 9, leg 4 up
		DATA 0,$42,170	'speed 4, servo 2, leg 3 forward
		DATA 0,$48,90	'speed 4, servo 8, leg 4 forward
		DATA "WU"		'wait for move to finish then Up
		DATA 0,$25,255	'speed 2, servo 5, leg 5 up
		DATA 0,$2B,0	'speed 2, servo 11, leg 6 up
		DATA 0,$F1,255	'speed 15, servo 1, leg 1 up
		DATA 0,$F7,0	'speed 15, servo 7, leg 2 up
		DATA 0,$40,200	'speed 4, servo 0, leg 1 forward
		DATA 0,$46,50	'speed 4, servo 6, leg 2 forward
		DATA "s",2,"WUz"	'speed 2, wait, Up, end of list



'Ultrasonic sensor
'-----------------
'"a" - Action sequence
'Rules - DATA direction,range,action if greater,action if less 

usaction	DATA "S"		'default action
ussequence	DATA word tround	'Action-sequence
usrules		DATA usF,	20,"XB"	'back if <=20
		DATA usR2,	20,"XL"	'left if <=20
		DATA usL2,	20,"XR"	'right if <=20
		DATA usF,	25,"Xa"	'Action if <=25
		DATA usR2,	25,"Xa"	'Action if <=25
		DATA usL2,	25,"Xa"	'Action if <=25
		DATA usF,	80,"XF"	'forward if <80
		DATA usR2,	80,"XR"	'right if <80
		DATA usL2,	80,"XL"	'left if <80
		DATA 0			'end of list
'==============================================================
'LCD commands.
'For more detailed information see the ILM-216 User Manual.
'Function						ASCII
'--------						-----
'Null							0
'Cursor home						1
'Hide cursor						4
'Show underline cursor					5
'Show blinking-block cursor				6
'Bell (not implemented with Hextor LCD)			7
'Backspace						8
'Horezontal tab (4 columns)				9
'Smart linefeed (cursor down one line)			10
'Vertical tab (cursor up one line)			11
'Formfeed (clear screen)				12
'Carriage return					13
'Backlight on						14
'Backlight off						15
'Accept cursor position entry				16
'	64 is the start of the first line
'	80 is the start of the second line
'Format right aligned text				18
'Escape (ESC; start multipart instruction)		27
'==========LCD messages - finish each message with Null =======
dbtxt	data	12,4,16,80,"back <    >  ok ",0
dbusy	data	16,76,"busy",0
dm0	data	1,"OPTIONS         ",0
dm1	data	1,"Wiggle 1        ",0
dm2	data	1,"Wiggle 2        ",0
dm3	data	1,"Wiggle 3        ",0
dm4	data	1,"Setting 1       ",0
dm5	data	1,"Setting 2       ",0
dm6	data	1,"US wander       ",0
dm7	data	1,"Beg             ",0
dm8	data	1,"BOS Control     ",0
'==============================================================
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
irrx		con	5	'pin infra red comms in, i96n
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
ustx		con	14	'pin ultrasonic commands,	i96n
usrx		con	15	'pin ultrasonic data in,	i96n 
poweruptime	con	1500	'pause before powering up electronics
ustout		con	10000	'timeout min 6000 for 0 to 255 travel
usmin		con	10	'right,increase if servo hits end stop 
usahead		con	127	'alter so servo points straight ahead
usmax		con	255	'left, decrease if servo hits end stop
i96n		con	16624	'9600 baud, 8 bit, no parity, inverted
'us servo directions, 0 is a reserved direction for end of list
usR3		con	27	'us servo direction Right a lot
usR2		con	77	'us servo direction Right middle
usR1		con	107	'us servo direction Right a little
usF		con	127	'us servo direction Forward
usL1		con	147	'us servo direction Left a little
usL2		con	177	'us servo direction Left middle
usL3		con	227	'us servo direction Left a lot
BOScmnd		var	byte	'byte to send to BOS processor
Sptr		var	word	'pointer to DATA sequence string
Sptrl		var	Sptr.lowbyte
Sptrh		var	Sptr.highbyte
arg1		var	byte	'command argument
arg2		var	byte	'command argument
irin		var	byte	'infra-red command
oldirin		var	byte	'old infra-red command
lcdbtns		var	byte	'button states are in lower nibble
btn1state	var	lcdbtns.bit0
btn2state	var	lcdbtns.bit1
btn3state	var	lcdbtns.bit2
btn4state	var	lcdbtns.bit3
btn		var	lcdbtns.lownib	'0 - 4 current button state
oldbtn		var	nib	'0 - 4 last button state  
lcdmsg		var	word	'pointer to current char in text 
lcdchar		var	byte	'character to send to LCD
btnc		var	nib	'current BuTtoN Choice from menu
mendo		var	nib	'selection chosen by OK
lcdoptions	con	8	'number of menu options
usdir		var	byte	'us servo direction, 255=left	0=right
usRange		var	byte	'us servo range, 15-255
usRptr		var	word	'pointer to DATA us Rules
usRrange	var	byte	'range to trigger action
usactiong	var	byte	'action byte if > triggered
usactionl	var	byte	'action byte if <= triggered

'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
init1:gosub irget
	pause 100		'no point in listening too often
	if irin<>0 then init1	'wait for ir to initialise
	debug cls
	pause 1000		'give time for lcd to get ready
'IR circuit sometimes starts up with a false key of 2 
	gosub irget
	pause 100		'no point in listening too often
	if irin<>0 then init1	'wait for false key to end
	lcdmsg =dbtxt		:gosub lcd	'write button text


'Program start
'-------------
start:
'---- Main Behaviour Controller - Infra-Red/Pendant buttons ---
ircon:toggle Rled
	pause 50
	mendo =0
	gosub lcdmenu		'returns mendo

case:	if mendo=1 then wig1		'key/mendo 1
	if mendo=2 then wig2		'key/mendo 2
	if mendo=3 then wig3		'key/mendo 3
	if mendo=4 then set1		'key/mendo 4
	if mendo=5 then set2		'key/mendo 5
	if mendo=6 then uswander	'key/mendo 6
	if mendo=7 then dobeg		'key/mendo 7
	if mendo=8 then BOScon		'key/mendo 8

	toggle Lled
	pause 50
	goto start

'------------- leaves --------------------------
wig1:	Sptr =wiggle1
	lcdmsg =dm1		:gosub lcd
	lcdmsg =dbusy	:gosub lcd
	gosub dosequence
	goto start

wig2:	Sptr =wiggle2
	lcdmsg =dm2		:gosub lcd
	lcdmsg =dbusy	:gosub lcd
	gosub dosequence
	goto start

wig3:	Sptr =wiggle3
	lcdmsg =dm3		:gosub lcd
	lcdmsg =dbusy	:gosub lcd
	gosub dosequence
	goto start

set1:	Sptr =setting1
	lcdmsg =dm4		:gosub lcd
	lcdmsg =dbusy	:gosub lcd
	gosub dosequence
	goto start

set2:	Sptr =setting2
	lcdmsg =dm5		:gosub lcd
	lcdmsg =dbusy	:gosub lcd
	gosub dosequence
	goto start

dobeg:Sptr =beg
	lcdmsg =dm7		:gosub lcd
	lcdmsg =dbusy	:gosub lcd
	gosub dosequence
	goto start

'---- Behaviour Controller - Ultrasonic Sensor ----
uswander:
	lcdmsg =dm6		:gosub lcd
	lcdmsg =dbusy	:gosub lcd
	gosub btns		'check pendant buttons
	if btn<>0 then start	'other command wanted
	gosub irget		'check ir
	if irin=6 then usdo	'for us
	if irin<>0 then start	'other ir command received
usdo:	read usaction,BOScmnd	'read default
	read ussequence,Sptrl	'read pointer to sequence
	read (ussequence+1),Sptrh	'read pointer to sequence
	usRptr =usrules		'point to desired rules
ustest:
	read usRptr,usdir
	if usdir=0 then usmove		'end of list, do default
	usRptr =usRptr +1
	read usRptr,usRrange
	usRptr =usRptr +1
	read usRptr,usactiong
	usRptr =usRptr +1
	read usRptr,usactionl
	usRptr =usRptr +1
'debug dec usdir,dec usRrange,usactiong,usactionl,cr 
	gosub usget
	if usRange>usRrange then usg
usl:	if usactionl="X" then ustest	'ignore it
	BOScmnd =usactionl		'action triggered
	goto usmove			'do it
usg:	if usactiong="X" then ustest	'ignore it
	BOScmnd =usactiong		'action triggered
usmove:
	if BOScmnd="a" then sact
	gosub tellBOS		'now do action
	goto uswander		'do it
sact: gosub dosequence
	goto uswander

'---- Behaviour Controller - Direct to BOS ----
'	turns on servo power and exits with servo power on
'	$speed;$servo - high nibble and low nibble respectively
'	0,$speed;$servo,position
'	1,$XX,$servo,X 	->P	actual servo position 
'	2,$XX,$servo,X 	->S	servo status, bit1-on bit0-done
'	3,X,X	->hiF,then loF	two bytes all servo moved status
'	4,X,X	->hiR,then loR	two bytes all servo on/off status
'	5,$XX;$servo,X		stop pulsing servo

BOScon:
	lcdmsg =dm8	:gosub lcd
	gosub irget			'check ir
	gosub btns			'check pendant buttons
	gosub irget			'check ir
	if irin=8 then BOSdo	'for us
	if irin<>0 then start	'other ir command received
	if btn<>0 then start	'other command wanted
BOSdo:BOScmnd =0
	arg1 =$90
	arg2 =200
	gosub tellBOS3	'move 0
	for arg2 =0 to 9	'use arg2 as a temp variable
	  BOScmnd =1
	  arg1 =0
	  gosub tellBOS3	'ask position
	  gosub BOSreply1
  debug"pulsing", bin8 BOScmnd,bin8 arg1,cr
	  pause 20
	 next
	BOScmnd =4
	gosub tellBOS3	'ask pulsing
	gosub BOSreply2
  debug"pulsing", bin8 BOScmnd,bin8 arg1,cr
	BOScmnd =5
	arg1 =0
	gosub tellBOS3	'stop pulsing
	pause 1000
	BOScmnd =4
	gosub tellBOS3	'ask pulsing
	gosub BOSreply2
  debug"pulsing", bin8 BOScmnd,bin8 arg1,cr
	BOScmnd =0
	arg1 =$30
	arg2 =50
	gosub tellBOS3	'move 0
	pause 150
	BOScmnd =3
	gosub tellBOS3	'ask finished
	gosub BOSreply2
  debug"finished", bin8 BOScmnd,bin8 arg1,cr
	pause 2000
	BOScmnd =3
	gosub tellBOS3	'ask finished
	gosub BOSreply2
  debug"finished", bin8 BOScmnd,bin8 arg1,cr
	goto BOScon

'------------- subroutines ---------------------
usget:'talk to UltraSonic co-processor
	usRange =0			'invalid value, used if no repyl
	usdir =usdir min usmin max usmax	'don't hit end stops
usout:serout ustx,i96n,["T",usdir]
	serin usrx,i96n,ustout,us1,[usRange]  'jump if no reply
	debug dec usdir," ",dec usRange,"   ",cr
	return
us1:	debug "no reply from ultrasonics",cr
	return
'--------------------
lcdmenu:
lcdit:lookup btnc,[dm0,dm1,dm2,dm3,dm4,dm5,dm6,dm7,dm8],lcdmsg
	gosub lcd
lcdb:	gosub irget
	if irin=oldirin then mbtns
	oldirin =irin
	if irin=0 then mbtns
	mendo =irin
	return				'with ir cmdn
mbtns:gosub btns
	if btn=oldbtn then lcdb
	oldbtn =btn
	if btn=2 then lcd2
	if btn=3 then lcd3
	if btn=4 then lcd4
	goto lcdb

lcd2: btnc =1 +btnc -1 min 1 -1	:goto lcdit
	'+1 -1 => don't go through 0
lcd3:	btnc =btnc +1 max lcdoptions	:goto lcdit
lcd4:	mendo =btnc	
	lcdmsg =dbusy	:gosub lcd
	gosub nobtns
	return
'--------------------
lcd:	read lcdmsg,lcdchar
	if lcdchar=0 then lcdret	'if 0 then end of msg
	serout lcdtx,i96n,[lcdchar]
	lcdmsg =lcdmsg +1		'point to next character
	goto lcd
lcdret:return
'--------------------
btns:	lcdbtns =0			'set a default value
	serout lcdtx,i96n,[27,"K0"]	'ask LCD for button status
	serin lcdrx,i96n,200,nolcd,[lcdbtns]	'get byte
nolcd:btn =NCD lcdbtns.lownib 		'0 - 4
	return
'-----------------------
nobtns:lcdbtns =0		'wait until no buttons are pressed
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
'--------------------
irget:'listen to InfraRed co-processor
	irin =0					'set to idle value
	serin irrx,i96n,100,noir,[irin]	'valid 0 - 8
	irin =irin & $F		'mask high nibble timing data
noir:	return
'--------------------
dosequence:				'loop round until BCbyte="z"
	read Sptr,BOScmnd
	if BOScmnd="z" then doneS
	Sptr =Sptr+1		'point to command
	if BOScmnd<6 then Scon
	arg1 =255
	lookdown BOScmnd,["srml"],arg1
	if arg1=255 then t1	'BOScmnd not srm or l, jump to t1
	read Sptr,arg1		'read it
	Sptr =Sptr +1
	gosub tellBOS1d		'send them
	goto dosequence
t1:	gosub tellBOS
	goto dosequence
Scon:	read Sptr,arg1
	Sptr =Sptr +1
	read Sptr,arg2
	Sptr =Sptr +1
	gosub tellBOS3	'wait until not busy then send 3 bytes
	if BOScmnd=0 then dosequence
	if BOScmnd>2 then rep2
rep1:	gosub BOSreply1		'reply not used
	goto dosequence 
rep2:	gosub BOSreply2		'reply not used
	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]  '50 pace time
	return
'--------------------
tellBOS3:if BOSbusy=1 then tellBOS3
	serout BOStx,i96n,1,[BOScmnd,arg1,arg2]	'pace min 1
	return
'--------------------
BOSreply1:			'wait min 40
	serin BOSrx,i96n,50,BOSr1,[arg1]	'get byte
BOSr1:	return
'--------------------
BOSreply2:			'wait min 210
				'get highbyte,lowbyte of flags
	serin BOSrx,i96n,250,BOSr2,[arg1,arg2]
BOSr2:	return
'------------------------ end program -------------------------