Hi guys,
I’ve been trying to program this biped robot to walk with a botboardII and a basic atom pro 28 processor. Right now I have 5 servos in each leg. I am using HS-5485HB for all of the servos but the two servos at the upper hip which are slightly more powerful HS-645MG.
The problem is that I can move all of the HS-5485HB together no problem, but when I try to move the HS-645MG, only the HS-645MG move while the board makes this really weird constant noise, and the others servos lose power. I uploaded a video on youtube so you guys can see and I also post my code below. Sounds kind of like a blender, here it is:
youtube.com/watch?v=BhtMLwcALWI
[code]rightknee2 con p7
rightknee con p6
rightankle2 con p5
rightankle con p4
leftknee2 con p3
leftknee con p2
leftankle2 con p1
leftankle con p0
lefthip con p8
righthip con p9
NUMSERVOS con 6
aServoOffsets var sword(NUMSERVOS)
ServoTable bytetable rightknee2,rightknee, rightankle,rightankle2,leftankle,leftankle2,leftknee,leftknee2,lefthip,righthip
TRUE con 1
FALSE con 0
;calibrate steps per degree.
stepsperdegree fcon 166.6
ENABLEHSERVO
;==============================================================================
; Complete initialization
;==============================================================================
aServoOffsets = rep 0\NUMSERVOS ; Use the rep so if size changes we should properly init
; try to retrieve the offsets from EEPROM:
; see how well my pointers work
gosub ReadServoOffsets
;Init positions
;Note, movement subroutine arguments are rightanklepos,rightanklepos2,rightkneepos,rightkneepos2,righthippos,leftanklepos,leftanklepos2,leftkneepos,leftkneepos2,lefthippos,speed
gosub movement 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
pause 5000
;low 1
;low 2
pause 1000
command var byte
xx var byte
bat var word
sound 9,[50\4400]
pause 50
sound 9,[50\3960]
pause 50
sound 9,[50\3400]
main
adin 16, bat
if (bat < 270) then;the battery is getting low
sound 5,[50\5000,25\4400,50\5000]
endif
for xx = 1 to 20
gosub move
next
end
move:
; Walk Forward
;rightanklepos,rightanklepos2,rightkneepos,rightkneepos2,righthippos,leftanklepos,leftanklepos2,leftkneepos,leftkneepos2,lefthippos,speed
;old Rightankle,Rightknee,Righthip,Leftankle,Leftknee,Lefthip,speed
gosub movement 7.0,0,0,30.0,-20.0, -7.0, 0.0, 0.0,0.0,20.0, 500.0]
gosub movement -7.0,0,0,-30.0,20.0, 7.0, 0.0, 0.0,0.0,-20.0, 500.0]
return
;Should never need to edit anything below this line. Add user subroutines above this and below main.
last_lefthippos var float
last_leftkneepos var float
last_leftanklepos var float
last_leftanklepos2 var float
last_righthippos var float
last_rightkneepos var float
last_rightanklepos var float
last_rightanklepos2 var float
last_rightkneepos2 var float
last_leftkneepos2 var float
leftkneepos2 var float
leftkneepos var float
leftanklepos var float
leftanklepos2 var float
rightkneepos2 var float
rightkneepos var float
rightanklepos var float
rightanklepos2 var float
lefthippos var float
righthippos var float
lhspeed var float
lkspeed var float
laspeed var float
la2speed var float
rhspeed var float
rkspeed var float
raspeed var float
ra2speed var float
lk2speed var float
rk2speed var float
speed var float
longestmove var float
; variables passed to gethservo
idle var byte
finished var byte
junk var word
movement [rightanklepos,rightanklepos2,rightkneepos,rightkneepos2,righthippos,leftanklepos,leftanklepos2,leftkneepos,leftkneepos2,lefthippos,speed]
if(speed<>0.0)then
gosub getlongest[lefthippos-last_lefthippos, |
leftkneepos-last_leftkneepos, |
leftanklepos-last_leftanklepos, |
righthippos-last_righthippos, |
rightkneepos-last_rightkneepos, |
rightanklepos2-last_rightanklepos2, |
leftanklepos2-last_leftanklepos2, |
rightanklepos-last_rightanklepos, |
rightkneepos2-last_rightkneepos2, |
leftkneepos2-last_leftkneepos2],longestmove
speed = ((longestmove*stepsperdegree)/(speed/20.0))
gosub getspeed[lefthippos,last_lefthippos,longestmove,speed],lhspeed
gosub getspeed[leftkneepos,last_leftkneepos,longestmove,speed],lkspeed
gosub getspeed[leftanklepos,last_leftanklepos,longestmove,speed],laspeed
gosub getspeed[leftanklepos2,last_leftanklepos2,longestmove,speed],la2speed
gosub getspeed[righthippos,last_righthippos,longestmove,speed],rhspeed
gosub getspeed[rightkneepos,last_rightkneepos,longestmove,speed],rkspeed
gosub getspeed[rightanklepos,last_rightanklepos,longestmove,speed],raspeed
gosub getspeed[rightanklepos2,last_rightanklepos2,longestmove,speed],ra2speed
gosub getspeed[rightkneepos2,last_rightkneepos2,longestmove,speed],rk2speed
gosub getspeed[leftkneepos2,last_leftkneepos2,longestmove,speed],lk2speed
else
lhspeed=0.0;
lkspeed=0.0;
laspeed=0.0;
rhspeed=0.0;
rkspeed=0.0;
raspeed=0.0;
ra2speed=0.0;
la2speed=0.0;
lk2speed=0.0;
rk2speed=0.0;
endif
hservo [lefthip\TOINT (-lefthipposstepsperdegree) + aServoOffsets(0)\TOINT lhspeed, |
righthip\TOINT (righthipposstepsperdegree) + aServoOffsets(0)\TOINT rhspeed, |
leftknee\TOINT (-leftkneeposstepsperdegree) + aServoOffsets(0)\TOINT lkspeed, |
rightknee\TOINT (rightkneeposstepsperdegree) + aServoOffsets(0)\TOINT rkspeed, |
leftankle\TOINT (-leftankleposstepsperdegree) + aServoOffsets(0)\TOINT laspeed, |
rightankle\TOINT (rightankleposstepsperdegree) + aServoOffsets(0)\TOINT raspeed, |
leftankle2\TOINT (leftanklepos2stepsperdegree) + aServoOffsets(0)\TOINT la2speed, |
rightankle2\TOINT (rightanklepos2stepsperdegree) + aServoOffsets(0)\TOINT ra2speed,|
rightknee2\TOINT (rightknee2stepsperdegree) + aServoOffsets(0)\TOINT rk2speed,|
leftknee2\TOINT (leftkneepos2stepsperdegree) + aServoOffsets(0)\TOINT lk2speed]
; Simple loop to check the status of all of servos. This loop will
; terminate when all of the servos say that they are idle. I.E.
; they reached their new destination.
do
finished = hservoidle(lefthip) and hservoidle(righthip) and hservoidle(leftknee) and hservoidle(rightknee) and hservoidle (leftankle) and hservoidle(rightankle) and hservoidle (leftankle2) and hservoidle(rightankle2) and hservoidle (leftknee2) and hservoidle (rightknee2)
; ??? - we say not to edit, yet???
;add sensor handling code here
;adin 0,ir
;if (ir > 210) then
; detect = true
;endif
while (NOT finished)
last_lefthippos = lefthippos
last_leftkneepos = leftkneepos
last_leftanklepos = leftanklepos
last_leftanklepos2 = leftanklepos2
last_righthippos = righthippos
last_rightkneepos = rightkneepos
last_rightanklepos = rightanklepos
last_rightanklepos2 = rightanklepos2
last_rightkneepos2 = rightkneepos2
last_leftkneepos2 = leftkneepos2
return
one var float
two var float
three var float
four var float
five var float
six var float
seven var float
eight var float
nine var float
ten var float
getlongest[one,two,three,four,five,six,seven,eight,nine,ten]
if(one<0.0)then ; these if make every value
one=-1.0one
endif
if(two<0.0)then
two=-1.0two
endif
if(three<0.0)then
three=-1.0three
endif
if(four<0.0)then
four=-1.0four
endif
if(five<0.0)then
five=-1.0five
endif
if(six<0.0)then
six=-1.0six
endif
if(seven<0.0)then
seven=-1.0seven
endif
if(eight<0.0)then
eight=-1.0eight
endif
if(nine<0.0)then
eight=-1.0eight
endif
if(ten<0.0)then
eight=-1.0eight
endif
if(one<two)then
one=two
endif
if(one<three)then
one=three
endif
if(one<four)then
one=four
endif
if(one<five)then
one=five
endif
if(one<six)then
one=six
endif
if(one<seven)then
one=seven
endif
if(one<eight)then
one=eight
endif
if(one<nine)then
one=eight
endif
if(one<ten)then
one=eight
endif
return one
newpos var float
oldpos var float
longest var float
maxval var float
getspeed[newpos,oldpos,longest,maxval]
if(newpos>oldpos)then
return ((newpos-oldpos)/longest)*maxval
endif
return ((oldpos-newpos)/longest)*maxval
;==============================================================================
; Subroutine: ReadServoOffsets
; Will read in the zero points that wer last saved for the different servos
; that are part of this robot.
;
;==============================================================================
pT var pointer ; try using a pointer variable
cSOffsets var byte ; number of
bCSIn var byte
bCSCalc var byte ; calculated checksum
b var byte ;
i var byte
ReadServoOffsets:
readdm 0, [cSOffsets, bCSIn]
;serout s_out, i9600, “RSO: cnt:”, dec cSOffsets, " CS in:", hex bcsIn];
if (cSOffsets > 0) and (cSOffsets <= NUMSERVOS) then ; offset data is bad so go to error location
; OK now lets read in the array of data
readdm 2, [str aServoOffsets\csOffsets*2]
;... calculate checksum...
bCSCalc = 0
for i = 0 to NUMSERVOS-1
bCSCalc = AServoOffsets(i).lowbyte + AServoOffsets(i).highbyte
; serout s_out, i9600, " “, sdec aServoOffsets(i),”:", hex aServoOffsets(i)]
next
; serout s_out, i9600, " CS Calc:", hex bCSCalc]
if bCSCalc <> bCSIn then
aServoOffsets = rep 0\NUMSERVOS
endif
endif
; serout s_out, i9600, [13]
return
[/code]
Thx for your help!