Xan's Phoenix Code

[xan]

Thanks Jim and Beth for updating my project page and making this topic sticky! 8) Good idea to put my explanation on there as well!

Thanks for the comments Zenta, This was the way that I figured it out for myself on paper so I thought this could help others to understand the gaits to!

Hi Galegu

@galegu

A picture of my Tripod Gait :

http://leguerroue.gael.free.fr/hexapode/gait.JPG

the problem is that during to steps the hexapod doesn’t move…
But it easy to start (first step send a Tripod in position 5).

To stop the walk : send up legs to position 2 and the over legs that are on the floor and not in postion 2 up in 5 then down in 2.

Thanks for your input galegu! How does your tripod look in motion? Is it smooth or does it shock a lot because of step 3 and 4 beeing on the same place? Do you have a vid?

I have to say that you’ve got a good point on the stopping positions. When I use 2 upper positions there is no middle-upper position. But when I stop motion, I use the middle-upper position from the other gaits.

This is how it works in pseudocode:

[code]IF RemoteInput=0 AND LegsNotInHomePosition AND LegsNotUp THEN
Goto MiddleUpperPosition
ENDIF

IF LegIsInMiddleUpperPosition THEN
Goto homePosition
[/code]
You can download V1.2 to see how it looks in BASIC. Let me know if there are questions.

Xan

[galegu]

Hi Zan !

I will put a video son with my tripod gait.
The problem is the two steps where the body doesn’t move. But by this way i sure that there is no problem while put down the 3 legs.

I have a question : how are you starting your motion ? in some of your gaits you haven’t a center position on the floor. How do you send the legs to there first positions ?

I have seen your BASIC program. It’s very interesting. But i intend to use PIC familly later… For the moment i control my hexapode with a computer.

To finish i use the same method to stop walking.

[xan]

Hi Galegu,

@galegu

I have a question : how are you starting your motion ? in some of your gaits you haven’t a center position on the floor. How do you send the legs to there first positions ?

Starting and stopping is always hard! I came up with a solution that works with both starting and changing step length or direction! If we take this movement as example:

When starting I drive the first leg to position1 which is only up! The other legs will move ¼ of the travel length. The next step the first leg will go to the travellength/2 and the second leg will got to position 1. All the other will move ¼ of the travel length again.

When start walking the legs won’t be in the correct position but after 6 steps they are. The only downside on this is that you can’t accelerate to fast because the legs can get out of the max range.

I hope this makes any sense…

Xan

[galegu]

Xan,
i undestand your technique. Effectively the first steps have to be small because the last leg move 4 times from the center bottom position to the front bottom position. But by dividing the normal displacement by two there is no problem.
And when the number of step on the floor isn’t even how are you doing ?

[xan]

Hi Galegu,

@galegu

And when the number of step on the floor isn’t even how are you doing ?

If the number is odd I will get a small error which will increase every step if the body is on the ground. But it also will reset when the leg returns to position 2 (front down position). I tried it and you don’t see the error at all! I don’t know what wil happen if there are a huge amount of steps in the gait but a 18 steps wave gait works just fine 8)

Xan

[galegu]

Hi Xan,

a video of my tripod gait :

fr.youtube.com/watch?v=5OiR9qcerns

now a will test your tripod gait to avoid stop the body durring the motion.

[zenta]

Hi,

As some of you know I’ve added a sort of balance mode to Xan’s code. The balance mode is similar to Matt Denton’s balance gesture.

The principle of how it works are quite simple. Just calculate how much each tars of the leg are rotated or translated relative to the center of body. And then add the values all together and divide by 6 and compensate the body (translate and rotate) with these values.

And here is the code (subs) for calculating balance of one leg and the body:

