Xan's Phoenix Code

Lynxmotion Legged Robots
561

Hi Zenta,

Thanks for sharing! I will defiantly try this out one day.

Xan

562

Well i had a chance to play with the gait this morning. very cool, and efficient.

again thanks for sharing. :wink: 8)

563

You are welcome guys, Iā€™m glad you got it to work Jonny. :smiley:

564

There is something just crossed my mind: Shouldnā€™t the Gaitspeed be a multiple of 20 to be smooth? The servos can only updated every 20 msec.

Greetings
Daniel

565

Hi Daniel,

Nope, the main key to get smooth movements from the phoenix code leis it 2 things. Calibration, speaks for itself I guess. The other one is timing. It is very important that the previous move is finished before the next one is send. This part is handled in this snippet of V2.0

[code] ;Sync BAP with SSC while walking to ensure the prev is completed before sending the next one
IF (GaitPosX(cRF) OR GaitPosX(cRM) OR GaitPosX(cRR) OR GaitPosX(cLF) OR GaitPosX(cLM) OR GaitPosX(cLR) OR |
GaitPosY(cRF) OR GaitPosY(cRM) OR GaitPosY(cRR) OR GaitPosY(cLF) OR GaitPosY(cLM) OR GaitPosY(cLR) OR |
GaitPosZ(cRF) OR GaitPosZ(cRM) OR GaitPosZ(cRR) OR GaitPosZ(cLF) OR GaitPosZ(cLM) OR GaitPosZ(cLR) OR |
GaitRotY(cRF) OR GaitRotY(cRM) OR GaitRotY(cRR) OR GaitRotY(cLF) OR GaitRotY(cLM) OR GaitRotY(cLR)) THEN

  ;Get endtime and calculate wait time
  GOSUB GetCurrentTime], lTimerEnd	
  CycleTime = (lTimerEnd-lTimerStart)/WTIMERTICSPERMS 

  ;Wait for previous commands to be completed while walking
  pause (PrevSSCTime - CycleTime - 45) MIN 1 ;	Min 1 ensures that there alway is a value in the pause command  
ENDIF

GOSUB ServoDriver [/code]

However, there is a limit on the SSCTime (Speed) The code need some time to read the input from the remote, calculate the next move and send the command to the SSC. V1.3 with the ps2 takes about 50 ms to read the ps2 inputs and calculate the next step. Sending the data to the SSC takes the 45ms youā€™ll see in the snippet above. V2.0 is capable to calculate the next step a lot quicker. Sending the data to the SSC can also be upgraded with the new SSC binary command set.

I hope this answers your question.

Xan

566

Another thing is that the SSC-32 servo controller take care of the 20mS update you mentioned here and has nothing to do with the Phoenix code running on the BAP.

567

I did not mean it in relation to each step. The transmission time is still the same from step to step, that is clear.
At a gate speed of 85 ms, we expected a timing between steps from the first step of 85 ms, 190 ms, 275 ms, 360 ms, 445 ms and so on. So the SSC will update the servos after 5, 10, 14, 18, 23 cycles.
The distance between the changes varied between 4 and 5 cycles.
If the gate speed is 80 ms it would be every time 4 cycles between the steps.

568

Hey guysā€¦ I am looking for a little guidance on a mod I want to make to the phoenix codeā€¦ I have a 5DOF arm on my CH3-R, and I want to add IK to it just like the legs use. I have some questions after bumbling thru the codeā€¦

Should I just make a duplicate ā€œLegIKā€ subroutine and call it something new? Then you that when the robot is in ā€œarmā€ mode?
Should I fake it and try to add the neck servos as another leg? and also just call it when in ā€œarmā€ mode?

Or what solution would you recommend?

569

Another possibility is you might look at the thread: Xanā€™s Rover with Arm In here he made a version of the IK code for the use of an Arm. You could then try to combine the two and use common functions for the majority of the stuffā€¦

Kurt

570

Brilliantā€¦ hence why I come and askā€¦ I will let you guys know what I come up with!

571

Hi,

For about a week ago when I was doing some GP stuff in the Phoenix code I looked at the calculations in the ServoDriver sub:

(CoxaAngle1(LegIndex) +900)*1000/1059+650

This formula give us a pwm/deg factor of 9,44. After checking the values I got from calibrating my LM Phoenix this factor was a little off. Here is a screenshoot from PEP that calculate the average pwm/deg factor of all 18 servos:

