First of all, I sorry for My English, It very bad, so some word I copy from google translate. I from VietNam, A country from Asian. I build Fire Ant, A-pod Clone from Zenta,. I hope he see my post. I realise some variable missing from Orion Red Ant of KurtE. I try to understand it with Basic.
Therefore, after 1 year of research. I done. But Rotate Mode, Walkmode, Translate Mode. never never smooth like apod of The Creator ( zenta
)
This is a little variable I change from Basic to C:
void of Abdom vs Head
void ServoDriver::OutputServoForHeadAbdomen(void){
g_InputController.AllowControllerInterrupts(false);
#ifdef ApodCode
HeadAbdomenAngle1[0] = min(max(HeadRotAngle1, cHeadRotMIN1), cHeadRotMAX1);
HeadAbdomenAngle1[1] = min(max(HeadPanAngle1, cHeadPanMIN1), cHeadPanMAX1);
HeadAbdomenAngle1[2] = min(max(HeadTiltAngle1, cHeadTiltMIN1), cHeadTiltMAX1);
HeadAbdomenAngle1[3] = min(max(AbdomPanAngle1, cAbdomPanMIN1), cAbdomPanMAX1);
HeadAbdomenAngle1[4] = min(max(AbdomTiltAngle1, cAbdomTiltMIN1), cAbdomTiltMAX1);
word PWMHeadAbdomen0;
word PWMHeadAbdomen1;
word PWMHeadAbdomen2;
word PWMHeadAbdomen3;
word PWMHeadAbdomen4;
PWMHeadAbdomen0 = ((long)(HeadAbdomenAngle1[0] + 900)) * 1000/991 + 592;
PWMHeadAbdomen1 = ((long)(HeadAbdomenAngle1[1] + 900)) * 1000/991 + 592;
PWMHeadAbdomen2 = ((long)(HeadAbdomenAngle1[2] + 900)) * 1000/991 + 592;
PWMHeadAbdomen3 = ((long)(HeadAbdomenAngle1[3] + 900)) * 1000/991 + 592;
PWMHeadAbdomen4 = ((long)(HeadAbdomenAngle1[4] + 900)) * 1000/991 + 592;
#ifdef cSSC_BINARYMODE
SSCSerial.write((byte)cHeadRotPin) + 0x80);
SSCSerial.write(PWMHeadAbdomen0 >> 8);
SSCSerial.write(PWMHeadAbdomen0 & 0xff);
…
Open / Close Man
#ifdef ApodCode
//Checks the mechanical limits of the Mandible servo
MandLeftPWM = min(max(MandLeftPWM, cMandLeftClosedPWM), cMandLeftOpenPWM);
MandRightPWM = min(max(MandRightPWM, cMandRightOpenPWM), cMandRightClosedPWM);
…
Attention Open and ClosePWM
Do not make mistakes 
***** Void Branch
void BranchRotateFunction(byte RotateFunction) {
switch (RotateFunction) {
case 0:
//HeadTracking
HeadControl (-IndDualShock[0], IndDualShock[2], -IndDualShock[1]);
g_InControlState.BodyRot1.x = -IndDualShock[1];
g_InControlState.BodyRot1.y = IndDualShock[2];
g_InControlState.BodyRot1.z = -IndDualShock[0];
HeadRotAngle1 = -HeadRotOut1;
HeadPanAngle1 = HeadPanOut1; // #### vAD1 CHANGED to invere
HeadTiltAngle1 = HeadTiltOut1;
break;
case 1:
//FixedHead:
g_InControlState.BodyRot1.x = -IndDualShock[1];
g_InControlState.BodyRot1.y = IndDualShock[2];
g_InControlState.BodyRot1.z = -IndDualShock[0];
HeadRotAngle1 = -g_InControlState.BodyRot1.z + HeadRotOut1;
HeadPanAngle1 = g_InControlState.BodyRot1.y + HeadPanOut1;
HeadTiltAngle1 = -g_InControlState.BodyRot1.x + HeadTiltOut1;
break;
case 2:
//HeadOnly:
HeadControl (IndDualShock[0], IndDualShock[2], IndDualShock[1]);
HeadRotAngle1 = -g_InControlState.BodyRot1.z + HeadRotOut1;
HeadPanAngle1 = g_InControlState.BodyRot1.y + HeadPanOut1;
HeadTiltAngle1 = -g_InControlState.BodyRot1.x + HeadTiltOut1;
break;
}
}
If someone see this line code and can fix. please help me. I want to see my robot rotate with Grimbal. My HeadRot is link 1 to body hex, HeadTilt is link 2, HeadPan is link 3. the HeadTilt and Pan not like apod from zenta 
BranchRotateFunction(RotateFunction);
g_InControlState.BodyPos.z = -IndDualShock[3]/3; // (PSS_LY) Translate (IndDualshock(6))
// Shift Center Point of Rotation for the body
if (ps2x.Button(PSB_R1)) { // R1 Button Test
g_InControlState.BodyRotOffset.z = -266.2; // I wrong here ? Pan or Tilt to Body dimension ?
}
else if(ps2x.Button(PSB_R2)){ // R2 Button Test
g_InControlState.BodyRotOffset.z = 238; // My Tilt Tail to center Body dimenssion
}
else
g_InControlState.BodyRotOffset.z = 0;
}
else{
g_InControlState.BodyRotOffset.z = 0;
g_InControlState.BodyRotOffset.y = 0;
g_InControlState.BodyRotOffset.x = 0;
}