[code];--------------------------------------------------------------------
;[BalCalcOneLeg]
BalCalcOneLeg [PosX, PosZ, PosY, BodyOffsetX, BodyOffsetZ]
;Calculating totals from center of the body to the feet
TotalZ = BodyOffsetZ+PosZ
TotalX = BodyOffsetX+PosX
TotalY = 150 + PosY’ using the value 150 to lower the centerpoint of rotation
TotalTransY = TotalTransY + PosY
TotalTransZ = TotalTransZ + TotalZ
TotalTransX = TotalTransX + TotalX
gosub GetBoogTan [TotalX, TotalZ]
TotalYbal = TotalYbal + TOINT((BoogTan180.0) / 3.141592)
gosub GetBoogTan [TotalX, TotalY]
TotalZbal = TotalZbal + TOINT((BoogTan180.0) / 3.141592)
gosub GetBoogTan [TotalZ, TotalY]
TotalXbal = TotalXbal + TOINT((BoogTan*180.0) / 3.141592)

return
;--------------------------------------------------------------------
;[BalanceBody]
BalanceBody:
TotalTransZ = TotalTransZ/6
TotalTransX = TotalTransX/6
TotalTransY = TotalTransY/6
if TotalYbal < -180 then 'Tangens fix caused by +/- 180 deg
TotalYbal = TotalYbal + 360
endif
if TotalZbal < -180 then
TotalZbal = TotalZbal + 360
endif
if TotalXbal < -180 then
TotalXbal = TotalXbal + 360
endif

;Balance rotation
TotalYBal = TotalYbal/6
TotalXBal = TotalXbal/6
TotalZBal = -TotalZbal/6
	
;Balance translation
LFGaitPosZ = LFGaitPosZ - TotalTransZ
LMGaitPosZ = LMGaitPosZ - TotalTransZ
LRGaitPosZ = LRGaitPosZ - TotalTransZ
RFGaitPosZ = RFGaitPosZ - TotalTransZ
RMGaitPosZ = RMGaitPosZ - TotalTransZ
RRGaitPosZ = RRGaitPosZ - TotalTransZ

LFGaitPosX = LFGaitPosX - TotalTransX
LMGaitPosX = LMGaitPosX - TotalTransX
LRGaitPosX = LRGaitPosX - TotalTransX
RFGaitPosX = RFGaitPosX - TotalTransX
RMGaitPosX = RMGaitPosX - TotalTransX
RRGaitPosX = RRGaitPosX - TotalTransX 

if (NOT((GaitType = 5) & (BalanceMode = 2))) then 
	LFGaitPosY = LFGaitPosY - TotalTransY
	LMGaitPosY = LMGaitPosY - TotalTransY
	LRGaitPosY = LRGaitPosY - TotalTransY
	RFGaitPosY = RFGaitPosY - TotalTransY
	RMGaitPosY = RMGaitPosY - TotalTransY
	RRGaitPosY = RRGaitPosY - TotalTransY
endif

return[/code]

This is the new BodyIK sub also taking care of the balance rotating:

[code];--------------------------------------------------------------------
;[BODY INVERSE KINEMATICS]
;BodyRotX - Global Input pitch of the body
;BodyRotY - Global Input rotation of the body
;BodyRotZ - Global Input roll of the body
;RotationY - Input Rotation for the gait
;PosX - Input position of the feet X
;PosZ - Input position of the feet Z
;BodyOffsetX - Input Offset betweeen the body and Coxa X
;BodyOffsetZ - Input Offset betweeen the body and Coxa Z
;RollY - RollY offset, needs to be added to the BodyPosY
;PitchY - PitchY offset, needs to be added to the BodyPosY
;BodyIKPosX - Output Position X of feet with Rotation
;BodyIKPosY - Output Position Y of feet with Rotation
;BodyIKPosZ - Output Position Z of feet with Rotation
BodyIK [PosX, PosZ, PosY, BodyOffsetX, BodyOffsetZ, RotationY]

;Calculating totals from center of the body to the feet
TotalZ = BodyOffsetZ+PosZ
TotalX = BodyOffsetX+PosX
;PosY are equal to a “TotalY”

