ok so i have begun porting over the phoenix code supplied by Kurte on Github.
It is an Arduino port that ill be running with an SSC32_PS2 and Botboarduino.
Im changing the leg configuration and i need to add an axis *HipRoll *to the IK.
So far i have gone through and added the axis to the various parts in the code but i am having a little brain fart understanding how the code will recognize the location and function of the HipRoll
Some of the dimensions are just guesses at this point and are not true as i dont have it assembled so i cannot bench test.
But this just shows the configuration i am going for. PLEASE NOTE I HAVE CHANGED COXA TO HipYaw BUT ALSO ADDED HipRoll.
image.ibb.co/gxMn36/Capture.png
image.ibb.co/gprWVm/Capture2.png
[code]//--------------------------------------------------------------------
//[LEG DIMENSIONS] (in mm)
//Right leg
#define RightHipRollLength 29
#define RightHipYawLength 29 //coxa
#define RightFemurLength 80
#define RightTibiaLength 80
#define RightFootLength 80
//Left leg
#define LeftHipRollLength 29
#define LeftHipYawLength 29 //coxa
#define LeftFemurLength 80
#define LeftTibiaLength 80
#define LeftFootLength 80
//--------------------------------------------------------------------
//[BODY DIMENSIONS]
#define RightHipRollAngle1 0 //Default HipYaw setup angle, decimals = 1
#define RightHipYawAngle1 0 //Default HipYaw setup angle, decimals = 1
#define LeftHipRollAngle1 0 //Default HipYaw setup angle, decimals = 1
#define LeftHipYawAngle1 0 //Default HipYaw setup angle, decimals = 1
#define RightOffsetX -50 //Distance X from center of the body to the Right Front HipYaw
#define RightOffsetZ 0 //Distance Z from center of the body to the Right Front HipYaw
#define LeftOffsetX 50 //Distance X from center of the body to the Left Front HipYaw
#define LeftOffsetZ 0 //Distance Z from center of the body to the Left Front HipYaw
//--------------------------------------------------------------------
//[START POSITIONS FEET] (COS) and (SIN) = BipedInitXZ Value x (0.5) and (0.866)
#define BipedInitXZ 75
#define BipedInitXZCos60 38 // COS(60) = .5
#define BipedInitXZSin60 65 // sin(60) = .866
#define BipedInitY 75
//Start positions of the Right leg
#define RightInitPosX 0
#define RightInitPosY 75
#define RightInitPosZ -10
//Start positions of the Left leg
#define LeftInitPosX 0
#define LeftInitPosY 75
#define LeftInitPosZ -10
//--------------------------------------------------------------------
//[Foot factors used in formula to calc Foot angle relative to the ground]
#define cFootConst 720
#define cFootMulti 2
#define cFootFactorA 70
#define cFootFactorB 60
#define cFootFactorC 50
[/code]
I am not sure i have added it to the index correctly?
for (LegIndex = 0; LegIndex <=1; LegIndex++)
{
HipRollAngle1[LegIndex] = min(max(HipRollAngle1[LegIndex], (short)pgm_read_word(&cHipRollMin1[LegIndex])),
(short)pgm_read_word(&cHipRollMax1[LegIndex]));
HipYawAngle1[LegIndex] = min(max(HipYawAngle1[LegIndex], (short)pgm_read_word(&cHipYawMin1[LegIndex])),
(short)pgm_read_word(&cHipYawMax1[LegIndex]));
FemurAngle1[LegIndex] = min(max(FemurAngle1[LegIndex], (short)pgm_read_word(&cFemurMin1[LegIndex])),
(short)pgm_read_word(&cFemurMax1[LegIndex]));
TibiaAngle1[LegIndex] = min(max(TibiaAngle1[LegIndex], (short)pgm_read_word(&cTibiaMin1[LegIndex])),
(short)pgm_read_word(&cTibiaMax1[LegIndex]));
#ifdef c5thDOF
if ((byte)pgm_read_byte(&cFootLength[LegIndex])) { // We allow mix of 3 and 4 DOF legs...
FootAngle1[LegIndex] = min(max(FootAngle1[LegIndex], (short)pgm_read_word(&cFootMin1[LegIndex])),
(short)pgm_read_word(&cFootMax1[LegIndex]));
}
#endif
//.........................................................>
//Default leg angle
const short cHipRollAngle1] PROGMEM = {RightHipRollAngle1, LeftHipRollAngle1};
const short cHipYawAngle1] PROGMEM = {RightHipYawAngle1, LeftHipYawAngle1};
I see in the part that handles the IK that the calculations are from the Coxa (HipYaw) to the Foot. Do i need to change that to HipRoll or do i need to add it elsewhere?
[code]//Calculate IKHipYawAngle and IKFeetPosXZ
GetATan2 (IKFeetPosX, IKFeetPosZ);
HipYawAngle1[LegIKLegNr] = (((long)Atan4*180) / 3141) + (short)pgm_read_word(&cHipYawAngle1[LegIKLegNr]);
//Using GetAtan2 for solving IKA1 and IKSW
//IKA14 - Angle between SW line and the ground in radians
IKA14 = GetATan2 (IKFeetPosY-FootOffsetY, IKFeetPosXZ-(byte)pgm_read_byte(&cHipYawLength[LegIKLegNr])-FootOffsetXZ);
[/code]
Any help would be good at this point. Once i have all the parts ordered and printed it will make this part of the project much easier to identify whats going on.
