I’ve been fighting to find the right sequence to make this thing walk. I am not much of a walking code guy, so this is driving me crazy, my project involves more AI; I wrote some algorithms for modeling free-will in robots and can’t wait to add servos and try it out. But first, I need to get this thing walking and man, it is freaking hard. This is what I have so far.
[code]righthip con p7
rightknee con p6
rightankle2 con p5
rightankle con p4
lefthip con p3
leftknee con p2
leftankle2 con p1
leftankle con p0
NUMSERVOS con 6
aServoOffsets var sword(NUMSERVOS)
ServoTable bytetable righthip,rightknee, rightankle,rightankle2,leftankle,leftankle2,leftknee,lefthip
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 Rightankle,Rightknee,Righthip,Leftankle,Leftknee,Lefthip,speed
gosub movement 0.0, 0.0, 0.0, -30.0, 0.0, 0.0, 0.0, -30.0, 0.0]
pause 2000
;low 1
;low 2
pause 1000
command var byte
xx var byte
bat var word
;main
;---------------------------------------;
;--------Command Quick Reference--------;
;---------------------------------------;
;- Command 1 = Walk Forward -;
;- Command 2 = Walk Backward -;
;- Command 3 = Long Stride Forward -;
;- Command 4 = Long Stride Backward -;
;- Command 5 = Kick -;
;- Command 6 = Headbutt -;
;- Command 7 = Get up from Front -;
;- Command 8 = Get up from Back -;
;- Command 9 = Rest Position -;
;- Command 0 = Home Position -;
;- Command 11= Turn Left -;
;---------------------------------------;
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 9,[50\5000,25\4400,50\5000]
endif
for xx = 1 to 20
gosub move
next
end
move:
; Walk Forward
;rightanklepos,rightanklepos2,rightkneepos,righthippos,leftanklepos,leftanklepos2,leftkneepos,lefthippos,speed
gosub movement -7.0, 20.0, 20.0, 20.0, 7.0,-20.0,-20.0, 500.0]
gosub movement 7.0, 20.0, 20.0, 20.0, -7.0,-20.0,-20.0, 500.0]
gosub movement 7.0,-20.0,-20.0,-20.0, -7.0, 20.0, 20.0, 500.0]
gosub movement -7.0,-20.0,-20.0,-20.0, 7.0, 20.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
lefthippos var float
leftkneepos var float
leftanklepos var float
leftanklepos2 var float
righthippos var float
rightkneepos var float
rightanklepos var float
rightanklepos2 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
speed var float
longestmove var float
; variables passed to gethservo
idle var byte
finished var byte
junk var word
movement [rightanklepos,rightanklepos2,rightkneepos,righthippos,leftanklepos,leftanklepos2,leftkneepos,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],longestmove
speed = ((longestmovestepsperdegree)/(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
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;
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 (rightanklepos2*stepsperdegree) + aServoOffsets(0)\TOINT ra2speed]
; 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)
; ??? - 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
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
getlongest[one,two,three,four,five,six,seven,eight]
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(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
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]
Yes, I took it from the brat and did some changes.
Any help will help.
Thx a lot.