PEP average pwm_deg.jpg
PEP average pwm_deg.jpg845Ɨ343 175 KB

As you can see the result was 10,09.

I thought I should give this a try an added two constants:

cPwmDiv con 991 ;old 1059; trying out other values based on PEP average calcs for calculating the true servopwm.. cPFConst con 592 ;old 650 ; 900*(1000/cPwmDiv)+cPFConst must always be 1500 ;A PWM/deg factor of 10,09 give cPwmDiv = 991 and cPFConst = 592 ;For a modified 5645 (to 180 deg travel): cPwmDiv = 1500 and cPFConst = 900.
In the ServoDriver sub the code should be:

(CoxaAngle1(LegIndex) +900)*1000/cPwmDiv+cPFConst

From my comment you can see that this can be useful if you choose to use other servos with different pwm/deg factor.

To be honest I didnā€™t see very much difference in the end result. So this made me think that what if we use the pwm/deg factor of 10? A factor of 10 means cPwmDiv = 1000 and cPFConst = 600. This would make the pwm calcs in the ServoDriver very simple and maybe a little bit faster?:

CoxaAngle1(LegIndex) + 1500 :open_mouth: (almost to easyā€¦)
I tried that too, and it seem to work fine.

To be honest I liked the two constants to make it more universal for other servos. Any comments?

572

Sounds like a good update. For example didnā€™t you find the values different for digital servos?

Kurt

573

Hi Zenta,

This is already an old post but Iā€™m adding all Kurtā€™s and Your last improvements and bumped into this one.

You asked for some review. I looked at it and changed it a bit so it is a bit faster and better readable. I also changed some variable names. I hope you donā€™t mind.

MIN gives the highest value. So GaitPeak contains the highest value for the leg. If one leg has a higher GaitPeak

[code]
GaitPeak = 0 ;Reset
Walking= 0
LegIndex = 0
; Finding any the biggest value for GaitPos/Rot:
WHILE (LegIndex < 6) AND NOT Walking
GaitPeak = ABS(GaitPosX(LegIndex)) MIN |
ABS(GaitPosY(LegIndex)) MIN |
ABS(GaitPosZ(LegIndex)) MIN |
ABS(GaitRotY(LegIndex)) MIN |
GaitPeak

	IF (GaitPeak > 2) THEN ;if GaitPeak  is higher than 2 the robot are still walking
	  Walking= 1;
	ENDIF
	LegIndex = LegIndex+1
WEND

[/code]Zenta, Can you check if this also solves the issue you had with the original sync part in combination with XBEE?

Xan

574

Hi Xan,

Your update looks great! Iā€™ll give this a try when I get time for it.

Btw, I did also do some minor changes to the BodyIK (maybe we should call it BodyFK?) and the balance part. Instead of using the TotalX,Y,Z variables. I replaced them with CPR_X,Y,Z (Center Point of Rotation). Also added a BodyRotOffsetY,Z for setting the actual rotation point.

[code]BodyIKLeg var nib
BodyIK [PosX, PosZ, PosY, RotationY, BodyIKLeg]

;Calculating totals from center of the body to the feet
CPR_X = cOffsetX(BodyIKLeg)+PosX
CPR_Y = PosY + BodyRotOffsetY ; Define centerpoint for rotation along the Y-axis
CPR_Z = cOffsetZ(BodyIKLeg) + PosZ + BodyRotOffsetZ

;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 [BodyRotX1+TotalXBal1]
SinG4 = Sin4
CosG4 = Cos4

GOSUB GetSinCos [BodyRotZ1+TotalZBal1]
SinB4 = Sin4
CosB4 = Cos4

GOSUB GetSinCos [BodyRotY1+(RotationY*c1DEC)+TotalYBal1]
SinA4 = Sin4
CosA4 = Cos4