;Successive global rotation matrix:
;Math shorts for rotation: Alfa (A) = Xrotate, Beta (B) = Zrotate, Gamma (G) = Yrotate
;Sinus Alfa = sinA, cosinus Alfa = cosA. and so on…

;First calculate sinus and cosinus for each rotation:
gosub GetSinCos [TOFLOAT(BodyRotX+TotalXBal)]
sinG = sinA
cosG = cosA
gosub GetSinCos [TOFLOAT(BodyRotZ+TotalZBal)]
sinB = sinA
cosB = cosA
gosub GetSinCos [TOFLOAT(BodyRotY+RotationY+TotalYBal)]

;Calcualtion of rotation matrix:
BodyIKPosX = TotalX-TOINT(TOFLOAT(TotalX)cosAcosB - TOFLOAT(TotalZ)cosBsinA + TOFLOAT(PosY)sinB)
BodyIKPosZ = TotalZ-TOINT(TOFLOAT(TotalX)cosGsinA + TOFLOAT(TotalX)cosAsinBsinG +TOFLOAT(TotalZ)cosAcosG-TOFLOAT(TotalZ)sinAsinBsinG-TOFLOAT(PosY)cosBsinG)
BodyIKPosY = PosY - TOINT(TOFLOAT(TotalX)sinAsinG - TOFLOAT(TotalX)cosAcosGsinB + TOFLOAT(TotalZ)cosAsinG + TOFLOAT(TotalZ)cosGsinA*sinB + TOFLOAT(PosY)cosBcosG)

return[/code]

And this is the code needed in the main loop:

TotalTransX = 0 'reset values used for calculation of balance TotalTransZ = 0 TotalTransY = 0 TotalXbal = 0 TotalZbal = 0 TotalYbal = 0 gosub BalCalcOneLeg -RFPosX+BodyPosX+RFGaitPosX, RFPosZ+BalCompZ+BodyPosZ+RFGaitPosZ,RFGaitPosY, RFOffsetX, RFOffsetZ] gosub BalCalcOneLeg -RMPosX+BodyPosX+RMGaitPosX, RMPosZ+BalCompZ+BodyPosZ+RMGaitPosZ,RMGaitPosY, RMOffsetX, RMOffsetZ] gosub BalCalcOneLeg -RRPosX+BodyPosX+RRGaitPosX, RRPosZ+BalCompZ+BodyPosZ+RRGaitPosZ,RRGaitPosY, RROffsetX, RROffsetZ] gosub BalCalcOneLeg [LFPosX-BodyPosX+LFGaitPosX, LFPosZ+BalCompZ+BodyPosZ+LFGaitPosZ,LFGaitPosY, LFOffsetX, LFOffsetZ] gosub BalCalcOneLeg [LMPosX-BodyPosX+LMGaitPosX, LMPosZ+BalCompZ+BodyPosZ+LMGaitPosZ,LMGaitPosY, LMOffsetX, LMOffsetZ] gosub BalCalcOneLeg [LRPosX-BodyPosX+LRGaitPosX, LRPosZ+BalCompZ+BodyPosZ+LRGaitPosZ,LRGaitPosY, LROffsetX, LROffsetZ] gosub BalanceBody

This is the main changes in the code. I’ve worked together with Xan to make this code work with his PS2 Phoenix code. Hopefully this will soon be available for all…

[kurte]

Thanks Zenta and Xan!!!

I can not wait to integrate this all in on my Hex and see it all work!

Xan, my guess is you are using an older ABB and not the BB2? I assume this as your LED code is using P4-P6 instead of the P12-P14.

Kurt

[xan]

Hi Galegu,

@galegu

a video of my tripod gait :
fr.youtube.com/watch?v=5OiR9qcerns

I did see pics of your wooden hexapod but I didn’t remember that it where yours I love the way that you used wood for the body parts! It moves ver fluent. Good luck implementing the other gaits.

Hi Zenta,

As I’ve said many times, and I can’t say it enough, your balance system is just brilliant! I’m very happy that we’ve got it to work with my PS2 version yesterday! I can’t wait to play with it some more this evening! 8)

