Xan's Phoenix Code

Lynxmotion Legged Robots
541

Hi Kurt,
I’ve just been reading this thread. And suddenly I got it! :laughing: I think this code will work. I knew that you would come out with something much more elegant! Looking forward to test it out later today.

Thanks Kurt!

542

Hi and welcome to this forum!

I’m not sure whats causing this at the moment but I know Kurt has been running Xan’s code on his CH3-R. And I believe he mentioned something about some variables that needed to be changed. I’m not sure though. Hopefully Kurt or others can give you a hint.

Btw, did you test the single leg control if that worked too for the middle legs?

543

Hi Kurt,

Did a short test of your latest suggestion and the IF (GaitPeak>2)… worked very fine!
Thanks! :smiley:

544

Hey zenta,

Did you ever make a t7C config for your phoenix?

I’m kinda guessing your phoenix is collecting dust since you have built your amazing A-pod and working on your T Hex thing I saw on another post.

Kurt your assembly code for reading pulsin commands is amazing, I copied and pasted it to test some RC inputs based on the problem I pmed you and was laughing about how easy it was to take that code paste it in and change some variables around a bit…

Even that code I sent you in a PM and put your Assembly bit in zenta’s RC 1.3 phoenix code he sent me “Replacing all 7 pulsin commands” for an un-modified receiver I was amazed the phoenix wasn’t shuddering anymore! it was almost as smooth as it was with the ps2 control.

I almost wish there was a dumbed down version of the 2.0 code without all the features so that someone such as myself could add features back into it, I’m finding it pretty challenging to take this 2.0 code and add RC features into it when the code for myself out of the box does not work for me with my unique setup “RC control”… I kinda like the ability to have something that works out of the box and then in the learning experience I have the ability to break it “so I know what i just did broke it” … weird I know but hey :slight_smile:

comments, thoughts, … all welcome in my book.

–Aaron

545

i do believe before Xan started work on his v2 i remember a discussion about expanding the configuration files for either:

Control
RC,
ps2,
autonomous,
PC,
**
Unit**
Octopod
Hex,
Quad,

Features
single leg control
Gaits,

etc.
i think you would need to wait for something like this. maybe V2.1?

546

Hi Zenta,
thanks for the welcome. I did try the single leg control as you suggested; in this mode it is possible to rotate the middle leg coxa. Although the control via the PS2 remote is rather jumpy - but this seems to be a precission issue with these controllers, as already discussed here. Eventually, I’d like the CH3 to run automomously, with behavioral routines adjusting the different parameters for body position and gait generator.
Best,
KarlElch

547

Yeah innerbreed I was looking at the discussion a while ago many moon ago.

I guess I spent a couple months working with the 1.3 code with RC that zenta had shared me with… I learned how it worked did some neat things with it.

Although the code he sent me worked out of the box I had the ability to change it, with this new 2.0 code I don’t have code that works for me out of the box so getting it working for my specific setup is much more of a a challenge, im almost semi blind when trying to get it working for myself.

other little quirks like that…

Its funny cause I was able to take Kurte’s assembly code and plop it into the code ive been working with “1.3 RC” code zenta shared with me and it 95% works! and it took roughly 10 minutes.

So that’s my story :slight_smile:

548

I have a couple issues still with mine also… I have been so busy with work that I havnt had a chance to debug it yet tho… try this and tell me what it does…
Push the button on the PS2 controller that puts you in the mode to control the body (I think its the L2 trigger button?)
If I twist the body, you would expect all the feet to stay on the ground and the body twists… my two middle legs hold their horizontal position at COXA so the feet scoot across the floor instead of being planted.
If I lean the body side to side, the middle legs come up off the ground in what appears to be opposite motion of what would be expected.
Could be something backwards in the middle leg configs??
I havnt had a chance to debug and look… but thats what I have noticed so far…
I can post some short vids also if you guys want to see what I am talking about

549

Xan is going to have a field day removing all of these posts from here. All of you guys need to start your own help request threads!..

550

Yah I know, but this really is a question about the code :laughing:

