//Shoulder -> forearm -> hand
int a = 58;//mm length from servo 2-3
int c = 110;//mm length from servo 3- End Effector
int i=0;
unsigned long time;

void echo(int echo){
  Serial.println(echo);
}
void moveArm(int legNum, int x, int y,int z){
    //Where should the legs be      
    double g;
      double f;
      double b;
      int d;
      int e;
    double truex;
    int h;
    truex = sqrt(sq(x)+sq(z));
    b = sqrt(sq(truex)+sq(y));    
      f = atan2(y, x) * 180 / PI;
     g = acos((sq(a)-sq(c)+sq(b)) / (2 * a * b)) * 180 / PI;
     d = g + f;    //femur
        e =