I’m thinking of cutting the speed in half when the balance mode is on to get the effect more visual. At this moment the steps are to small when walking slow, so the effect is small. When walking fast, the speed is to high to see the effect.

Hi Kurte,

@kurte

Xan, my guess is you are using an older ABB and not the BB2? I assume this as your LED code is using P4-P6 instead of the P12-P14.

I started with the ABB but I upgraded to the BB2 when I switched from the wii-remote to the PS2 remote. So currently I’m running on a BB2. The ABB and BB2 both got the leds/buttons connected to P12-P14 by the way But i’ve got another reason for the pinning!

The pull-resistors for the PS2 remote are on the same I/O as the leds/buttons So what I did is add some wires from the jumpers, in the middle of the board, to P4-P6 to use them.

Xan

[zenta]

@Xan

As I’ve said many times, and I can’t say it enough, your balance system is just brilliant! I’m very happy that we’ve got it to work with my PS2 version yesterday! I can’t wait to play with it some more this evening! 8)

Thanks Xan!
It was fun working together with you. But I was a bit occupied with my own Phoenix playing with all the gait’s
Btw I’ve added another gait, a 12 step wave gait with only one swing position. The 18 step was a bit slow. I’ll test it later today and post the code.

@Xan

I’m thinking of cutting the speed in half when the balance mode is on to get the effect more visual. At this moment the steps are to small when walking slow, so the effect is small. When walking fast, the speed is to high to see the effect.

I couldn’t agree more about that. I did the same with my code. The balance mode isn’t that effective while moving very fast either, actually the balance code just make it a bit slower due to the calc speed.

BTW: Talking about speed… I wish the BAP was 10 times faster (yes I’m thinking of terrain adaption). Does anyone know if Basicmicro are going to make an even faster basic atom?

[xan]

Hi Zenta,

@zenta

It was fun working together with you. But I was a bit occupied with my own Phoenix playing with all the gait’s

The pleasure was all on my side Zenta!

@zenta

Btw I’ve added another gait, a 12 step wave gait with only one swing position. The 18 step was a bit slow. I’ll test it later today and post the code.

I thought that the 18 steps wave had only one upper already… I just have to wait until you post it

@zenta

I couldn’t agree more about that. I did the same with my code. The balance mode isn’t that effective while moving very fast either, actually the balance code just make it a bit slower due to the calc speed.

The balance mode is not about speed but about “the looksâ€

[kurte]

@Xan

Hi Kurte,

@kurte

Xan, my guess is you are using an older ABB and not the BB2? I assume this as your LED code is using P4-P6 instead of the P12-P14.

I started with the ABB but I upgraded to the BB2 when I switched from the wii-remote to the PS2 remote. So currently I’m running on a BB2. The ABB and BB2 both got the leds/buttons connected to P12-P14 by the way But i’ve got another reason for the pinning!

The pull-resistors for the PS2 remote are on the same I/O as the leds/buttons So what I did is add some wires from the jumpers, in the middle of the board, to P4-P6 to use them.

Good point about about using the jumpers… Some of us may want to jumper them back to the old positions (P4-P6 ) or any other unused pins. For example I may do that on my Rover using the new board I may not want to use these or at least P14 so I can use the Hardware Serial Port.

But again thanks for all the work you put into this!

Kurt

[flowbotx]

Xan/Zenta. This code is great! I probably won’t be able to get it into my VB.net controls on the first version release, though. (which, by the way, I am hoping to post by the end of the week).

Keep up the good work.

[kurte]

Ok, I have it somewhat working again on my CHR-3 with the PS2 reinstalled. I need to work some on my initial height off the ground calculation as when I say go on the floor it is still aways up… Boy it does walk fast in that one gait!!!

Next to modify and put in Zenta’s code.

