' BI-PED PROGRAM FOR STAMP1
' de Regis 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=4 'port detection switches
symbol eye=6 'port infra-red eye
symbol so=7 'port buzzer piezo
symbol led=3 'port led
symbol latd=200 'max position lateral right
symbol latg=75 'max position lateral left
symbol lat0=133 '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
symbol out=7
'__________________________________________________________________________
' MAIN LOOP :
main:
gosub lr 'gosub lateral right loop
gosub fl 'gosub left leg forward loop
gosub test
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)
'_________________________________________________________________________
' 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
pulsout 7,40000
sound out,(100,12)
pause 55
sound out,(100,12)
pause 55
sound out,(100,12)
pause 55
sound out,(110,25)
pause 55
sound out,(110,25)
pause 55
sound out,(112,25)
pause 55
sound out,(112,25)
pause 55
sound out,(116,54)
pause 55
sound out,(114,15)
pause 55
sound out,(110,15)
pause 55
'thoose loops are to emit a sound and light,
pulsout so,90 'really useful because you know when the
pulsout led,250 'robot will starts to avoid objects,as
this
'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 b3=1 to 5 '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 b3
return
'________________________________________________________________________
' TURN LEFT LOOP
turnl: 'same as before for backward and left turn
for b3=1 to 5 'depending on which feet (switch) detect
gosub ll 'object
gosub fl
gosub l0
gosub fr
next b3
return
'________________________________________________________________________
' SWITCHES TEST LOOP
'one switch on each feet is parallel to
test: 'a different resistor and both serial connected
random w2
'to input pin 5 the use of Pot instruction to
b5=w2//4
'mesure the value on pin 5
pot sw,6,b2 'mesure
the value on pin 5
if b2 > 12 and b2< 18
then turnr 'will tell wich of the switch
if b2 >= 19 then turnl 'is activated
if pin6=0 and b5>=2 then turnr
'check of eye and random choose
if pin6=0 and b5<2 then turnl
'of direction to turn
return