IK - Chapter II

Using the equations described in first chapter I want to describe now how to make the robot body move up and down as the video below:

To achieve this, the end position of the leg in the X axis has to remain constant and it's the Z position at the coxa that has to move up and down (the body it's attached to the coxa).

As you can see in the image below (made using CAD software) I show 2 configurations for 1 leg. the X dimension is constant at 80 mm and the Z dimension moves from 35 mm to 60 mm. I deleted the lengths for femur and tibia to make drawing less confusing. The lengths are 70 mm and 111 mm respectively.

As you can see in the same image above, the angles for the femur and tibia are different in the two configurations. These angles are the ones we need to find to make the robot to move up and down from a height of 35 mm to a height of 60 mm.

We can then apply these angles to the servos on all legs to achieve the movement shown in the video.

Let's see the code:

float XPosition[2] = {80, 80};                        //place x positions
float ZPosition[2] = {35,60};                         //place z positions
float X0;                                                    //used to enter inside function calcul angles
float Z0;                                                    //used to enter inside function calcul angles
int pulsewidth1;
int pulsewidth2;

void setup() {
Serial.begin (115200);

Serial.println ("#0 P680 #3 P2013 #4 P1500 #11 P1500 #12 P2013 #15 P680 T800" );      //Set servos to starting position
Serial.println ("#16 P680 #19 P2013 #20 P1500 #27 P1500 #28 P2013 #31 P680 T800");

delay (2000);

void loop() {

  for (int i = 0; i <= 1; i++){
    X0 = XPosition[i];
    Z0 = ZPosition[i];
    calculAngles (X0, Z0);                                     // Calls function Calcul angles
    moveservo (0, pulsewidth1, 800);                     // Calls function to move the servo
    moveservo (15, pulsewidth1, 800);
    moveservo (16, pulsewidth1, 800);
    moveservo (31, pulsewidth1, 800);
    moveservo (3, pulsewidth2, 800);
    moveservo (12, pulsewidth2, 800);
    moveservo (19, pulsewidth2, 800);
    moveservo (28, pulsewidth2, 800);
    delay (800);

void calculAngles(float X0, float Y0) {
  const float a = 70;                                             //this value is known (femur)
  const float b = 111;                                           //this value is known (tibia)
  float hypo;
  float alpha;
  const float pi=3.141592653;
  float beta;
  float gamma;
  float delta;
  float epsilon;
  float roundBeta; //to round value before convert from float to int
  float roundEpsilon;
  hypo = sqrt((sq(X0) + sq(Z0)));
  alpha = (acos((sq(a)+sq(hypo)-sq(b))/(2*a*hypo)))*180/pi;                // cosines law in degrees
  gamma = (atan2(Z0,X0))*180/pi; 
  beta = 90 - alpha + gamma;
  Serial.print ("Beta = ");                                                              // prints this text in the serial monitor
  Serial.println (beta);                                                                  // prints beta value in serial monitor
  delta = (acos((sq(a)+sq(b)-sq(hypo))/(2*a*b)))*180/pi;
  epsilon = 180 - delta;
  Serial.print ("Epsilon = ");
  roundBeta = lround((beta*11)+560);                                             // convert angle to pulsewidth and rounds it
  roundEpsilon = lround ((epsilon*11)+560);

  pulsewidth1 = (int)roundBeta;
  pulsewidth2 = (int)roundEpsilon;
void moveservo (int servo, int positions, int time) {

  Serial.print ("#");
  Serial.print (servo);
  Serial.print (" P");
  Serial.print (positions);
  Serial.print (" T");
  Serial.println (time);

See below a screen shot of the Serial monitor.

As you can see the angle values are the same than the ones on first picture made with a CAD software.

The code above calculates the angles over and over. I will probably follow a different approach in my coding when more calculations are needed.

Every servo is slightly different in terms of the mechanics (gears) so it will be better if we transform the angle to a pulsewidth as individual cases. This means that lines like:

roundBeta = lround((beta*11)+560); 

will be different for every servo. I will explain this deeper in a future post.


  1. really your information is very useful buddy!! i have one doubt!!! y cant we write servo angles using servo.Write(desired angle) ???? y we have to complicate the program with pwm values?? y cant direct angles!!??

    1. Hi, I'm not sure if I understood your question but you can easily move a servo with the command servo.write (angle). Just remember to include the library and define your servo an attach it to a digital pin. In my case I chose to use a serial card because it makes the physical connections easy and I can move lots of servos at different speeds and move them at same time.

  2. sir i still cant get it how come the Round beta and epsilon works, and from where that equation come from , sir? could you help us to explain that??