A head for my Hexapod

kurte · July 31, 2009, 10:52pm

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

mano1979 · August 1, 2009, 12:51am

Yes it does. Now i can use the bot and it will work normally. Only the head ain’t moving…

kurte · August 1, 2009, 1:45am

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

mano1979 · August 1, 2009, 2:33am

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?

kurte · August 1, 2009, 2:44am

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

mano1979 · August 1, 2009, 12:05pm

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.

kurte · August 1, 2009, 1:37pm

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

mano1979 · August 1, 2009, 3:47pm

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.

kurte · August 1, 2009, 4:03pm

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

mano1979 · August 1, 2009, 5:23pm

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…?!?

mano1979 · August 2, 2009, 9:36pm

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]

mano1979 · August 8, 2009, 2:32am

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…

innerbreed · September 19, 2009, 7:48pm

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?

mano1979 · September 23, 2009, 1:39am

i haven’t been busy with this code anymore since i have my sensor setup. But do you happen to know how i can shut the “walk back head movement”?

since my sensor is attached to the head it is anoying when it moves while using the sensor to walk back…