I have been playing with this code and I am trying to understand what it is supposed to do:

[code]LegIKLegNr var nib
LegIK [IKFeetPosX, IKFeetPosY, IKFeetPosZ, LegIKLegNr]

;Calculate IKCoxaAngle and IKFeetPosXZ
GOSUB GetATan2 [IKFeetPosX, IKFeetPosZ]
CoxaAngle1(LegIKLegNr) = ((ATan4*180) / 3141) + cCoxaAngle1(LegIKLegNr)

;Length between the Coxa and tars (foot)
IKFeetPosXZ = XYhyp2/c2DEC

;Using GetAtan2 for solving IKA1 and IKSW
;IKA14 - Angle between SW line and the ground in radians
GOSUB GetATan2 [IKFeetPosY, IKFeetPosXZ-cCoxaLength], IKA14
;IKSW2 - Length between femur axis and tars
IKSW2 = XYhyp2
;IKA2 - Angle of the line S>W with respect to the femur in radians
Temp1 = (((cFemurLength*cFemurLength) - (cTibiaLength*cTibiaLength))*c4DEC + (IKSW2*IKSW2))
Temp2 = ((2*cFemurlength)*c2DEC * IKSW2)
GOSUB GetArcCos [Temp1 / (Temp2/c4DEC) ], IKA24