Then I may put back in my Laser6 code. On the other hand I may wait and integrate in a version of the DIY remote control. I may try the airmod. But also I have ordered 3 XBee cards (1 1mw normal and 2 50mw Pros with a range of up to 1 mile). I also ordered 2 carrier AppBee-Sip cardsselmaware.com/appbee/home.htm, so I will then probably try converting a simple serial packet interface between the remote and the Hex and/or Rover. With Zigbee, I should be able to use 1 remote to talk to several different ones maybe all at the same time. But I will have to wait untill all of the necessary hardware arrives!

Other side note, Also depending on how well the Xbee works, I may also add a node onto my PC, so could then maybe use a VB program to talk to all and maybe add code on the different Atom Pros, that when some button is pressed either on the remote or maybe one of the buttons on the PRO, that puts the Pro into some transparent communication mode between the PC and the SSC-32 card through the PRO such that maybe you can use SEQ or the like without mods…

But all of this will probably all go into a new thread… Not sure which forum yet…

But for now I am having too much fun playing with my Hex!!! Great work!

Kurt

[zenta]

Hi,

Here are some more gaits. I added one 12 step wave gait with one swing position (NrLiftedPos = 1) and a 8 step tripod gait with 3 swing positions and HalfLiftHeight = TRUE.
Code:

[code]IF (GaitType = 5) THEN ;Tripod 8 steps
LRGaitLegNr = 5
RFGaitLegNr = 1
LMGaitLegNr = 1
RRGaitLegNr = 1
LFGaitLegNr = 5
RMGaitLegNr = 5

NrLiftedPos = 3
HalfLiftHeigth = TRUE	
TLDivFactor = 4	  
StepsInGait = 8	    
NomGaitSpeed = 110
sound P9,[150\1500]

ENDIF

IF (GaitType = 6) THEN ;Initate 12 step wavegait
LRGaitLegNr = 1
RFGaitLegNr = 11
LMGaitLegNr = 3
RRGaitLegNr = 7
LFGaitLegNr = 5
RMGaitLegNr = 9

NrLiftedPos = 1
TLDivFactor = 10
StepsInGait = 12
HalfLiftHeigth = FALSE
NomGaitSpeed = 120
sound P9,[150\1700]  

ENDIF [/code]

So now we have 8 GaitTypes (3 ripple/quadripple, 3 tripod and 2 wave). More to play with
Combined with the balance mode I find the gait with 2 or more swing positions (NrLiftedPos) to be the best.

[kurte]

Hi Zenta,

I thought I would have some fun and try to integrate your stuff in to my version. So far I have found that there are several undefined variables. So far I have added:

TotalY var sword ;BUGBUG:: From Zenta;Total X distance between the center of the body and the feet TotalXBal var sword ;BUGBUG:: From Zentas new code TotalYBal var sword ;BUGBUG:: From Zentas new code TotalZBal var sword ;BUGBUG:: From Zentas new code TotalTransY var sword ; TotalTransZ var sword TotalTransX var sword

Now I am getting a compile error on the line:

   if (NOT((GaitType = 5) & (BalanceMode = 2))) then 

As Balance mode is not defined anywhere and this is the only reference. Suggestions?

Also for anyone else merging in your new Gaits, you will need to edit the processing of the select button in the PS2 code to allow these new ones to be selected:
This line:

  		IF GaitType<6 THEN

used to have a 4 in it.

That is all for now. Cann’t wait to see it all work!

Kurt

[zenta]

@kurte

So far I have found that there are several undefined variables.

@Zenta

This is the main changes in the code.

Hi Kurt,

You are right about all the variables. There are some new of them. I’ll think you’ll get the picture when Xan releases his next version sometime

If you want I could post my complete code. But I’m not that structured programmer and tends to mess it all up Xan are a pro at this, so I think his “clean and tidy” code would be more easy to read.

But these are my balance variables:

;Balance BalanceMode var nib BalanceOneLeg var bit EnableBalRotation var bit TestLeg var nib BalCompZ var sbyte TravelHeightY var sword TotalTransX var sword TotalTransZ var sword TotalTransY var sword TotalYbal var sword TotalXBal var sword TotalZBal var sword TotalY var sword

