Rosalinda - 3 servo hexapod

 

• Lenght: 10.5 cm
• Width: 7.5 cm
• Height: 8 cm
• Weight: 144 g

Components

• 1x Picaxe 18X
• 4x AAA 800mAh NiMh cell
• 3x Hitec HS-55 servo
• 1x Sharp GP2D120 IR Sensor

 

 

Schmatics

Source code

'pin
symbol pinServoL=2
symbol pinServoR=3
symbol pinServoC=1
symbol pinAdcIr=0
symbol pinOutLed=6

'servopos
symbol servoLF=175
symbol servoLB=100
symbol servoLN=145

symbol servoRF=125
symbol servoRB=175
symbol servoRN=150

symbol servoCR=170
symbol servoCL=140
symbol servoCN=157

symbol servoT=150
symbol warn=60

'brain
symbol irReadM=b13
symbol irRead=b12
symbol prefSide=bit0
symbol leftSide=0
symbol rightSide=1

'init brain
prefSide=leftSide

'init robot
servo pinServoL, servoLN
servo pinServoR, servoRN
servo pinServoC, servoCN
high pinOutLed
pause 2000

'shakedown
for b2=0 to 20
servo pinServoC, servoCL:pause 50
low pinOutLed
servo pinServoC, servoCR:pause 50
high pinOutLed
next

'init robot
servo pinServoL, servoLN
servo pinServoR, servoRN
servo pinServoC, servoCN
high pinOutLed
pause 200

 

walk_forward:'walk forwart till obsta
servopos pinServoC,servoCR:pause servoT
servopos pinServoR,servoRF:pause servoT
do
servopos pinServoC,servoCL:pause servoT 'right legs on the ground
servopos pinServoR,servoRB: servopos pinServoL,servoLF: pause servoT 'left forward - right backward
servopos pinServoC,servoCR:pause servoT 'left legs on the ground
servopos pinServoR,servoRF: servopos pinServoL,servoLB: pause servoT 'right forward - left backward
'chk ir
readADC pinAdcIr,irReadM
if irReadM>warn then
low pinOutLed
if prefSide=leftSide then
goto try_left
else
goto try_right
end if
else
high pinOutLed
end if
loop
end

 

 

try_left:'step left then compare readings
servopos pinServoC,servoCL:pause servoT
servopos pinServoL,servoLF:pause servoT
for b2=0 to 2
servopos pinServoC,servoCR:pause servoT 'left legs on the ground
servopos pinServoR,servoRF: servopos pinServoL,servoLF: pause servoT 'left forward - right forward
servopos pinServoC,servoCL:pause servoT 'right legs on the ground
servopos pinServoR,servoRB: servopos pinServoL,servoLB: pause servoT 'left backward - right backward
next
readADC pinAdcIr,irRead
if irRead<warn then
prefSide=leftSide
goto walk_forward'clear -> ok go forward
else
if irRead<irReadM then
goto go_left 'left is better -> go left at will
else
goto go_right 'right is better -> go right at will
end if
end if
end

 

 

go_left:'rotate left till clear
servopos pinServoC,servoCL:pause servoT
servopos pinServoL,servoLF:pause servoT
do
servopos pinServoC,servoCR:pause servoT 'left legs on the ground
servopos pinServoR,servoRF: servopos pinServoL,servoLF: pause servoT 'left forward - right forward
servopos pinServoC,servoCL:pause servoT 'right legs on the ground
servopos pinServoR,servoRB: servopos pinServoL,servoLB: pause servoT 'left backward - right backward
readADC pinAdcIr,irRead
if irRead<warn then
prefSide=leftSide
goto walk_forward'clear-> ok goforward
end if
loop
end

 

 

try_right:'step right then compare readings
servopos pinServoC,servoCR:pause servoT
servopos pinServoL,servoRF:pause servoT
for b2=0 to 2
servopos pinServoC,servoCL:pause servoT 'right legs on the ground
servopos pinServoR,servoRF: servopos pinServoL,servoLF: pause servoT 'left forward - right forward
servopos pinServoC,servoCR:pause servoT 'left legs on the ground
servopos pinServoR,servoRB: servopos pinServoL,servoLB: pause servoT 'left backward - right backward
next
readADC pinAdcIr,irRead
if irRead<warn then
prefSide=rightSide
goto walk_forward'clear-> ok goforward
else
if irRead<irReadM then
goto go_right 'right is better -> go right at will
else
goto go_left 'left is better -> go left at will
end if
end if
end

 

 

go_right:'rotate right till clear
servopos pinServoC,servoCR:pause servoT
servopos pinServoL,servoRF:pause servoT
do
servopos pinServoC,servoCL:pause servoT 'right legs on the ground
servopos pinServoR,servoRF: servopos pinServoL,servoLF: pause servoT 'left forward - right forward
servopos pinServoC,servoCR:pause servoT 'left legs on the ground
servopos pinServoR,servoRB: servopos pinServoL,servoLB: pause servoT 'left backward - right backward
readADC pinAdcIr,irRead
if irRead<warn then
prefSide=rightSide
goto walk_forward'clear-> ok goforward
end if
loop
end