Hexapod Single leg test

#include <Servo.h>
Servo legServo[3];

// const float PI = 3.141592;
const float PIby2 = PI/2;


const float cos30 = cos(0.5236);
const float sin30 = sin(0.5236);

const float cos90 = cos(PI/2);
const float sin90 = sin(PI/2);

const float cos150 = cos(2.618);
const float sin150 = sin(2.618);

const float cos210 = cos(3.6652);
const float sin210 = sin(3.6652);

const float cos270 = cos(PI/2+PI);
const float sin270 = sin(PI/2+PI);

const float cos330 = cos(5.7596);
const float sin330 = sin(5.7596);





const float CoxaLength  = 13.0;          //Length of the Coxa [mm]
const float FemurLength = 34.0;          //Length of the Femur [mm]
const float TibiaLength = 72.0;          //Lenght of the Tibia [mm]

/*
int x = (CoxaLength+FemurLength)*cos(60.0*PI/180.0);
int z = (CoxaLength+FemurLength)*sin(60.0*PI/180.0);
int y = TibiaLength;


int x = CoxaLength+FemurLength;
int z = 0;
int y = TibiaLength;


int x = (CoxaLength+FemurLength)*cos(60*PI/180.0);
int z = -(CoxaLength+FemurLength)*sin(60*PI/180.0);
int y = TibiaLength;

int x = -(CoxaLength+FemurLength)*cos(60*PI/180.0);
int z = -(CoxaLength+FemurLength)*sin(60*PI/180.0);
int y = TibiaLength;

int x = -(CoxaLength+FemurLength);
int z = 0;
int y = TibiaLength;


int x = -(CoxaLength+FemurLength)*cos(60*PI/180.0);
int z = (CoxaLength+FemurLength)*sin(60*PI/180.0);
int y = TibiaLength;

*/

int x = (CoxaLength+FemurLength)*cos(60*PI/180.0);
int z = -(CoxaLength+FemurLength)*sin(60*PI/180.0);
int y = TibiaLength;



const int MAX_COMMAND_LENGTH = 8;

int command[MAX_COMMAND_LENGTH] = {0};
int index = 0;

void setup ()
{


     Serial.begin (9600);

  // =========== Servos===============================
          legServo[0].attach(29);
          delay(10);
          legServo[0].writeMicroseconds(1500);
          delay(20);
          legServo[1].attach(30);
          delay(10);
          legServo[1].writeMicroseconds(1500);
          delay(20);
          legServo[2].attach(31);
          delay(10);
          legServo[2].writeMicroseconds(1500);
          delay(20);
     //ResetLegPosition();
     delay(1500);
  // ================================================


}


void loop (){
 
  float CAngle=0;
  float FAngle=0;
  float TAngle=0;
 
  int xc = 0;
  int zc = 0;
  int yc = 0;
 
  int x2 = 0;
  int z2 = 0;
  int y2 = 0;
 
  if (ReceiveCommand() == true){
    index = 0;

    int temp = 0;
    int i = 1;
    int sign = 1;
   
    while (command[i] != '/'){
      if (command[i] == '-') {
        sign = -1;
        i++;
      }
      temp = temp*10 + Char2Int(command[i]);
      i++;
    }
   
    temp = sign*temp;
   
    if (command[0] == 'x')  xc = temp;
    else if (command[0] == 'z') zc = temp;
    else if (command[0] == 'y') yc = temp;
   
    x = x+xc;
    z = z+zc;
    y = y+yc;
   
  Serial.print("X(");
    Serial.print(x,DEC);
    Serial.print(")  Z(");
    Serial.print(z,DEC);
    Serial.print(")  Y(");
    Serial.print(y,DEC);
    Serial.println(")");

 /*
   x2 = x*cos30 + z*-sin30;
   z2 = x*sin30 + z*cos30;
 
   x2 = x*cos90 + z*-sin90;
   z2 = x*sin90 + z*cos90;
 
   x2 = x*cos150 + z*-sin150;
   z2 = x*sin150 + z*cos150;

   x2 = x*cos210 + z*-sin210;
   z2 = x*sin210 + z*cos210;
 
   x2 = x*cos270 + z*-sin270;
   z2 = x*sin270 + z*cos270;
 
   x2 = x*cos330 + z*-sin330;
   z2 = x*sin330 + z*cos330;
 
   */
 
   x2 = x*cos150 + z*-sin150;
   z2 = x*sin150 + z*cos150;
      y2 = y;
 
    Serial.print("X2(");
    Serial.print(x2,DEC);
    Serial.print(")  Z2(");
    Serial.print(z2,DEC);
    Serial.print(")  Y2(");
    Serial.print(y2,DEC);
    Serial.println(")");
 
    LegIK(x2,y2,z2,&CAngle,&FAngle,&TAngle);
 
          //CAngle = constrain(CAngle, -PIby2, PIby2);
          legServo[0].writeMicroseconds(RadToMicro(CAngle + PIby2));
          //FAngle = constrain(FAngle, -PIby2, PIby2);
          legServo[1].writeMicroseconds(RadToMicro(FAngle + PIby2));
          //TAngle = constrain(TAngle, -PIby2, PIby2);
          legServo[2].writeMicroseconds(RadToMicro(TAngle + PIby2));
         
          delay(100);
   
  }
         
 
}




void LegIK (float FeetPosX, float FeetPosY, float FeetPosZ, float *OUT_IKCoxaAngle, float *OUT_IKFemurAngle, float *OUT_IKTibiaAngle){

//code has been removed by author

}

boolean ReceiveCommand(){

if (Serial.available() > 0) {

// determine
if (index >= MAX_COMMAND_LENGTH) return false;

// Receiving
command[index] = Serial.read();;
if (command[index] == '/')  return true;
index++;

}

// no signal available
return false;

}


int RadToMicro(float rad){

// contrain: 0<rad<PI
if (rad<0) rad = -rad;
while (rad>PI) rad -= PI;

//Deg/180*1800+600
return rad / PI *1800 + 600;
}

float oz_atan2(float a, float b){
// atan(a/b)

// reflected about origin (angle can only be -90 to 90)

if (a==0){
if (b>=0) return 0;
else return 0; // PI - PI
}
else if (a>0){
if (b==0) return PIby2;
else return atan(a/b);
}
else { // a<0
if (b==0) return -PIby2;
else return atan(a/b);
}

}

int Char2Int(char chr) {
if ((chr < '0') || (chr > '9'))
return -1;
else
return chr - '0';
}