I hope this will help you out.

[zenta]

@kurte

Now I am getting a compile error on the line:

   if (NOT((GaitType = 5) & (BalanceMode = 2))) then 

As Balance mode is not defined anywhere and this is the only reference. Suggestions?

To make it simple delete this line and only work with BalanceMode = 0 and 1 (off/on). In the main loop you can deside with a [code]if (BalanceMode = 1) then

	gosub BalCalcOneLeg -RFPo........[/code]

if you want to enable the balancemode or not.

[kurte]

Thanks Zenta,

I now have everything compiling and I have it downloaded to my hex. I added code to the X button on the PS2 remote to set the mode to 0-2. But I am not sure though the proper order of the code. In particular the code you added into the main loop. Ie where do you call the BalCalcOneLeg. After all of the other leg calcs? Before? Or as part of the leg calcs… Currently my main starts off like:

[code]main:
'Start time
GOSUB GetCurrentTime], lTimerStart

;Gait
GOSUB GaitSeq

'Reset IKsolution indicators
IKSolution = False
IKSolutionWarning = False
IKSolutionError = False

;Right Front leg
GOSUB BodyIK -RFPosX+BodyPosX+RFGaitPosX, RFPosZ+BodyPosZ+RFGaitPosZ,RFPosY+BodyPosY+RFGaitPosY, RFOffsetX, RFOffsetZ, RFGaitRotY]
GOSUB LegIK [RFPosX-BodyPosX+BodyIKPosX-RFGaitPosX, RFPosY+BodyPosY-BodyIKPosY+RFGaitPosY, RFPosZ+BodyPosZ-BodyIKPosZ+RFGaitPosZ]
RFCoxaAngle = IKCoxaAngle + CoxaAngle ;Angle for the basic setup for the front leg
RFFemurAngle = IKFemurAngle
RFTibiaAngle = IKTibiaAngle

;Right Middle leg
GOSUB BodyIK -RMPosX+BodyPosX+RMGaitPosX, RMPosZ+BodyPosZ+RMGaitPosZ,RMPosY+BodyPosY+RMGaitPosY, RMOffsetX, RMOffsetZ, RMGaitRotY]
GOSUB LegIK [RMPosX-BodyPosX+BodyIKPosX-RMGaitPosX, RMPosY+BodyPosY-BodyIKPosY+RMGaitPosY, RMPosZ+BodyPosZ-BodyIKPosZ+RMGaitPosZ]
RMCoxaAngle = IKCoxaAngle
RMFemurAngle = IKFemurAngle
RMTibiaAngle = IKTibiaAngle

;Right Rear leg
GOSUB BodyIK -RRPosX+BodyPosX+RRGaitPosX, RRPosZ+BodyPosZ+RRGaitPosZ,RRPosY+BodyPosY+RRGaitPosY, RROffsetX, RROffsetZ, RRGaitRotY]
GOSUB LegIK [RRPosX-BodyPosX+BodyIKPosX-RRGaitPosX, RRPosY+BodyPosY-BodyIKPosY+RRGaitPosY, RRPosZ+BodyPosZ-BodyIKPosZ+RRGaitPosZ]
RRCoxaAngle = IKCoxaAngle - CoxaAngle ;Angle for the basic setup for the front leg
RRFemurAngle = IKFemurAngle
RRTibiaAngle = IKTibiaAngle

;Left Front leg
GOSUB BodyIK [LFPosX-BodyPosX+LFGaitPosX, LFPosZ+BodyPosZ+LFGaitPosZ,LFPosY+BodyPosY+LFGaitPosY, LFOffsetX, LFOffsetZ, LFGaitRotY]
GOSUB LegIK [LFPosX+BodyPosX-BodyIKPosX+LFGaitPosX, LFPosY+BodyPosY-BodyIKPosY+LFGaitPosY, LFPosZ+BodyPosZ-BodyIKPosZ+LFGaitPosZ]
LFCoxaAngle = IKCoxaAngle + CoxaAngle ;Angle for the basic setup for the front leg
LFFemurAngle = IKFemurAngle
LFTibiaAngle = IKTibiaAngle