;Calcualtion of rotation matrix:
BodyIKPosX = (CPR_Xc2DEC - ( CPR_Xc2DECCosA4/c4DECCosB4/c4DEC - CPR_Zc2DECCosB4/c4DECSinA4/c4DEC + CPR_Yc2DECSinB4/c4DEC ))/c2DEC
BodyIKPosZ = (CPR_Zc2DEC - ( CPR_Xc2DECCosG4/c4DECSinA4/c4DEC + CPR_Xc2DECCosA4/c4DECSinB4/c4DECSinG4/c4DEC + CPR_Zc2DECCosA4/c4DECCosG4/c4DEC - CPR_Zc2DECSinA4/c4DECSinB4/c4DECSinG4/c4DEC - CPR_Yc2DECCosB4/c4DECSinG4/c4DEC ))/c2DEC
BodyIKPosY = (CPR_Y c2DEC - ( CPR_Xc2DECSinA4/c4DECSinG4/c4DEC - CPR_Xc2DECCosA4/c4DECCosG4/c4DECSinB4/c4DEC + CPR_Zc2DECCosA4/c4DECSinG4/c4DEC + CPR_Zc2DECCosG4/c4DECSinA4/c4DECSinB4/c4DEC + CPR_Yc2DECCosB4/c4DEC*CosG4/c4DEC ))/c2DEC

return [/code]

[code];[BalCalcOneLeg]
BalLegNr var nib
BalCalcOneLeg [PosX, PosZ, PosY, BalLegNr]
;Calculating centerpoint (of rotation) of the body to the feet
CPR_Z = cOffsetZ(BalLegNr)+PosZ
CPR_X = cOffsetX(BalLegNr)+PosX
CPR_Y = 150 + PosYā€™ using the value 150 to lower the centerpoint of rotation 'BodyPosY +
TotalTransY = TotalTransY + PosY
TotalTransZ = TotalTransZ + CPR_Z
TotalTransX = TotalTransX + CPR_X

gosub GetATan2 [CPR_X, CPR_Z]
TotalYbal1 = TotalYbal1 + (ATan4*1800) / 31415

gosub GetATan2 [CPR_X, CPR_Y]
TotalZbal1 = TotalZbal1 + ((ATan4*1800) / 31415) -900 'Rotate balance circle 90 deg

gosub GetATan2 [CPR_Z, CPR_Y]
TotalXbal1 = TotalXbal1 + ((ATan4*1800) / 31415) - 900 'Rotate balance circle 90 deg

return[/code]

575

Weā€™re currently working on TA. To get a good starting point I want to add all the improvements done by forum members into a new version. We already have a list with stuff to add from the topic New take on an old problemā€¦ Ground contact!

I have some things I like to discuss and would like your opinions.
List with new features for V2.1:

  1. binary SSC command set (KurtE) DONE
  2. Fast 115200 baud rate (KurtE) DONE
  3. New Gait Triple Tripod (Zenta) DONE, and it ROCKS!
  4. More accurate PWM constant (Zenta) DONE
  5. Something about SSC/BAP sync timing. (KurtE / Zenta) DONE
  6. Optional 3DOF/4DOF using optional compiling
  7. UseCodeOffsets (select between PO or your own offsets) CANCELLED
  8. LiPo Safety, useful for all LiPo owners DONE
    ***Edit:*9. Movable Center Point of Rotation DONE
    ***Edit:*10. Configure leg lengths for each leg DONE
    ***Edit:*11. Change to TimerA for less interrupts (KurtE) DONE
    ***Edit:*12. ReInitialize PS2 remote after timeout (KurtE) DONE
    ***Edit:*13. Change SSC/BAP/ARC specific functions to a separate driver file (KurtE) DONE
    ***Edit:*14. Update GP player for speed control (Zenta)

Making good code involves a couple of things. One of the most important things is to keep the code as readable as possible. This is already done by splitting up parts of the code to configure the program for different kind of hexapods. Some points in the list can be build using conditional compiling using #IFDEF. But keeping both options in will decrease the readability. So I want to use it as less as possible. With this in mind I would like to discuss the following things:

Questions referring to the list above:

  1. The binary command set is part of the latest SSC firmware. I would like to replace the ā€œnormalā€ command set with the binary. Are there any reasons to keep the ā€œnormalā€ command set around?
  2. The additional code for the 4DOF legs is basically spread trough whole the code. Making this optional really lowers the readability of the code. Personally I think it might be better to keep the 4DOF version separated. Also because I know there are also some quad (4 legs) versions of the phoenix code around. Iā€™m a bit worried the next step is to make the code optional for quads to. It would be hard to build the code optional for multiple DOF and Multiple number of legs. Separated files for 3DOF / 4DOF like we did for the remotes isnā€™t really an option since the extra DOF is so spread-ed. But I also understand that getting the latest functionality into your bot would be easiest if you can simply update to the latest core file. I also find it tricky since I donā€™t have a 4DOF bot. Anyway, input on this is very welcome!!
  3. Zenta seems to have found a bug in the GP player in combination with the offsets located in the registers. Is there a reason to keep the offsets in the code once this (bug?) is fixed? I like the way that the low level calibration values are stored in the low level driver.
  4. What do you have in mind with this? An analog input which measures the VS voltage and turns off the hex if the voltage drops below a certain point?

