'>Hextor11.BSx
'==============================================================
'{$stamp BS2Sx}
'==============================================================
'A new method of specifying Utrasonic Sensor action where 
'action can be specified for greater than the range as well as 
'less than or equal to the range. This allows the robot to 
'follow objects as well as avoid them. 

'New
'---
'New rule added at 'usactions' to allow simple following
'Default action byte for US wander now in DATA.
'
'Required knowledge:
'
'Meta Command: X
'Constants:
'Variables: usaction renamed usactiong, usactionl
'Routine:   uswander rewritten
'==============================================================
'Understand the new routine 'uswander' and experiment with
'the tests in the DATA statements at 'usactions'.
'==============================================================
'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
'==============================================================
'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"

'Ultrasonic sensor
'-----------------
'Sensor position 0=right 127=ahead 255=left
'DATA position1,range1,action1 if >=,action1 if <
'DATA position2,range2,action2 if >=,action2 if <
'DATA ....
'at position, if usRange>range then do first action
'at position, if usRange=<range then do second action
'If action ="X" then ignore that result.
'eg 127,50,"XB" 
'	- ignore test if usRange>50, action=Back if <=50 
'If action ="z" then the action will be ignored by the BOS.
'The default could be "z" so if the robot was sitting or
'standing and no action initiated it would remain as it was.
'End the list with 0


usactions	DATA "S"			'Sit =default action
		DATA 127,60,"FX"		'forward until 60
		DATA 127,50,"XB"		'back if <50
		DATA  77,50,"XL"		'left if Right range<50
		DATA 177,50,"XR"		'right if Left range<50
		DATA 0			'end of list

'--------------------------------------------------------------
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 
i96n		con	16624	'9600 baud, 8 bit, no parity, inverted
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
BOScmnd		var	byte	'byte to send to BOS processor
Sptr		var	byte	'pointer to DATA sequence string
arg1		var	byte	'command argument
irin		var	byte	'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	6	'number of menu options
usdir		var	byte	'us servo direction, 255=left	0=right
usRange		var	byte	'us servo range, 15-255
usRptr		var	byte	'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:toggle Rled
	pause 50
	mendo =0
	gosub lcdmenu		'returns butndo

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

	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

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:	usRptr =usactions	'point to desired actions
	read usRptr,BOScmnd	'read default
	usRptr =usRptr +1
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:
	gosub tellBOS			'now do action
	goto uswander			'do it
'------------- 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],lcdmsg
	gosub lcd
lcdb:	gosub irget
	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 doneR
	arg1 =255
	lookdown BOScmnd,["srml"],arg1
	if arg1=255 then t1	'BOScmnd not srm or l, 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
doneR: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
'------------------------ end program -------------------------