;Left Middle leg
GOSUB BodyIK [LMPosX-BodyPosX+LMGaitPosX, LMPosZ+BodyPosZ+LMGaitPosZ,LMPosY+BodyPosY+LMGaitPosY, LMOffsetX, LMOffsetZ, LMGaitRotY]
GOSUB LegIK [LMPosX+BodyPosX-BodyIKPosX+LMGaitPosX, LMPosY+BodyPosY-BodyIKPosY+LMGaitPosY, LMPosZ+BodyPosZ-BodyIKPosZ+LMGaitPosZ]
LMCoxaAngle = IKCoxaAngle
LMFemurAngle = IKFemurAngle
LMTibiaAngle = IKTibiaAngle

;Left Rear leg
GOSUB BodyIK [LRPosX-BodyPosX+LRGaitPosX, LRPosZ+BodyPosZ+LRGaitPosZ,LRPosY+BodyPosY+LRGaitPosY, LROffsetX, LROffsetZ, LRGaitRotY]
GOSUB LegIK [LRPosX+BodyPosX-BodyIKPosX+LRGaitPosX, LRPosY+BodyPosY-BodyIKPosY+LRGaitPosY, LRPosZ+BodyPosZ-BodyIKPosZ+LRGaitPosZ]
LRCoxaAngle = IKCoxaAngle - CoxaAngle ;Angle for the basic setup for the front leg
LRFemurAngle = IKFemurAngle
LRTibiaAngle = IKTibiaAngle

;zenta stuff start
TotalTransX = 0 'reset values used for calculation of balance
TotalTransZ = 0
TotalTransY = 0
TotalXbal = 0
TotalZbal = 0
TotalYbal = 0

if BalanceMode <> 0 then
gosub BalCalcOneLeg -RFPosX+BodyPosX+RFGaitPosX, RFPosZ+BalCompZ+BodyPosZ+RFGaitPosZ,RFGaitPosY, RFOffsetX, RFOffsetZ]
gosub BalCalcOneLeg -RMPosX+BodyPosX+RMGaitPosX, RMPosZ+BalCompZ+BodyPosZ+RMGaitPosZ,RMGaitPosY, RMOffsetX, RMOffsetZ]
gosub BalCalcOneLeg -RRPosX+BodyPosX+RRGaitPosX, RRPosZ+BalCompZ+BodyPosZ+RRGaitPosZ,RRGaitPosY, RROffsetX, RROffsetZ]
gosub BalCalcOneLeg [LFPosX-BodyPosX+LFGaitPosX, LFPosZ+BalCompZ+BodyPosZ+LFGaitPosZ,LFGaitPosY, LFOffsetX, LFOffsetZ]
gosub BalCalcOneLeg [LMPosX-BodyPosX+LMGaitPosX, LMPosZ+BalCompZ+BodyPosZ+LMGaitPosZ,LMGaitPosY, LMOffsetX, LMOffsetZ]
gosub BalCalcOneLeg [LRPosX-BodyPosX+LRGaitPosX, LRPosZ+BalCompZ+BodyPosZ+LRGaitPosZ,LRGaitPosY, LROffsetX, LROffsetZ]
gosub BalanceBody
endif
;zenta stuff end

GOSUB CheckAngles[/code]
But I could easily imagine the integration would be to conditionally call BalCalcOneLeg right after each to LegIK, which I may try next as I don’t think my results are looking right.

Sorry If I am jumping into too quickly…

Kurt

[zenta]

@kurte

Thanks Zenta,

I now have everything compiling and I have it downloaded to my hex. I added code to the X button on the PS2 remote to set the mode to 0-2. But I am not sure though the proper order of the code. In particular the code you added into the main loop. Ie where do you call the BalCalcOneLeg. After all of the other leg calcs? Before?

Hi, you must place all the main code right after the GaitSeq sub and most important before the BodyIK !