I want to hear your ideas about the given points! Please fill me in!

Xan

576

I will change the name to BodyFK and add this functionality. This sure is a great addin! :slight_smile:

577

Like I said in our last chat the #IFDEF sure has its con and pros, especially when it comes to readability. but at some point it might be hard to avoid. Keeping them to a minimum sounds like a good thing.

I vote for removing the old command set. But this will exclude all owners of the old V1 SSC-32 boards, if there are any leftā€¦ :unamused:

Personally I prefer the 3/4 DOF support in the same code. But Quad support should be a different project, like biped support.

Iā€™m willing to let the offsets go if Mike solve the GP bugā€¦ :wink:
The large offsets can be solved using the ā€œsnap hornā€ method and an additional offset in the code. This mean Iā€™ve to recalibrate my T-Hex :imp: .

If you use the LiPo for powering both electronics and servos youā€™ve to measure the VL. But yes, the main function is to turn off the hex when voltage drop to a certain level.

If the normal/binary command must be supported (and also the servooffsets) one solution could be a ServoDriver file in the project, like we do for the controller. That would make the code much easier to read. Is that doable?

578

Hi Xan and Zenta,

I totally agree with you about the readability of code is of great importance! But I also think having a different code base for each different configuration would also be difficult to a continue to evolve the code base for all of the 3 and 4 DOF Hex robots that Lynxmotion sells. This is why I personally think we should aim toward one code base for both 3 and 4 DOF T-Hexs. I also personally believe that we will want to evolve this such that not all of the legs are the same, as when we look at spiders and the like not all of their legs have the same length of joints. That is why I tried to add the Per leg config config code. I am sure we could do all of the 3/4/per without #ifdef, that is we could generalize to per leg config, Where in the 4DOF mode all of the values are the same for each leg. Likewise for the 3 DOF version, with the addition that for the one joint, you could either define as some fixed value (maybe 0) and with the Min/Max angles defined as the sameā€¦ The issue here is you would probably be taking a slight code/speed hit to generalize it this way.

I also vote for Binary mode. If the user has an old SSC-32, they can always order a new chip for it. This will probably only impact some older CHR-3 type robots that are several years old. Another option: split the SSC-32 code out of the main file into a Servo Drivers module, that is included as part of the project (This also would include the interface to running sequences). Then generate two versions of this, one for Binary the other for Ascii. Actually you could then generate a 3rd version for Arc32 which uses HSERVO2 support and rolls itā€™s own sequences code. This is how my Phoenix works! (i.e. - it would be great to roll my Arc32 phoenix version into this as well)

Note: I did something like this with my port to spin (propeller), I created an object to handle the SSC-32, I also developed a version that the servos were controlled directly by the propeller. Note: I only tested the second version with one leg as I did not wish to tear apart one of the robots to do this and I only had enough spare hardware to build one legā€¦

  1. I agree with simply using the SSC-32 offsetsā€¦ On the Arc32 code, I store them in EEPROM and have code that reads it in and also additional code that allows you to update themā€¦

Kurt

579

Hi Guys,

Thanks for the feedback!

1.) We will stick with the Binary mode for now. If someone wants to use the ā€œnormalā€ command set it is still a matter of copy, pasting 2 functions in to the new version. Jim, are you fine with this?

6.) Ok, we keep the source general for both 3DOF and 4DOF. Different number of legs will have a different source.

7.) Finally I talked you out of code offsets Zenta 8). Offsets will be removed from the list.

10.) New point, have separate length configurations for each of the legs. This will allow to build a hexapod using different legs. This will not be optional though, just add the same numbers if the same legs are used.

Thanks for the input!

Xan

580

Iā€™m ok with the way you want to handle the Binary mode. :smiley: