Software listing for BigFoot

The program incorporates a command processor which interprets high level robot commands stored in EEPROM at the section ‘user routine‘ many routines can appear in the listing but due to memory constraints only one can be active. The rest must be commented out.

'>BigFt-1S
'BigFoot walked 2:45 am Saturday 7 Nov. 98 DLB 7-11-98
'Program for Parallax Stamp BS-1
	READ 255,b0 :DEBUG b0 'b0 =15
'
'History
'-------
'based on BigFt-1
'_WAIT added, _Stop removed, jump changed to _Start, labels changed
'alias rfootat=atpace added
'pE was pu
'Sensors incorporated
'stackmove added
'nomove added
'
'Bugs
'----
'none
'
'DigiFleet servos in protoype
'roll - 100=left 150=stand 200=right nominal
'pace - 100=left(FD) 150=stand 200=right(FD) nominal
SYMBOL servoroll=0 	'servo on pin 0
SYMBOL servopace=1 	'servo on pin 1
SYMBOL S_rtoe =pin2 	'right toe switch on d2 - normally high
SYMBOL S_ltoe =pin3 	'left toe switch on d3 - normally high
SYMBOL touched =0	'p2,p3 state if sensor touched
SYMBOL atroll =b1
SYMBOL roll =b2
SYMBOL atpace =b3
SYMBOL rfootat =b3
SYMBOL pace =b4
SYMBOL atX =b5
SYMBOL toX =b6
SYMBOL servoX =b7
SYMBOL pE =b8 		'pointer to current move in user EEPROM
SYMBOL m =b9 		'current move
SYMBOL i =b10 		'loop counter in init
SYMBOL Sstate =b11      'sensor state; 0=none,1=right,2=left,3=both
SYMBOL Sdo =i		'flag, 1=>act on toe switch sensors
SYMBOL stackmove=b12
SYMBOL r_left =125 	'roll_left
SYMBOL r_stand =155 	'roll_upright
SYMBOL r_right =185 	'roll_right
SYMBOL p_l_fd =120 	'left foot forwards
SYMBOL p_r_bk =p_l_fd
SYMBOL p_stand =145 	'feet together
SYMBOL p_r_fd =180 	'right foot forwards
SYMBOL p_l_bk =p_r_fd
SYMBOL speed =1         'servo increment =1,2(make 'to' even),3+(beware)
			'far too jumpy with speed =2
SYMBOL tweenpulse =10	'with processing overhead =>frame rate of ~20mS
SYMBOL turnRbias =8
SYMBOL turnLbias =5
'---------------- main user routine names ----------
SYMBOL nomove =0 'null move
SYMBOL S =1 'Start, jump back to EEPROM 0
SYMBOL F =2 'Forward
SYMBOL B =3 'Back
SYMBOL R =4 'Right turn
SYMBOL L =5 'Left turn
SYMBOL H =6 'Halt, stand to attention
SYMBOL W =7 'Wait secs in next byte
'---------------- user routine ----------------
'uncomment the desired routine
EEPROM 0,(F,F,F,F,R,R,H,W,2,L,L,B,H,S) 'roughly F & B about same spot
'EEPROM 0,(F,R,F,L,F,S)'roughly F & B about same spot
'EEPROM 0,(H,W,2,F,F,H,W,1,B,B,S)
'EEPROM 0,(W,2,B,H,W,1,F,S)
'EEPROM 0,(H,W,1,F,W,1,F,S)
'EEPROM 0,(R,R,R,R,R,L,L,L,L,L,J,S)
'EEPROM 0,(L,L,S)
'EEPROM 0,(R,R,S)
'EEPROM 0,(R,F,R,F,L,F,L,B,S)
'EEPROM 0,(F,S)
'----------------------------------------------
init: 
	atroll =r_stand
	atpace =p_stand
	' FOR i=0 to 20
	' PULSOUT servoroll,r_stand
	' PULSOUT servopace,p_stand
	' PAUSE tweenpulse
	' NEXT
	stackmove =H
	pE =0                               'set pointer to start of DATA
sense:
	dirs =%11
	' Sstate =pins^255&15/4
	' debug %Sstate
	IF S_rtoe=0 THEN _LT
	IF S_Ltoe=0 THEN _RT
	m =stackmove
	stackmove =nomove

case: 			
	'debug m'"c"                        'CASE movetype
	BRANCH m,(_Read,_Start,_FD,_BK,_RT,_LT,_HALT,_WAIT)
		'illegal value =>fall through
	GOTO init

_Read:
	READ pE,m
	'debug #pE,#m,cr
	pE =pE +1
	GOTO case

_FD:
	'debug atpace,"F"
	IF rfootat<p_stand THEN r_fd         'moverightfoot else moveleft
l_fd:
	pace =p_l_fd                         'move left foot forwards
	GOTO R_roll
r_fd:
	pace =p_r_fd                         'move right foot forwards
	GOTO L_roll

_BK:
	IF rfootat>p_stand THEN r_bk         'moverightfoot else moveleft
l_bk: 
	pace =p_l_bk                         'move left foot backwards
	GOTO R_roll
r_bk: 
	pace =p_r_bk                         'move right foot backwards
	GOTO L_roll

_RT:
	'debug "R"
	IF rfootat>p_stand THEN StackandBK
	pace =p_r_fd                         'swivel right
	GOTO T_rollR

_LT: 	
	'debug "L"
	IF rfootat<p_stand THEN StackandBK
	pace =p_l_fd                         'swivel left
	GOTO T_rollL

StackandBK:                                  'step back and do turn again
	' debug "sB",#pE,cr
	' pE =pE -1
	stackmove =m
	GOTO _BK

_HALT:
	'debug atpace,"H"
	IF rfootat<p_stand THEN r_fd_H

l_fd_H:
	pace =p_stand
	GOTO R_roll

r_fd_H: 
	pace =p_stand
	GOTO L_roll

_WAIT:	
	'DEBUG "W"
	READ pE,m
	pE =pE +1
	SLEEP m
	GOTO sense

_Start: 
	'DEBUG "S",cr
	pE =0
	GOTO sense

'------- end of branch (CASE movetype)

R_roll: 
	roll =r_right
	GOTO move

T_rollL:
	roll =r_stand -turnLbias
	GOTO move

S_roll: 
	roll =r_stand
	GOTO move

T_rollR:
	roll =r_stand +turnRbias
	GOTO move

L_roll: 
	roll =r_left
	'GOTO move

move: 
	'DEBUG "move"
'--------roll onto foot
	toX =roll :GOSUB rmove :atroll =atX
'--------move other foot
	atX =atpace :toX =pace :servoX =servopace
	GOSUB domove :atpace =atX
'--------roll upright
	toX =r_stand :GOSUB rmove :atroll =atX
	GOTO sense
rmove: 
	atX =atroll :servoX =servoroll
domove: 
	IF atX=toX THEN endmove
	IF atX<toX THEN addspeed          'else subtract speed
	atX =atX-speed :GOTO Spulse
addspeed: 
	atX =atX+speed
Spulse: 
	PULSOUT servoX,atX
	PAUSE tweenpulse
	GOTO domove
endmove: 
	RETURN
'------------------------ end program -------------------