I checked the source code, and it sure looks like the PO values are taken into account. The register offset values are just the initial power-on values for the PO settings.
Mike
Thanks for your fast reply! 
Ok, thats good news. I might move over to use the reg.offsets on Phoenix then. So this mean that the SSC-32 configuration in the Visual SEQ should be left to default (0 deg = 1500) then?
I’m then wondering whats the default pwm/deg factor for Visual SEQ. Maybe 1000/90 =11,1 ?
Anyway, I’ll do some testing and let you know.
Hi,
Just a little update. Complete Phoenix code with the new GP with variable speed feature.
Thanks to Xan and Kurt for sharing your knowledge about programming!
PhoenixV22_PS2_XBee_API_GP.zip (55.9 KB)
Hi Zenta,
I just watched your video about the GP player with variable speed. And I am amazed! Having the speed variable is way cooler then I expected! Again great work and thanks for sharing!!!
Xan
ps. I’m not sure if you posted a link to the video someware
Awesome Work!
Hi Guys.
I am desperately trying to understand the Phoenix Leg IK for 4DOF, for a quadruped i am building.
I have managed to Get 4DOF of freedom leg to work in a spreadsheet (Not using Phoenix Code) if the tarsus remains vertical, but i am looking to have a Tarsus that adjusts its angle to allow for more flexibility.
I can see you have solved the problem, with “TarsToGroundAngle” so i have been picking at your Leg IK to see if i can see what’s going on, and make use of your amazing skills in my project.
I have a stripped down version of your leg IK Function, simplified just for one leg, so i can test the IK.
However I seem to be getting a TarsToGroundAngle of 1970.0!
I have been trying to solve this problem for 3 days now.
Any chance you can see what i am doing wrong ?
Thanks in advance.
Martin Turner
**
DEBUG**
TarsToGroundAngle=1970.0000
feetPosXZ=150.0000
TarsOffsetXZ=14.4127
TarsOffsetY=-81.7391
feetPosX=150.0000
feetPosY=100.0000
feetPosZ=0.0000
IKSW2=202.6203
IKA14=0.4580
IKA24=nan
CoxaAngle=0.00
FemurAngle=nan
TibiaAngle=nan
TarsAngle=nan
CODE
#include <math.h>
void setup() {
// put your setup code here, to run once:
Serial.begin(9600);
LegIK(150,100,0);
}
void loop() {
}
void LegIK(float feetPosX, float feetPosY, float feetPosZ) {
float IKSW2; //Length between Shoulder and Wrist, decimals = 2
float IKA14; //Angle of the line S>W with respect to the ground in radians, decimals = 4
float IKA24; //Angle of the line S>W with respect to the femur in radians, decimals = 4
float feetPosXZ; //Diagonal direction from Input X and Z
float TarsOffsetXZ; //Vector value \ ;
float TarsOffsetY; //Vector value / The 2 DOF IK calcs (femur and tibia) are based upon these vectors
float TarsToGroundAngle = 0; //Angle between tars and ground. Note: the angle are 0 when the tars are perpendicular to the ground
float TGA_A_H4;
float TGA_B_H3;
int COXALENGTH = 46;
int FEMURLENGTH = 70;
int TIBIALENGTH = 68;
int TARSLENGTH = 83;
int CFEMURHORNOFFSET1 = 0;
int CTIBIAHORNOFFSET1 = 0;
int CTARSHORNOFFSET1 = 0;
float cCoxaAngle = 0; //Initial Leg Angle
float CoxaAngle;
float FemurAngle;
float TibiaAngle;
float TarsAngle;
#define cTarsConst 720
#define cTarsMulti 2
#define cTarsFactorA 70
#define cTarsFactorB 60
#define cTarsFactorC 50
//Z = Forward / Backwardds
//X = Left / Right
//Y = Up / Down
//Calculate IKCoxaAngle and feetPosXZ
CoxaAngle = atan2f( feetPosZ, feetPosX ) * 180.0 / PI + cCoxaAngle;
//Length between the Coxa and tars [foot]
feetPosXZ = sqrtf ( square(feetPosX) + square(feetPosZ) );
//Calc the TarsToGroundAngle:
TarsToGroundAngle = -cTarsConst + (cTarsMulti*feetPosY) + (feetPosXZ*cTarsFactorA) - (feetPosXZ*feetPosY)/cTarsFactorB;
if (feetPosY < 0) //Always compensate TarsToGroundAngle when feetPosY it goes below zero
TarsToGroundAngle = TarsToGroundAngle - (feetPosY*cTarsFactorC); //TGA base, overall rule
if (TarsToGroundAngle > 40)
TGA_B_H3 = 20 + (TarsToGroundAngle/2);
else
TGA_B_H3 = TarsToGroundAngle;
if (TarsToGroundAngle > 30)
TGA_A_H4 = 24 + (TarsToGroundAngle/5);
else
TGA_A_H4 = TarsToGroundAngle;
if (feetPosY > 0) //Only compensate the TarsToGroundAngle when it exceed 30 deg (A, H4 PEP note)
TarsToGroundAngle = TGA_A_H4;
else if (((feetPosY <= 0) & (feetPosY > -10))) // linear transition between case H3 and H4 (from PEP: H4-K5*(H3-H4))
TarsToGroundAngle = TGA_A_H4 - (feetPosY*(TGA_B_H3-TGA_A_H4));
else //feetPosY <= -10, Only compensate TGA1 when it exceed 40 deg
TarsToGroundAngle = TGA_B_H3;
//Calc Tars Offsets:
TarsOffsetXZ = sin(TarsToGroundAngle*PI/180.0) * TARSLENGTH;
TarsOffsetY = cos(TarsToGroundAngle*PI/180.0) * TARSLENGTH;
//Using GetAtan2 for solving IKA1 and IKSW
//IKA14 - Angle between SW line and the ground in radians
IKA14 = atan2f(feetPosXZ-COXALENGTH-TarsOffsetXZ, feetPosY-TarsOffsetY);
//IKSW2 - Length between femur axis and tars
IKSW2 = sqrtf ( square(feetPosXZ-COXALENGTH-TarsOffsetXZ) + square(feetPosY-TarsOffsetY ) );
//IKA24 - Angle of the line S>W with respect to the femur in radians
IKA24 = acosf((square(FEMURLENGTH) - square(TIBIALENGTH) + square(IKSW2)) / (2 * FEMURLENGTH * IKSW2));
//IKFemurAngle
FemurAngle = -(IKA14 + IKA24) * 180.0 / PI + 90.0 + CFEMURHORNOFFSET1/10.0; //Normal
//IKTibiaAngle
IKA24 = acosf ((square(FEMURLENGTH) + square(TIBIALENGTH) - square(IKSW2)) / (2 * FEMURLENGTH * TIBIALENGTH)); // rused IKA24
TibiaAngle = -(90.0 - IKA24*180.0/PI + CTIBIAHORNOFFSET1/10.0);
TarsAngle = (TarsToGroundAngle + FemurAngle - TibiaAngle) + CTARSHORNOFFSET1/10.0;
//Set the Solution quality
// if(IKSW2 < (FEMURLENGTH+TIBIALENGTH-30))
// IKSolution = 1;
// else {
// if(IKSW2 < (FEMURLENGTH+TIBIALENGTH))
// IKSolutionWarning = 1;
// else
// IKSolutionError = 1;
// }
Serial.print("TarsToGroundAngle=");
Serial.print(TarsToGroundAngle, 4);
Serial.print("\n");
Serial.print("feetPosXZ=");
Serial.print(feetPosXZ, 4);
Serial.print("\n");
Serial.print("TarsOffsetXZ=");
Serial.print(TarsOffsetXZ, 4);
Serial.print("\n");
Serial.print("TarsOffsetY=");
Serial.print(TarsOffsetY, 4);
Serial.print("\n");
Serial.print("feetPosX=");
Serial.print(feetPosX, 4);
Serial.print("\n");
Serial.print("feetPosY=");
Serial.print(feetPosY, 4);
Serial.print("\n");
Serial.print("feetPosZ=");
Serial.print(feetPosZ, 4);
Serial.print("\n");
Serial.print("IKSW2=");
Serial.print(IKSW2, 4);
Serial.print("\n");
Serial.print("IKA14=");
Serial.print(IKA14, 4);
Serial.print("\n");
Serial.print("IKA24=");
Serial.print(IKA24, 4);
Serial.print("\n");
Serial.print("CoxaAngle=");
Serial.print(CoxaAngle, 2);
Serial.print("\n");
Serial.print("FemurAngle=");
Serial.print(FemurAngle, 2);
Serial.print("\n");
Serial.print("TibiaAngle=");
Serial.print(TibiaAngle, 2 );
Serial.print("\n");
Serial.print("TarsAngle=");
Serial.print(TarsAngle, 2);
Serial.print("\n");
}
#define square(x) ((x)*(x))