'__________________________________________________________________________
'			BI-PED PROGRAM FOR STAMP1
' de R‚gis CHEVREUIL					version 1.1
'Septembre 1999
'__________________________________________________________________________
'	This robot use two servos, one connected to the body makes it slant
'to the right or left, thus raising the opposite feet.
'the second is connected to both legs, when the right feet goes forward
'the left goes backward, and vice versa.
'	To walk one step forward, it needs to slant to one side to raise 
'the opposite feet. To move this feet forward, slant to the other side 
' and thus move the other feet which looks more like a penguin than
'a tyranausorus rex ! 
'	To turn, it needs to move the feet (one forward thus the other
' backward) while they are both on the ground so both feet slip and the
'robot turns.
'	So far, it works  well on flat and horizontal (!)  ceramic floor or
'carpet. If you go for a holiday in Pisa (Italy) please don't bring it with
'you on your visit to the tower !
'__________________________________________________________________________
'				CONSTANTS :
'				
symbol av=0			'port servo legs
symbol lat=1			'port servo body
symbol sw=5			'port detection switches
symbol so=6			'port buzzer piezo
symbol led=7			'port led
symbol latd=200			'max position lateral right
symbol latg=90			'max position lateral left
symbol lat0=146 		'position lateral 0=straight
symbol avg=90			'max position left leg forward(right back.)
symbol avd=190 			'max position right leg forward(left back.)
symbol av0=140			'center legs
symbol speed=4			'speed of body+legs movements

'__________________________________________________________________________
'				MAIN LOOP :
main:				
	gosub lr		'gosub lateral right loop
	gosub fl		'gosub left leg forward loop
	gosub test		'gosub test loop (legs switches)
	gosub ll		'gosub lateral left loop
	gosub fr		'gosub right leg forward loop
	gosub test		'gosub test loop (legs switches)
				'THIS IS ONE FULL STEP FORWARD !
goto main			'(test the switches after each movement
				'forward of the feet)
'_________________________________________________________________________
'				STOP POSITION LOOP

stop:					
	for b0=1 to 10		'10 loops to be sure to reach 0 position
	pulsout lat,lat0	'body servo to position 0 (straight)
	pulsout av, av0		'legs servo to position 0 (parallel)
	pause 20		'THIS POSITION IS NOT USED YET !
return

'_________________________________________________________________________
'				STRAIGHT POSITION LOOP


l0:				'body straight position,do not mind legs ! 
	for b0=1 to 25  	'this position is use only for turns
				'25 loop to stabilize the robot before
	pulsout lat,lat0	'turning.
	pause 20
	next
	for b2=1 to  30		'this loop is to emit a bip and light,
	pulsout so,90		'really useful because you know when the 
	pulsout led,250		'robot will starts to avoid objects,as this 
	next			'position is a part of the turning process
return

'_________________________________________________________________________
'				LATERAL RIGHT POSITION LOOP


lr:			
	for b0=latg to latd  step speed	     'from max left to max right  
	pulsout lat,b0		'the change of the speed factor will make 
	pause 20		'movement faster or slower,same to all the 
	next			'following loops.
	pause 30
return

'__________________________________________________________________________
'				LATERAL LEFT POSITION LOOP


ll:						'same as before for left
						' position movement
	for b0=latd to latg  step -speed	'the sign - because the b0
	pulsout lat,b0				'value is decreasing !
	pause 20
	next
	pause 30
return:

'__________________________________________________________________________
'				RIGHT LEG FORWARD LOOP


fr:				         'same loop for right leg forward
	for b0=avg  to avd  step speed
	pulsout av,b0
	pause 20
	next
	pause 30
return
'_________________________________________________________________________
'				LEFT LEG FORWARD LOOP


fl:				         'same loop for left leg forward
	for b0=avd to avg  step -speed
	pulsout av,b0
	pause 20
	next
	pause 30
return
'_________________________________________________________________________
'				TURN RIGHT LOOP



turnr:				'this is the loop for avoiding object
for b1 =1 to 12			'while going backward and turning right
	gosub lr		'body to the right
	gosub fr		'right leg forward
	gosub l0		'body straight
	gosub fl		'left leg forward as they are both on the
   				'ground making the robot turn by "shuffing"
next b1
return
'________________________________________________________________________
'				TURN LEFT LOOP


turnl:				'same as before for backward and left turn 
for b1 =1 to 12			'depending on which feet (switch) detect 
	gosub ll		'object
	gosub fl
	gosub l0
	gosub fr
	
next b1
return
'________________________________________________________________________
'				SWITCHES TEST LOOP

				'one switch on each feet is parallel to 
test:				'a different resistor and both serial connected
	pot sw,6,b2		'to input pin 5 the use of Pot instruction to
	if b2 > 12 and b2< 18 then turnr	'mesure the value on pin 5
	if b2 >= 19 then turnl			'will tell wich of the switch
return						'is activated