so any other suggestions?
I tried pasting in the code all over again with a clean file but the same thing happens.
I really can’t find the problem…
Personally if it were me I would break this down to smaller pieces.
That is have you tried what Robot Dude suggested, and write a real simple program that simply moves all 4 of these servos and sees what happens. For example something like:
[code]
angle var long
main:
for angle = 900 to 2100 step 100 ; about -45 to 45
serout SSC_OUT, SC_BAUTE, “#28P”, dec angle, “#29P”,dec angle,"#30P", dec angle, “#31P”, dec angle, “T100”, 13]
pause 100
next
goto main[/code]
Note: I just typed this in on the fly, there may be problems, you may want to play with timings, but assuming I did this right it would imply that the servos work. You may also then wish to change the 900,2100 to the full range of the servos, to make sure that there is not some problem.
If this works then can you add a simple output to the 18 leg servos to put them all at their zero point and see if it still works, if it does then we probaly need to look more closely at the code.
Kurt
FYI - I did take a quick look through the code and do have some concerns.
As far as I can tell you are currently only playing with HeadHAngle. If it were me I would change the 4 variables HeadHAngle, HeadVAngle… to SLONG from SWORD. Here is why…
in the code you have:
Serout SSC_OUT,SSC_BAUTE,"#",dec HeadHPin,"P",dec (HeadHAngle+60)*10000/1059+650]Now assuming word math, you have lets say HeadHAngle at 20 degrees (actually zero may have problems as well)
Your math goes 20+60 = 80 * 10000 = 80000 which will not fit into a 16 bit value, now maybe internally it does it as 32 bit math but maybe not
Kurt
Update: This may not be a problem:
headhangle var sword
headhangle2 var slong
for headhangle = -60 to 60 step 10
headhangle2 = headhangle ; move to long
Serout s_out,I9600,[SDEC HEADHANGLE, "=", dec (HeadHAngle+60)*10000/1059+650, " ", dec ((HeadHAngle2+60)*10000)/1059+650, 13]
next
end
Had the output:
-60=650 650
-50=744 744
-40=838 838
-30=933 933
-20=1027 1027
-10=1122 1122
0=1216 1216
10=1311 1311
20=1405 1405
30=1499 1499
40=1594 1594
50=1688 1688
60=1783 1783
Thanx a lot! I just wanted to ask you if you can help me implement it in the coe. I don’t know where to put it… and what to remove…
I more or less had the first version there, except I did not add the defines for the SSC pin and baud. Here is a version that first walks through with only the head and tail, then after the first pass should try to set the leg servos all to 1500…
[code]SSC_OUT con P11 ;Output pin for (SSC32 RX) on BotBoard (Yellow)
SSC_IN con P10 ;Input pin for (SSC32 TX) on BotBoard (Blue)
SSC_BAUTE con i38400 ;SSC32 Baute rate
angle var long
time var long
Sound P9,[60\4000,80\4500,100\5000]
; get them first to init position.
serout SSC_OUT, SSC_BAUTE, “#28P1500#29P1500#30P1500#31P1500”, 13]
; first time through will only move the head and tail…
main:
time = 250
for angle = 900 to 2100 step 100 ; about -45 to 45
serout SSC_OUT, SSC_BAUTE, “#28P”, dec angle, “#29P”, dec angle, “#30P”, dec angle, “#31P”, dec angle, “T”, dec time, 13]
pause time
time = 100 ; make first one move slower, longer ways to go…
next
pause 5000 ; wait a few seconds.
; now set all of the leg pins to 1500
serout SSC_OUT, SSC_BAUTE, "#00P1500 #01P1500 #02P1500 #04P1500 #05P1500 #06P1500",|
"#08P1500 #09P1500 #10P1500 #16P1500 #17P1500 #18P1500", |
"#20P1500 #21P1500 #22P1500 #24P1500 #25P1500 #26P1500", 13]
goto main
[/code]
This is the whole program, you simply need to create a new file, cut and paste, save and download…
Indeed! this worked fine! So what does this tell? Can we fix my problem now?
Ok, so it looks like software. Another look through the code and I think I see the problem.
Kurt.
Oh, I guess you might like to know what it is 
If you look at this code:
[code] ;Headtracking
GOSUB HeadTracking
GOSUB CheckAngles
LedC = IKSolutionWarning
LedA = IKSolutionError
;Read input
GOSUB Ps2Input
;GOSUB ReadButtons ;I/O used by the PS2 remote
;GOSUB WriteLeds ;I/O used by the PS2 remote
;Head Angle
HeadHAngle = (HeadHAngle min HeadHPin_MIN) max HeadHPin_MAX
HeadVAngle = (HeadVAngle min HeadVPin_MIN) max HeadVPin_MAX
TailHAngle = (TailHAngle min TailHPin_MIN) max TailHPin_MAX
TailVAngle = (TailVAngle min TailVPin_MIN) max TailVPin_MAX
;Get endtime and calculate wait time
GOSUB GetCurrentTime], lTimerEnd
CycleTime = (lTimerEnd-lTimerStart)/WTIMERTICSPERMS
IF(HexOn)THEN
;Wait for previous commands to be completed while walking
IF(ABS(TravelLengthX)>TravelDeadZone | ABS(TravelLengthZ)>TravelDeadZone | ABS(TravelRotationY*2)>TravelDeadZone) THEN
pause (PrevSSCTime - CycleTime -50) MIN 1 ; Min 1 ensures that there alway is a value in the pause command
IF(BalanceMode=0)THEN
SSCTime = NomGaitSpeed + (InputTimeDelay*2)
ELSE
SSCTime = NomGaitSpeed + (InputTimeDelay*2) + 100
ENDIF
ELSE
SSCTime = 200 ;NomGaitSpeed
ENDIF
GOSUB ServoDriver
ELSE
;Turn the bot off
GOSUB FreeServos
ENDIF
HeadTracking: ;(Head Tracking)
;Return to the middle position
HeadHAngle=0
HeadVAngle=0
TailHAngle=0
TailVAngle=0
IF (ABS(TravelLengthX)>TravelDeadZone | ABS(TravelLengthZ)>TravelDeadZone | ABS(TravelRotationY*2)>TravelDeadZone) THEN
;Calculate walking direction X and Z
TravelLengthXZ = SQR((TravelLengthX * TravelLengthX) + TravelLengthZ * TravelLengthZ)
HeadHAngle = TOINT(FACOS(TOFLOAT(TravelLengthZ) / TOFLOAT(TravelLengthXZ)) * 180.0 / 3.141592)-180
;Add sign depending on the direction of X
HeadHAngle = HeadHAngle * (TravelLengthX/ABS(TravelLengthX))
ENDIF
;Calculate body angle depending on rotation
IF ABS(TravelRotationY2) >TravelDeadZone & ABS(TravelRotationY3) > ABS(HeadHAngle) THEN
HeadHAngle = -TravelRotationY3 ; Rotation max = 166 to get max range of 90 deg.
ENDIF
RETURN
goto main
[/code]
You placed the Head tracking subroutine in the wrong place. IE you put it in such that once you call the ServoDriver, your continues to fall through into the HeadTracking function which does a return, which corrupts the stack. What you need to do is to move the “Goto Main” line up above this subroutine, such that it goes back to top of the loop and not fall into the subroutine.
I hope that makes sense.
Kurt
Yes it does. Now i can use the bot and it will work normally. Only the head ain’t moving… 
Now it is time for you to start debugging it…
If it were me I would start off by adding some debug outputs to S_OUT to see what values I am getting.
For example:
[code];----------------------------------------------------------------------------------------------------------------
DTLX var sword ; debug
DTLZ var sword
DTRY var sword
HeadTracking: ;(Head Tracking)
;Return to the middle position
HeadHAngle=0
HeadVAngle=0
TailHAngle=0
TailVAngle=0
IF (ABS(TravelLengthX)>TravelDeadZone | ABS(TravelLengthZ)>TravelDeadZone | ABS(TravelRotationY*2)>TravelDeadZone) THEN
;Calculate walking direction X and Z
TravelLengthXZ = SQR((TravelLengthX * TravelLengthX) + TravelLengthZ * TravelLengthZ)
HeadHAngle = TOINT(FACOS(TOFLOAT(TravelLengthZ) / TOFLOAT(TravelLengthXZ)) * 180.0 / 3.141592)-180
;Add sign depending on the direction of X
HeadHAngle = HeadHAngle * (TravelLengthX/ABS(TravelLengthX))
ENDIF
;Calculate body angle depending on rotation
IF ABS(TravelRotationY2) >TravelDeadZone & ABS(TravelRotationY3) > ABS(HeadHAngle) THEN
HeadHAngle = -TravelRotationY3 ; Rotation max = 166 to get max range of 90 deg.
ENDIF
; debug print…
if (DTLX <> TravelLengthX) or (DTLZ <> TravelLengthZ) or (DTRY <> TravelRotationY) then
DTLX = TravelLengthX
DTLZ = TravelLengthZ
DTRY = TravelRotationY
serout s_out, i9600, "Head Track: ", sdec DTLX, " ", sdec DTLZ, " ", sdec DTRY, “->”, sdec HeadHAngle, 13]
endif
RETURN
[/code]
I did this more compilicated than maybe necessary in that it only prints if one of the main inputs changes. This keeps from having lots of debug outputs. If no outputs come out then you are not calling this function. If the inputs change but the last value for the head h angle does not change then something is wrong in the calculation. If it changes, but the head does not move, then maybe need to check to see what the values are in the servo driver to make sure they have not been updated…
Kurt
euhm… i really don’t know how to use this. I tried something but didn’t give me any info. i put the code in my bas file on the spot where you showed. At least… This goes under the “goto main” script right?
Ok and then?
Hi again,
Sorry but I don’t have much time to spend here. But I think you need to start learning how to do some of the debugging.
The Serout command outputs data to a serial port. The reserved name S_OUT goes out over the serial port that is connected to the 9 pin serial connector on your Bot Board 2. So the idea is to sprinkly debug printouts in key locations in your code to help figure out what is going on.
These can be as simple as:
serout s_out, i9600, “I got here”, 13]
which will output the simple string to the terminal followed by a CR, like you use in the other serouts.
So once you compile and download the program to your board, you leave the serial connector hooked up. You then go to the terminal 1 tab at the bottom of your basic Atom Pro IDE and select the correct comm port and the baud rate of 9600 and then click on the connect button. At this point your robot will probably reboot. Hopefully at this point you will start getting debug information to help you figure what is going on with your program.
Good Luck
Kurt
Thanx! I did what you said but i get no information at all. It does connect, and works fine . I can walk and do everything. but no feedback.
it seems like the head servo’s ar not powered. I can move them by hand.
Ok, you got now outputs in the terminal window. Did you add any other debug outputs to see what is going on? Do you still have the gosub to Head tracking in your code, like before?
[code] ;Left Rear leg
GOSUB BodyIK [LRPosX-BodyPosX+LRGaitPosX, LRPosZ+BodyPosZ+LRGaitPosZ,LRPosY+BodyPosY+BodyYShift+LRGaitPosY, LROffsetX, LROffsetZ, LRGaitRotY]
GOSUB LegIK [LRPosX+BodyPosX-BodyIKPosX+LRGaitPosX, LRPosY+BodyPosY+BodyYShift-BodyIKPosY+LRGaitPosY, LRPosZ+BodyPosZ-BodyIKPosZ+LRGaitPosZ]
LRCoxaAngle = IKCoxaAngle - CoxaAngle ;Angle for the basic setup for the front leg
LRFemurAngle = IKFemurAngle
LRTibiaAngle = IKTibiaAngle
;Headtracking
GOSUB HeadTracking
GOSUB CheckAngles
[/code]
Are you sure you are outputing any commands to the SSC-32 about your head and tail? Do you still have code in your ServoDriver? Like:
[code] ;[SERVO DRIVER “FUTABA SERVOS”] Updates the positions of the servos (CCW)
ServoDriver:
;Head & Tail
Serout SSC_OUT,SSC_BAUTE,"#",dec HeadHPin,“P”,dec (HeadHAngle+60)*10000/1059+650]
Serout SSC_OUT,SSC_BAUTE,"#",dec HeadVPin,“P”,dec (HeadVAngle+60)*10000/1059+650]
Serout SSC_OUT,SSC_BAUTE,"#",dec TailHPin,“P”,dec (TailHAngle+60)*10000/1059+650]
Serout SSC_OUT,SSC_BAUTE,"#",dec TailVPin,“P”,dec (TailVAngle+60)*10000/1059+650]
;Front Right leg
[/code]
Did you add some debug outputs in here to see what the values for the head and tail are?
This is the fun part, try experimenting to see what is going on/
Kurt
I know this is the fun part. i agree! Unless there is nothing to see…
I get no output (echo) at all in the terminal window.
What are the settings i have to use? I now have 9600 tried 38400 also. no flow or parity.
I actually never got the terminal to work. only the lynxterm.
OK, lets back up.
You have the same wire connected to the BB2 that you used to program the Atom Pro with. You click to get to the terminal window at the bottom of the Basic Atom Pro IDE. You have verified that the COMM port that is shown in the list is the same one that you used to program the BAP. You set the baud rate to 9600 as that is what I have choosen to use in my serouts that look like: serout s_out, i9600, …
You have clicked on Connect, and the program on the Atom PRO resets. You should hear the startup sounds.
And then you should start to see things in the IDE, especially if you added more like I suggested…
Did you verify that you had the proper serouts to controll the head in the driver function?
Kurt
IT WORKS!!
I need to add the rest of the code now for the tail etc.
Only thing. It doesn’t move the head when i use the body motion (L2 + joystick) and i need to adjust the widthness. it touches the legs now…
Any ideas?
this is the output:
Head Track: 17 0 0->-90
Head Track: 38 0 0->-90
Head Track: 53 0 0->-90
Head Track: 58 0 0->-90
Head Track: 32 0 0->-90
Head Track: 0 0 0->0
Head Track: -63 0 0->90
Head Track: 0 0 0->0
Head Track: 0 -14 0->9
Head Track: 0 -27 0->9
Head Track: 0 -54 0->9
Head Track: 0 -53 0->9
Head Track: 0 -54 0->9
Head Track: 0 -7 0->9
Head Track: 0 0 0->0
Head Track: 0 63 0->-189
Head Track: 0 0 0->0
*EDIT:
I adjusted the turning degree to 30 or -30 intead of 60. That did it!
Now how to get the body motion head thingy to work…?!? 
I found out that i forgot to change the X-code for horizontal movement, to Y for vertical movement.
This might be the cause of the head not moving with the body movements.
I just don’t know what to change… below is the code i need to adjust.
[code];[TAILVANGLE]
IF (ABS(TravelLengthX)>TravelDeadZone | ABS(TravelLengthZ)>TravelDeadZone | ABS(TravelRotationY*2)>TravelDeadZone) THEN
;Calculate walking direction X and Z
TravelLengthXZ = SQR((TravelLengthX * TravelLengthX) + TravelLengthZ * TravelLengthZ)
TailVAngle = TOINT(FACOS(TOFLOAT(TravelLengthZ) / TOFLOAT(TravelLengthXZ)) * 180.0 / 3.141592)-180
;Add sign depending on the direction of X
TailVAngle = TailVAngle * (TravelLengthX/ABS(TravelLengthX))
ENDIF
;Calculate body angle depending on rotation
IF ABS(TravelRotationY2) >TravelDeadZone & ABS(TravelRotationY3) > ABS(TailVAngle) THEN
TailVAngle = -TravelRotationY3 ; Rotation max = 166 to get max range of 90 deg.
ENDIF
; debug print…
if (DTLX <> TravelLengthX) or (DTLZ <> TravelLengthZ) or (DTRY <> TravelRotationY) then
DTLX = TravelLengthX
DTLZ = TravelLengthZ
DTRY = TravelRotationY
serout s_out, i9600, "Head Track: ", sdec DTLX, " ", sdec DTLZ, " ", sdec DTRY, “->”, sdec HeadHAngle, 13]
endif [/code]
Ive been working on this a lot the last days but can’t seem to find a solution.
I have this code for horizontal movement:
[code]IF (ABS(TravelLengthX)>TravelDeadZone | ABS(TravelLengthZ)>TravelDeadZone | ABS(TravelRotationY*2)>TravelDeadZone) THEN
;Calculate walking direction X and Z
TravelLengthXZ = SQR((TravelLengthX * TravelLengthX) + TravelLengthZ * TravelLengthZ)
HeadHAngle = TOINT(FACOS(TOFLOAT(TravelLengthZ) / TOFLOAT(TravelLengthXZ)) * 180.0 / 3.141592)-180
;Add sign depending on the direction of X
HeadHAngle = HeadHAngle * (TravelLengthX/ABS(TravelLengthX))
ENDIF [/code]
Now i need to change the code for vertical movement:
[code];[HEADVANGLE]
IF (ABS(TravelLengthX)>TravelDeadZone | ABS(TravelLengthZ)>TravelDeadZone | ABS(TravelRotationY*2)>TravelDeadZone) THEN
;Calculate walking direction X and Z
TravelheightY = SQR((TravelHeightY * TravelHeightY) + TravelrotationY * TravelrotationY)
HeadVAngle = TOINT(FACOS(TOFLOAT(TravelHeightY) / TOFLOAT(TravelHeightY)) * 60.0 / 3.141592)-60
;Add sign depending on the direction of X
HeadVAngle = HeadVAngle * (TravelHeightY/ABS(TravelHeightY))
ENDIF
;Calculate body angle depending on rotation
IF ABS(TravelRotationY2) >TravelDeadZone & ABS(TravelRotationY3) > ABS(HeadVAngle) THEN
HeadVAngle = -TravelRotationY3 ; Rotation max = 166 to get max range of 90 deg.
ENDIF [/code]
This obviously doesn’t work. Can anyone tell me what the right travellength or height or rotation is, that i have to replace it with. I can’t find anything about it on the web…
Hi again.
i too have been working on a head (pan and tilt)
iv been getting the head to *pan *(Y) when body moves left or right and also to *tilt *up and down when moving forward or back using the “left stick” (walk direction)
works fine. Iv added code at bottom of post for you to see.
it too rotates “pan” when using the right stick left or right but not tilt!
this is were my problem is. i would like to be using the right stick up/down for tilt!
can get it to work!!!
is this also true with you.?
i believe we need ***BodyYShift ***?
PAN:
[code];[Head Tracking]
HeadTracking:
;Return to the middle position
HeadPanAngle=0
IF (ABS(TravelLengthX)>TravelDeadZone | ABS(TravelLengthZ)>TravelDeadZone | ABS(TravelRotationY2)>TravelDeadZone) THEN
;Calculate walking direction X and Z
TravelLengthXZ = SQR((TravelLengthX * TravelLengthX) + TravelLengthZ * TravelLengthZ)
HeadPanAngle = TOINT(FACOS(TOFLOAT(TravelLengthZ) / TOFLOAT(TravelLengthXZ)) * 180.0 / 3.141592)-180
;Add sign depending on the direction of X
HeadPanAngle = HeadPanAngle * (TravelLengthX/ABS(TravelLengthX))
ENDIF
;Calculate body angle depending on rotation
IF ABS(TravelRotationY2)>TravelDeadZone & ABS(TravelRotationY3) > ABS(HeadPanAngle) THEN
HeadPanAngle = -TravelRotationY3 ; Rotation max = 16*6 to get max range of 90 deg.
ENDIF[/code]
TILT but only works for left stick not right!
[code] HeadTiltAngle=0
IF (ABS(TravelLengthZ)>TravelDeadZone | ABS(TravelLengthX)>TravelDeadZone | ABS(TravelRotationY2)>TravelDeadZone) THEN
;Calculate walking direction X and Z
TravelLengthXZ = SQR((TravelLengthZ * TravelLengthZ) + TravelLengthX * TravelLengthX)
HeadTiltAngle = TOINT(FACOS(TOFLOAT(TravelLengthX) / TOFLOAT(TravelLengthXZ)) * 180.0 / 3.141592)-180
;Add sign depending on the direction of X
HeadTiltAngle = HeadTiltAngle * (TravelLengthZ/ABS(TravelLengthZ))
ENDIF
;Calculate body angle depending on rotation
IF ABS(TravelRotationY2)>TravelDeadZone & ABS(TravelRotationY3) > ABS(HeadTiltAngle) THEN
HeadTiltAngle = -TravelRotationY3 ; Rotation max = 16*6 to get max range of 90 deg.
ENDIF
return[/code]
obviously if you wish to use this you will need to rename commands as iv used different.
it might help you but i thought id add this post here as i feel we have the same problem?