…
[/code]
For the fun of it I am trying to make this code work in C running on a Bap28 on a CHR-3. Ok Assuming I am just starting up the bot (just hit the start button. Suppose that the IKFeetPosX,Y,Z passed in was: 52 80 91 and Suppose that Coxa=29, Femur=57, Tibia= 141

The first call GetAtan2(52, 91) generates XYHp2 = sqrt(525210000 + 9191100000) = 10480
So IKFeetPosXZ = 104
So GetAtan2(80,104-29)… IKSW2= XYHyp2 = 10965
Temp1 = ((5757)-(141141)10000 + 1096510965 = 4248878521
Temp2 = 2 57100 * 10965=125,001,000

Now you pass (4248878521/(125001000/10000)) to GetArcCos. If my calculations are correct this comes out to something like: 339910
This does not fit into a sword variable. Besides it is obviously larger than the C4DEC max that this function handles. So am I miscalculating something?

thanks
Kurt

551

Hi Kurt,

You forgot one set of brackets, it should be Temp1 = (((5757)-(141141))10000 + 1096510965 = -46088775

552

Thanks Zenta,

I had same results printing out in Basic and C for Temp1, so I assumed it was correct. But then the issue probably is:

Temp1 var long Temp2 var long
Both Temp1 and Temp2 are defined as LONG not SLONG…

Kurt

Edit: changed my c version from “unsigned long” to “long” and now the values are looking more correct. Now back to fixing my printf… Converting to be able to output longs screwed up signed shorts…

553

I’m just getting started with robotics and I’m most interested in hexapods, so I purchased a phoenix (newest, Basic Atom Pro 28), assembled it and its working like a charm. Now I’m tinkering in the code and trying to learn from it, but you have so many features, various modes and walking gaits, I’m finding it a bit overwhelming when still trying to understand the basic process of walking.

Does anyone have a very simple version floating around somewhere? Possible a: power on -> walk/strafe/turn response from joystick -> power down feature set? No GP player or single leg control or multiple gait selection. I think that this would really help me get my hands dirty faster and build something I understand from the ground up. Thanks for any help!

554

yes you can download V1.3 or lower, its a bit more easier to work with. :wink:
you will find them at the bottom of the page:
lynxmotion.com/images/html/proj102.htm

555

Excellent, I was having the worst luck tracking down these old versions. Thanks for the link.

556

Hi everyone,

I’ve been lurking around for some time, looking at all the awesome stuff here (including skimming the 500+ previous posts in this thread ^^), and finally I am starting to act on my own project. Just for background, the project I started is a 4DOF quadrapod with a leg design different from Zenta’s T-HEX, using full IK (it’s gonna be bloody, be prepared).

Along the way, I came to study Xan’s great code, and even if I usually practice more C or various object languages than BASIC, I think I understood the parts I was interested in, i.e. the leg IK and gait sequencer. However your implementation of the body IK part still bugs me for some reason.

If I understand correctly, what is done in the bodyIK subroutine is as follow:
First, express the position of the foot relative to the center of the body (change the coordinate system to go from a system centered on the coxa to a system centered on the center of the body)
Then compute the sin and cos of each of the angles of the body rotation.
And finally, you do in one big step what appears to be both the application of the 3 successive rotation matrices (haven’t checked the details but it looks quite like that) and a shift of the coordinate system back to where the coxa is after this rotation.
So the results (BodyIKPosX, BodyIKPosY, BodyIKPosY) are the new target coordinates of the foot after applying body rotation. This could be looked at as keeping the body level and rotating/translating the floor while keeping the feet on it, and then measuring the new position of the feet.

Am I correct?

The thing I don’t get is why you have to recompute the rotation matrices for each leg. I would have thought you could compute it once, then apply it to each foot target position (expressed in the body coordinate system). It would also be quite an economy in term of processing time… Am I missing something? Maybe something to do with the RotationY parameter (I don’t really get this one) ?

I hope all this is comprehensible, my knowledge of the proper mathematical vocabulary in English is not as thorough as I would like it to be… Sorry for the inconvenience :confused:

557

Hi again,

I (still) don’t really get the way the “RotationY” parameter of the BodyIK() function works.
The comments in the code tells me that it is the “Input for rotation of a single feet for the gait” and “Input Rotation for the gait”.

I presume this means that this represents the rotation of the robot, like when the robot is moving along a curve, but then why is it needed to have one value per leg, instead of only one value (equal to the rotation of the robot) ?

558

RotationY = GaitRotY(LegIndex)
It simply describe how much each leg needs to be rotated (in relative to center of body) when doing walking-rotation, the TravelRotationY is the input variable for gait rotation.

559

Ok I think I get it now, thank you.

560

Hi,

Like I mentioned in the Archer thread I did some modifications to the gait section in the Phoenix code by adding support for up to 5 lifted leg positions. And also introduced a new gait pattern called Triple tripod gait.

At first, the HalfLiftHeight variable are changed from bit to nib.

Currently this is the gait code I’m using:

[code]GaitSelect
;Gait selector
;Removed some gait types, gaits using HalfLiftHeigth = 3 gives the best result

IF (GaitType = 0) THEN ;Ripple Gait 12 steps
GaitLegNr(cLR) = 1
GaitLegNr(cRF) = 3
GaitLegNr(cLM) = 5
GaitLegNr(cRR) = 7
GaitLegNr(cLF) = 9
GaitLegNr(cRM) = 11

NrLiftedPos = 3
HalfLiftHeigth = 3;GaitPosY(GaitCurrentLegNr) = -3*LegLiftHeight/(3+HalfLiftHeigth) = -LegLiftHeight*(3/6) =  -LegLiftHeight/2 
TLDivFactor = 8	  
StepsInGait = 12	
NomGaitSpeed = 70

ENDIF

IF (GaitType = 1) THEN ;Tripod 8 steps
GaitLegNr(cLR) = 5
GaitLegNr(cRF) = 1
GaitLegNr(cLM) = 1
GaitLegNr(cRR) = 1
GaitLegNr(cLF) = 5
GaitLegNr(cRM) = 5

NrLiftedPos = 3
HalfLiftHeigth = 3;GaitPosY(GaitCurrentLegNr) = -3*LegLiftHeight/(3+HalfLiftHeigth) = -LegLiftHeight*(3/6) =  -LegLiftHeight/2 	
TLDivFactor = 4	  
StepsInGait = 8	    
NomGaitSpeed = 70

ENDIF

IF (GaitType = 2) THEN ;Triple Tripod 12 steps
GaitLegNr(cRF) = 3
GaitLegNr(cLM) = 4
GaitLegNr(cRR) = 5
GaitLegNr(cLF) = 9
GaitLegNr(cRM) = 10
GaitLegNr(cLR) = 11

NrLiftedPos = 3
HalfLiftHeigth = 3 	
TLDivFactor = 8  
StepsInGait = 12	    
NomGaitSpeed = 60

ENDIF

IF (GaitType = 3) THEN ;Triple Tripod 16 steps
GaitLegNr(cRF) = 4
GaitLegNr(cLM) = 5
GaitLegNr(cRR) = 6
GaitLegNr(cLF) = 12
GaitLegNr(cRM) = 13
GaitLegNr(cLR) = 14

NrLiftedPos = 5 ;! ;)
HalfLiftHeigth = 1 ;GaitPosY(GaitCurrentLegNr) = -3*LegLiftHeight/(3+HalfLiftHeigth) = -LegLiftHeight*(3/4) 	
TLDivFactor = 10  
StepsInGait = 16	    
NomGaitSpeed = 60

ENDIF

IF (GaitType = 4) THEN ;Wave 24 steps
GaitLegNr(cLR) = 1
GaitLegNr(cRF) = 21
GaitLegNr(cLM) = 5

GaitLegNr(cRR) = 13
GaitLegNr(cLF) = 9
GaitLegNr(cRM) = 17
  
NrLiftedPos = 3
HalfLiftHeigth = 3	
TLDivFactor = 20	  
StepsInGait = 24	    
NomGaitSpeed = 70

ENDIF
return
;--------------------------------------------------------------------
;[GAIT Sequence]
GaitSeq
;Calculate Gait sequence
LastLeg = 0
;Check IF the Gait is in motion
GaitInMotion = ((ABS(TravelLengthX)>cTravelDeadZone) | (ABS(TravelLengthZ)>cTravelDeadZone) | (ABS(TravelRotationY)>cTravelDeadZone) )
IF NrLiftedPos = 5 THEN
LiftDivFactor = 4
ELSE
LiftDivFactor = 2
ENDIF

for LegIndex = 0 to 5 ; for all legs

if LegIndex = 5 then ; last leg
  LastLeg = 1 
endif 

GOSUB Gait [LegIndex]

next ; next leg
return
;--------------------------------------------------------------------
;[GAIT]
GaitCurrentLegNr var nib
Gait [GaitCurrentLegNr]

;Clear values under the cTravelDeadZone
IF (GaitInMotion=0) THEN
TravelLengthX=0
TravelLengthZ=0
TravelRotationY=0
ENDIF

;Leg middle up position
;Gait in motion Gait NOT in motion, return to home position
IF (GaitInMotion & (NrLiftedPos=1 | NrLiftedPos=3 | NrLiftedPos=5) & GaitStep=GaitLegNr(GaitCurrentLegNr)) | (NOT GaitInMotion & GaitStep=GaitLegNr(GaitCurrentLegNr) & ((ABS(GaitPosX(GaitCurrentLegNr))>2) | (ABS(GaitPosZ(GaitCurrentLegNr))>2) | (ABS(GaitRotY(GaitCurrentLegNr))>2))) THEN ;Up
GaitPosX(GaitCurrentLegNr) = 0
GaitPosY(GaitCurrentLegNr) = -LegLiftHeight
GaitPosZ(GaitCurrentLegNr) = 0
GaitRotY(GaitCurrentLegNr) = 0
ELSE

;Optional Half heigth Rear
IF ((NrLiftedPos=2 & GaitStep=GaitLegNr(GaitCurrentLegNr)) | (NrLiftedPos>=3 & (GaitStep=GaitLegNr(GaitCurrentLegNr)-1 | GaitStep=GaitLegNr(GaitCurrentLegNr)+(StepsInGait-1)))) & GaitInMotion THEN
  GaitPosX(GaitCurrentLegNr) = -TravelLengthX/LiftDivFactor
  GaitPosY(GaitCurrentLegNr) = -3*LegLiftHeight/(3+HalfLiftHeigth) ; Easier to shift between div factor: /1 (3/3), /2 (3/6) and 3/4
  GaitPosZ(GaitCurrentLegNr) = -TravelLengthZ/LiftDivFactor
  GaitRotY(GaitCurrentLegNr) = -TravelRotationY/LiftDivFactor
ELSE
  
  ;Optional half heigth front
  IF (NrLiftedPos>=2) & (GaitStep=GaitLegNr(GaitCurrentLegNr)+1 | GaitStep=GaitLegNr(GaitCurrentLegNr)-(StepsInGait-1)) & GaitInMotion THEN
    GaitPosX(GaitCurrentLegNr) = TravelLengthX/LiftDivFactor
    GaitPosY(GaitCurrentLegNr) = -3*LegLiftHeight/(3+HalfLiftHeigth)
    GaitPosZ(GaitCurrentLegNr) = TravelLengthZ/LiftDivFactor
    GaitRotY(GaitCurrentLegNr) = TravelRotationY/LiftDivFactor
  ELSE    
  		;5 LiftedPos Half heigth Rear
		IF ((NrLiftedPos=5 & (GaitStep=GaitLegNr(GaitCurrentLegNr)-2 ))) & GaitInMotion THEN
  			GaitPosX(GaitCurrentLegNr) = -TravelLengthX/2
  			GaitPosY(GaitCurrentLegNr) = -LegLiftHeight/2
  			GaitPosZ(GaitCurrentLegNr) = -TravelLengthZ/2
  			GaitRotY(GaitCurrentLegNr) = -TravelRotationY/2
		ELSE
  		
  			;5 LiftedPos Half heigth Front
  			IF (NrLiftedPos=5) & (GaitStep=GaitLegNr(GaitCurrentLegNr)+2 | GaitStep=GaitLegNr(GaitCurrentLegNr)-(StepsInGait-2)) & GaitInMotion THEN
    			GaitPosX(GaitCurrentLegNr) = TravelLengthX/2
    			GaitPosY(GaitCurrentLegNr) = -LegLiftHeight/2
    			GaitPosZ(GaitCurrentLegNr) = TravelLengthZ/2
    			GaitRotY(GaitCurrentLegNr) = TravelRotationY/2
  			ELSE
   				;Leg front down position (only occour when NrLiftedPos = 1 ?)
  				IF (GaitStep=GaitLegNr(GaitCurrentLegNr)+NrLiftedPos | GaitStep=GaitLegNr(GaitCurrentLegNr)-(StepsInGait-NrLiftedPos)) & GaitPosY(GaitCurrentLegNr)<0 THEN
      			  GaitPosX(GaitCurrentLegNr) = TravelLengthX/2
      			  GaitPosZ(GaitCurrentLegNr) = TravelLengthZ/2
      			  GaitRotY(GaitCurrentLegNr) = TravelRotationY/2      	
      			  GaitPosY(GaitCurrentLegNr) = 0	;Only move leg down at once if terrain adaption is turned off
  				;Move body forward      
  				ELSE
      			  GaitPosX(GaitCurrentLegNr) = GaitPosX(GaitCurrentLegNr) - (TravelLengthX/TLDivFactor)     
      			  GaitPosY(GaitCurrentLegNr) = 0  
      			  GaitPosZ(GaitCurrentLegNr) = GaitPosZ(GaitCurrentLegNr) - (TravelLengthZ/TLDivFactor)
      			  GaitRotY(GaitCurrentLegNr) = GaitRotY(GaitCurrentLegNr) - (TravelRotationY/TLDivFactor)
    			ENDIF
    		ENDIF
    	ENDIF	
  ENDIF
ENDIF

ENDIF

;Advance to the next step
IF LastLeg THEN ;The last leg in this step
GaitStep = GaitStep+1
IF GaitStep>StepsInGait THEN
GaitStep = 1
ENDIF
ENDIF

return[/code]

For simplification I’ve set the GaitPosY = -LegLiftHeight/2 constantly on the rear and front position when choosing 5 lifted positions. By setting the HalfLiftHeight to 0, 1 or 3 the divfactor between GaitPosY and LegLiftHeigth is /1, /2 or 3/4.

GaitPosY(GaitCurrentLegNr) = -3*LegLiftHeight/(3+HalfLiftHeigth)

I found the the 3/4 useful when using 5 lifted positions