Along 3 years I have been trying several leg mechanism, at first I decided to do a simple desing with tibial motor where placed on femur joint.

This design had several problems, like it wasn't very robust and the most importat is that having the motor (with big mass) that far from the rotating axis, caused that in some movements it generate unwanted dynamics to the robot body, making controlability worse.

New version have both motors of femur/tibial limb at coxa frame, this ends with a very simple setup and at the same time, the heaviest masses of the mechanism are centered to the rotating axis of coxa limb, so even though the leg do fast movements, inertias won't be strong enough to affect the hole robot mass, achieving more agility.

Inverse Kinematics of the mechanism

After building it I notice that this mechanism was very special for another reason, at the domain the leg normally moves, it acts as a diferential mecanism, this means that torque is almost all the time shared between both motor of the longer limbs. That was an improvent since with the old mechanism tibial motor had to hold most of the weight and it was more forced than the one for femur.

To visualize this, for the same movement, we can see how tibial motor must travel more arc of angel that the one on the new version.

In order to solve this mechanism, just some trigonometry is needed. Combining both cosine and sine laws, we can obtain desired angle (the one between femur and tibia) with respect to the angle the motor must achieve.

Observing these equations, with can notice that this angle (the one between femur and tibia) depends on both servos angles, which means both motors are contributing to the movement of the tibia.

Calibration of servos

Another useful thing to do if we want to control servo precisely is to print a calibration tool for our set up. As shown in the image below, in order to know where real angles are located, angle protactor is placer just in the origin of the rotating joint, and choosing 2 know angles we can match PWM signal to the real angles we want to manipulate simply doing a lineal relation between angles and PWM pulse length.

Then a simple program in the serial console can be wrtten to let the user move the motor to the desired angle. This way the calibration process is only about placing motor at certain position and everything is done and we won't need to manually introduce random values that can be a very tedious task.

With this I have achieved very good calibrations on motors, which cause the robot to be very simetrial making the hole system more predictable. Also the calibration procedure now is very easy to do, as all calculations are done automatically. Check Section 1 for the example code for calibration.

More about this can be seen in the video below, where all the building process is shown as well as the new leg in action.

SECTION 1:

In the example code below, you can see how calibration protocol works, it is just a function called calibrationSecuence() which do all the work until calibration is finished. So you only need to call it one time to enter calibration loop, for example by sending a 'c' character thought the serial console.

Also some useful function are used, like moving motor directly with analogWrite functions which all the calculations involved, this is a good point since no interrupts are used.

This code also have the feature to calibrate the potentiometer coming from each motor.

#define MAX_PULSE 2500 #define MIN_PULSE 560 /*---------------SERVO PIN DEFINITION------------------------*/ int m1 = 6;//FR int m2 = 5; int m3 = 4; int m4 = 28;//FL int m5 = 29; int m6 = 36; int m7 = 3;//BR int m8 = 2; int m9 = 1; int m10 = 7;//BL int m11 = 24; int m12 = 25; int m13 = 0;//BODY /*----------------- CALIBRATION PARAMETERS OF EACH SERVO -----------------*/ double lowLim[13] = {50, 30, 30, 50, 30, 30, 50, 30, 30, 50, 30, 30, 70}; double highLim[13] = {130, 150, 150, 130, 150, 150, 130, 150, 150, 130, 150, 150, 110}; double a[13] = { -1.08333, -1.06667, -1.07778, //FR                -1.03333, 0.97778, 1.01111, //FL                1.03333, 1.05556, 1.07778,  //BR                1.07500, -1.07778, -1.00000, //BL                1.06250              }; double b[13] = {179.0, 192.0, 194.5, //FR               193.0,  5.5,  -7.5,  //FL               7.0, -17.0, -16.0,    //BR               -13.5, 191.5, 157.0, //BL               -0.875              }; double ae[13] = {0.20292, 0.20317, 0.19904 ,                0.21256, -0.22492, -0.21321,                -0.21047, -0.20355, -0.20095,                -0.20265, 0.19904, 0.20337,                -0.20226               }; double be[13] = { -18.59717, -5.70512, -2.51697,                 -5.75856, 197.29411, 202.72169,                 185.96931, 204.11902, 199.38663,                 197.89534, -5.33768, -32.23424,                 187.48058               }; /*--------Corresponding angles you want to meassure at in your system-----------*/ double x1[13] = {120, 135, 90, 60, 135 , 90, 120, 135, 90, 60, 135, 90, 110}; //this will be the first angle you will meassure double x2[13] = {60,  90, 135, 120, 90, 135, 60, 90, 135, 120, 90, 135, 70};//this will be the second angle you will meassure for calibration /*--------You can define a motor tag for each servo--------*/ String motorTag[13] = {"FR coxa", "FR femur", "FR tibia",                      "FL coxa", "FL femur", "FL tibia",                      "BR coxa", "BR femur", "BR tibia",                      "BL coxa", "BL femur", "BL tibia", "Body angle"                     }; double ang1[13] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; double ang2[13] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; float xi[500]; float yi[500]; float fineAngle; float fineL; float fineH; int motorPin; int motor = 0; float calibrationAngle; float res = 1.0; float ares = 0.5; float bres = 1.0; float cres = 4.0; float rawAngle; float orawAngle; char cm; char answer; bool interp = false; bool question = true; bool swing = false; int i; double eang; int freq = 100; // PWM frecuency can be choosen here. void connectServos() { analogWriteFrequency(m1, freq); //FR coxa digitalWrite(m1, LOW); pinMode(m1, OUTPUT); analogWriteFrequency(m2, freq); //femur digitalWrite(m2, LOW); pinMode(m2, OUTPUT); analogWriteFrequency(m3, freq); //tibia digitalWrite(m3, LOW); pinMode(m3, OUTPUT); analogWriteFrequency(m4, freq); //FL coxa digitalWrite(m4, LOW); pinMode(m4, OUTPUT); analogWriteFrequency(m5, freq); //femur digitalWrite(m5, LOW); pinMode(m5, OUTPUT); analogWriteFrequency(m6, freq); //tibia digitalWrite(m6, LOW); pinMode(m6, OUTPUT); analogWriteFrequency(m7, freq); //FR coxa digitalWrite(m7, LOW); pinMode(m7, OUTPUT); analogWriteFrequency(m8, freq); //femur digitalWrite(m8, LOW); pinMode(m8, OUTPUT); analogWriteFrequency(m9, freq); //tibia digitalWrite(m9, LOW); pinMode(m9, OUTPUT); analogWriteFrequency(m10, freq); //FR coxa digitalWrite(m10, LOW); pinMode(m10, OUTPUT); analogWriteFrequency(m11, freq); //femur digitalWrite(m11, LOW); pinMode(m11, OUTPUT); analogWriteFrequency(m12, freq); //tibia digitalWrite(m12, LOW); pinMode(m12, OUTPUT); analogWriteFrequency(m13, freq); //body digitalWrite(m13, LOW); pinMode(m13, OUTPUT); } void servoWrite(int pin , double angle) { float T = 1000000.0f / freq; float usec = float(MAX_PULSE - MIN_PULSE) * (angle / 180.0) + (float)MIN_PULSE; uint32_t duty = int(usec / T * 4096.0f); analogWrite(pin , duty); } double checkLimits(double angle , double lowLim , double highLim) { if ( angle >= highLim ) {   angle = highLim; } if ( angle <= lowLim ) {   angle = lowLim; } return angle; } int motorInfo(int i) { enc1 , enc2 , enc3 , enc4 , enc5 , enc6 , enc7 , enc8 , enc9 , enc10 , enc11 , enc12 , enc13 = readEncoders(); if (i == 0) {   rawAngle = enc1;   motorPin = m1; } else if (i == 1) {   rawAngle = enc2;   motorPin = m2; } else if (i == 2) {   rawAngle = enc3;   motorPin = m3; } else if (i == 3) {   rawAngle = enc4;   motorPin = m4; } else if (i == 4) {   rawAngle = enc5;   motorPin = m5; } else if (i == 5) {   rawAngle = enc6;   motorPin = m6; } else if (i == 6) {   rawAngle = enc7;   motorPin = m7; } else if (i == 7) {   rawAngle = enc8;   motorPin = m8; } else if (i == 8) {   rawAngle = enc9;   motorPin = m9; } else if (i == 9) {   rawAngle = enc10;   motorPin = m10; } else if (i == 10) {   rawAngle = enc11;   motorPin = m11; } else if (i == 11) {   rawAngle = enc12;   motorPin = m12; } else if (i == 12) {   rawAngle = enc13;   motorPin = m13; } return rawAngle , motorPin; } void moveServos(double angleBody , struct vector anglesServoFR , struct vector anglesServoFL , struct vector anglesServoBR , struct vector anglesServoBL) { //FR anglesServoFR.tetta = checkLimits(anglesServoFR.tetta , lowLim[0] , highLim[0]); fineAngle = a[0] * anglesServoFR.tetta + b[0]; servoWrite(m1 , fineAngle); anglesServoFR.alpha = checkLimits(anglesServoFR.alpha , lowLim[1] , highLim[1]); fineAngle = a[1] * anglesServoFR.alpha + b[1]; servoWrite(m2 , fineAngle); anglesServoFR.gamma = checkLimits(anglesServoFR.gamma , lowLim[2] , highLim[2]); fineAngle = a[2] * anglesServoFR.gamma + b[2]; servoWrite(m3 , fineAngle); //FL anglesServoFL.tetta = checkLimits(anglesServoFL.tetta , lowLim[3] , highLim[3]); fineAngle = a[3] * anglesServoFL.tetta + b[3]; servoWrite(m4 , fineAngle); anglesServoFL.alpha = checkLimits(anglesServoFL.alpha , lowLim[4] , highLim[4]); fineAngle = a[4] * anglesServoFL.alpha + b[4]; servoWrite(m5 , fineAngle); anglesServoFL.gamma = checkLimits(anglesServoFL.gamma , lowLim[5] , highLim[5]); fineAngle = a[5] * anglesServoFL.gamma + b[5]; servoWrite(m6 , fineAngle); //BR anglesServoBR.tetta = checkLimits(anglesServoBR.tetta , lowLim[6] , highLim[6]); fineAngle = a[6] * anglesServoBR.tetta + b[6]; servoWrite(m7 , fineAngle); anglesServoBR.alpha = checkLimits(anglesServoBR.alpha , lowLim[7] , highLim[7]); fineAngle = a[7] * anglesServoBR.alpha + b[7]; servoWrite(m8 , fineAngle); anglesServoBR.gamma = checkLimits(anglesServoBR.gamma , lowLim[8] , highLim[8]); fineAngle = a[8] * anglesServoBR.gamma + b[8]; servoWrite(m9 , fineAngle); //BL anglesServoBL.tetta = checkLimits(anglesServoBL.tetta , lowLim[9] , highLim[9]); fineAngle = a[9] * anglesServoBL.tetta + b[9]; servoWrite(m10 , fineAngle); anglesServoBL.alpha = checkLimits(anglesServoBL.alpha , lowLim[10] , highLim[10]); fineAngle = a[10] * anglesServoBL.alpha + b[10]; servoWrite(m11 , fineAngle); anglesServoBL.gamma = checkLimits(anglesServoBL.gamma , lowLim[11] , highLim[11]); fineAngle = a[11] * anglesServoBL.gamma + b[11]; servoWrite(m12 , fineAngle); //BODY angleBody = checkLimits(angleBody , lowLim[12] , highLim[12]); fineAngle = a[12] * angleBody + b[12]; servoWrite(m13 , fineAngle); } double readEncoderAngles() { enc1 , enc2 , enc3 , enc4 , enc5 , enc6 , enc7 , enc8 , enc9 , enc10 , enc11 , enc12 , enc13 = readEncoders(); eang1 = ae[0] * enc1 + be[0]; eang2 = ae[1] * enc2 + be[1]; eang3 = ae[2] * enc3 + be[2]; eang4 = ae[3] * enc4 + be[3]; eang5 = ae[4] * enc5 + be[4]; eang6 = ae[5] * enc6 + be[5]; eang7 = ae[6] * enc7 + be[6]; eang8 = ae[7] * enc8 + be[7]; eang9 = ae[8] * enc9 + be[8]; eang10 = ae[9] * enc10 + be[9]; eang11 = ae[10] * enc11 + be[10]; eang12 = ae[11] * enc12 + be[11]; eang13 = ae[12] * enc13 + be[12]; return eang1 , eang2 , eang3 , eang4 , eang5 , eang6 , eang7 , eang8 , eang9 , eang10 , eang11 , eang12 , eang13; } void calibrationSecuence( ) { //set servos at their middle position at firstt for (int i = 0; i <= 12; i++) {   rawAngle , motorPin = motorInfo(i);   servoWrite(motorPin , 90); } //   sensorOffset0 = calibrateContacts(); Serial.println(" "); Serial.println("_________________________________SERVO CALIBRATION ROUTINE_________________________________"); Serial.println("___________________________________________________________________________________________"); Serial.println("(*) Don't send several caracter at the same time."); delay(500); Serial.println(" "); Serial.println("Keyboard: 'x'-> EXIT CALIBRATION.  'c'-> ENTER CALIBRATION."); Serial.println("         'i'-> PRINT INFORMATION.  "); Serial.println(" "); Serial.println("         'n'-> CHANGE MOTOR (+).  'b' -> CHANGE MOTOR (-)."); Serial.println("         'm'-> START CALIBRATION."); Serial.println("         'q'-> STOP CALIBRATION."); Serial.println(" "); Serial.println("         'r'-> CHANGE RESOLUTION."); Serial.println("         'p'-> ADD ANGLE.         'o'-> SUBTRACT ANGLE.  "); Serial.println("         's'-> SAVE ANGLE."); delay(500); Serial.println(" "); Serial.println("---------------------------------------------------------------------------------------------------"); Serial.print("SELECTED MOTOR: "); Serial.print(motorTag[motor]); Serial.print(".    SELECTED RESOLUTION: "); Serial.println(res); while (CAL == true) {   if (Serial.available() > 0) {     cm = Serial.read();     if (cm == 'x') {       Serial.println("Closing CALIBRATION program...");       CAL = false;       secuence = false;       startDisplay(PAGE);       angleBody = 90;       anglesIKFR.tetta = 0.0;       anglesIKFR.alpha = -45.0;       anglesIKFR.gamma = 90.0;       anglesIKFL.tetta = 0.0;       anglesIKFL.alpha = -45.0;       anglesIKFL.gamma = 90.0;       anglesIKBR.tetta = 0.0;       anglesIKBR.alpha = 45.0;       anglesIKBR.gamma = -90.0;       anglesIKBL.tetta = 0.0;       anglesIKBL.alpha = 45.0;       anglesIKBL.gamma = -90.0;     }     else if (cm == 'i') { // +       Serial.println(" ");       Serial.println("---------------------------------------------------------------------------------------------------");       Serial.println("---------------------------------------------------------------------------------------------------");       Serial.println("(*) Don't send several caracter at the same time.");       delay(500);       Serial.println(" ");       Serial.println("Keyboard: 'x'-> EXIT CALIBRATION.  'c'-> ENTER CALIBRATION.");       Serial.println("         'i'-> PRINT INFORMATION.  ");       Serial.println(" ");       Serial.println("         'n'-> CHANGE MOTOR (+).  'b' -> CHANGE MOTOR (-).");       Serial.println("         'm'-> START CALIBRATION.");       Serial.println("         'q'-> STOP CALIBRATION.");       Serial.println(" ");       Serial.println("         'r'-> CHANGE RESOLUTION.");       Serial.println("         'p'-> ADD ANGLE.         'o'-> SUBTRACT ANGLE.      's'-> SAVE ANGLE.");       Serial.println(" ");       delay(500);       Serial.println(" ");       Serial.println("---------------------------------------------------------------------------------------------------");       Serial.println(" ");       Serial.print("SELECTED MOTOR: ");       Serial.print(motorTag[motor]);       Serial.print(".    SELECTED RESOLUTION: ");       Serial.println(res);       Serial.println("Actual parameters of the motor: ");       Serial.print("High limit: ");       Serial.print(highLim[motor]);       Serial.print("  Low limit: ");       Serial.print(lowLim[motor]);       Serial.print("  Angle 1: ");       Serial.print(ang1[motor]);       Serial.print("  Angle 2: ");       Serial.println(ang2[motor]);       Serial.println("---------------------------------------------------------------------------------------------------");     }     else if (cm == 'm') { // +       secuence = true;     }     else if (cm == 's') { // +     }     else if (cm == 'n') { // +       motor++;       if (motor >= 13) {         motor = 0;       }       Serial.print("SELECTED MOTOR: ");       Serial.println(motorTag[motor]);     }     else if (cm == 'b') { // +       motor--;       if (motor < 0) {         motor = 13 - 1;       }       Serial.print("SELECTED MOTOR: ");       Serial.println(motorTag[motor]);     }     else if (cm == 'r') { // +       if (res == ares) {         res = bres;       }       else if (res == bres) {         res = cres;       }       else if (res == cres) {         res = ares;       }       Serial.print("SELECTED RESOLUTION: ");       Serial.println(res);     }   }      if (secuence == true) {     Serial.print("Starting secuence for motor: ");     Serial.println(motorTag[motor]);     for (int i = 0; i <= 30; i++) {       delay(20);       Serial.print(".");     }     Serial.println(".");     while (question == true) {       unsigned long currentMicros = micros();       if (currentMicros - previousMicros >= 100000) {         previousMicros = currentMicros;         if (Serial.available() > 0) {           answer = Serial.read();           if (answer == 'y') {             question = false;             interp = true;             secuence = true;           }           else if (answer == 'n') {             question = false;             interp = false;             secuence = true;           }           else {             Serial.println("Please, select Yes(y) or No(n).");           }         }       }     }     answer = 't';     question = true;     if (interp == false) {       Serial.println("___");       Serial.println("  | Place motor at 1ts position and save angle");       Serial.println("  | This position can be the higher one");       rawAngle , motorPin = motorInfo(motor);       calibrationAngle = 90; //start calibration at aproximate middle position of the servo.       while (secuence == true) { /*  find first calibration angle   */         if (Serial.available() > 0) {           cm = Serial.read();           if (cm == 'p') { // +             Serial.print("  | +");             Serial.print(res);             Serial.print(" : ");             calibrationAngle = calibrationAngle + res;             servoWrite(motorPin , calibrationAngle);             Serial.println(calibrationAngle);           }           else if (cm == 'o') { // -             Serial.print("  | -");             Serial.print(res);             Serial.print(" : ");             calibrationAngle = calibrationAngle - res;             servoWrite(motorPin , calibrationAngle);             Serial.println(calibrationAngle);           }           else if (cm == 'r') { // +             if (res == ares) {               res = bres;             }             else if (res == bres) {               res = cres;             }             else if (res == cres) {               res = ares;             }             Serial.print("SELECTED RESOLUTION: ");             Serial.println(res);           }           else if (cm == 'q') { //   quit secuence             secuence = false;             Serial.println("  | Calibration interrupted!!");           }           else if (cm == 's') { //   save angle             ang1[motor] = calibrationAngle;             secuence = false;             Serial.print("  | Angle saved at ");             Serial.println(calibrationAngle);           }         }       }       if (cm == 'q') {         Serial.println("  |");       }       else {         secuence = true;         Serial.println("___");         Serial.println("  | Place motor at 2nd position and save angle");         Serial.println("  | This position can be the lower one");       }       while (secuence == true) {    /*  find second calibration angle   */         if (Serial.available() > 0) {           cm = Serial.read();           if (cm == 'p') { // +             Serial.print("  | +");             Serial.print(res);             Serial.print(" : ");             calibrationAngle = calibrationAngle + res;             servoWrite(motorPin , calibrationAngle);             Serial.println(calibrationAngle);           }           else if (cm == 'o') { // -             Serial.print("  | -");             Serial.print(res);             Serial.print(" : ");             calibrationAngle = calibrationAngle - res;             servoWrite(motorPin , calibrationAngle);             Serial.println(calibrationAngle);           }           else if (cm == 'r') { // +             if (res == ares) {               res = bres;             }             else if (res == bres) {               res = cres;             }             else if (res == cres) {               res = ares;             }             Serial.print("SELECTED RESOLUTION: ");             Serial.println(res);           }           else if (cm == 'q') { //   quit secuence             secuence = false;             Serial.println("  | Calibration interrupted!!");           }           else if (cm == 's') { //    save angle             ang2[motor] = calibrationAngle;             secuence = false;             Serial.print("  | Angle saved at ");             Serial.println(calibrationAngle);           }         }       }       /*--------------------start calibration calculations------------------*/       if (cm == 'q') {         Serial.println("___|");         Serial.println("Calibration finished unespected.");         Serial.println(" Select another motor.");         Serial.print("SELECTED MOTOR: ");         Serial.print(motorTag[motor]);         Serial.print(".    SELECTED RESOLUTION: ");         Serial.println(res);       }       else {         Serial.println("___");         Serial.println("  |___");         Serial.print( "  |  | Interpolating for motor: ");         Serial.println(motorTag[motor]);         secuence = true;         //real angle is calculated interpolating both angles to a linear relation.         a[motor] = (ang2[motor] - ang1[motor]) / (x2[motor] - x1[motor]);         b[motor] = ang1[motor] - x1[motor] * (ang2[motor] - ang1[motor]) / (x2[motor] - x1[motor]);         Serial.println("  |  |");       }       interp = true;     }     /*---------------------------make swing movement to interpolate motor encoder-----*/     if (interp == true and secuence == true) {       delay(200);       double x;       int k = 0;              int stp = 180;              swing = true;       i = 0;       orawAngle , motorPin = motorInfo(motor);       previousMicros = 0;              while (swing == true) { // FIRST         unsigned long currentMicros = micros();         if (currentMicros - previousMicros >= 10000) {           // save the last time you blinked the LED           previousMicros = currentMicros;                      x = x2[motor];           calibrationAngle = a[motor] * x + b[motor];           servoWrite(motorPin , calibrationAngle);           rawAngle , motorPin = motorInfo(motor);           if ((i % 3) == 0) {             yi[k+1] = x;             xi[k] = rawAngle;             Serial.print("  |  | Real ang: ");             Serial.print(x);             Serial.print(" -> Servo ang: ");             Serial.print(calibrationAngle);             Serial.print(" Enc: ");             Serial.println(rawAngle);             k++;           }           if (i >= stp) {             swing = false;           }           i++;         }       }       swing = true;       i = 0;       while (swing == true) { // moving         unsigned long currentMicros = micros();         if (currentMicros - previousMicros >= 10000) {           // save the last time you blinked the LED           previousMicros = currentMicros;           x = x2[motor] + float(i) * (x1[motor] - x2[motor]) / stp;           calibrationAngle = a[motor] * x + b[motor];           servoWrite(motorPin , calibrationAngle);           rawAngle , motorPin = motorInfo(motor);           if ((i % 6) == 0) {             yi[k+1] = x;             xi[k] = rawAngle;             Serial.print("  |  | Real ang: ");             Serial.print(x);             Serial.print(" -> Servo ang: ");             Serial.print(calibrationAngle);             Serial.print(" Enc: ");             Serial.println(rawAngle);             k++;           }           if (i >= stp) {             swing = false;           }           i++;         }       }       swing = true;       i = 0;       while (swing == true) { // SECOND         unsigned long currentMicros = micros();         if (currentMicros - previousMicros >= 10000) {           // save the last time you blinked the LED           previousMicros = currentMicros;           x = x1[motor];           calibrationAngle = a[motor] * x + b[motor];           servoWrite(motorPin , calibrationAngle);           rawAngle , motorPin = motorInfo(motor);           if ((i % 3) == 0) {             yi[k+1] = x;             xi[k] = rawAngle;             Serial.print("  |  | Real ang: ");             Serial.print(x);             Serial.print(" -> Servo ang: ");             Serial.print(calibrationAngle);             Serial.print(" Enc: ");             Serial.println(rawAngle);             k++;           }           if (i >= stp) {             swing = false;           }           i++;         }       }       swing = true;       i = 0;       while (swing == true) { // moving         unsigned long currentMicros = micros();         if (currentMicros - previousMicros >= 10000) {           // save the last time you blinked the LED           previousMicros = currentMicros;           x = x1[motor] + float(i) * (x2[motor] - x1[motor]) / stp;           calibrationAngle = a[motor] * x + b[motor];           servoWrite(motorPin , calibrationAngle);           rawAngle , motorPin = motorInfo(motor);           if ((i % 6) == 0) {             yi[k+1] = x;             xi[k] = rawAngle;             Serial.print("  |  | Real ang: ");             Serial.print(x);             Serial.print(" -> Servo ang: ");             Serial.print(calibrationAngle);             Serial.print(" Enc: ");             Serial.println(rawAngle);             k++;           }           if (i >= stp) {             swing = false;           }           i++;         }       }       swing = true;       i = 0;       while (swing == true) { // FIRST         unsigned long currentMicros = micros();         if (currentMicros - previousMicros >= 10000) {           // save the last time you blinked the LED           previousMicros = currentMicros;           x = x2[motor];           calibrationAngle = a[motor] * x + b[motor];           servoWrite(motorPin , calibrationAngle);           rawAngle , motorPin = motorInfo(motor);           if ((i % 3) == 0) {             yi[k+1] = x;             xi[k] = rawAngle;             Serial.print("  |  | Real ang: ");             Serial.print(x);             Serial.print(" -> Servo ang: ");             Serial.print(calibrationAngle);             Serial.print(" Enc: ");             Serial.println(rawAngle);             k++;           }           if (i >= stp) {             swing = false;           }           i++;         }       }              swing = true;       i = 0;       while (swing == true) { // moving         unsigned long currentMicros = micros();         if (currentMicros - previousMicros >= 10000) {           // save the last time you blinked the LED           previousMicros = currentMicros;           x = x2[motor] + float(i) * (x1[motor] - x2[motor]) / stp;           calibrationAngle = a[motor] * x + b[motor];           servoWrite(motorPin , calibrationAngle);           rawAngle , motorPin = motorInfo(motor);           if ((i % 6) == 0) {             yi[k+1] = x;             xi[k] = rawAngle;             Serial.print("  |  | Real ang: ");             Serial.print(x);             Serial.print(" -> Servo ang: ");             Serial.print(calibrationAngle);             Serial.print(" Enc: ");             Serial.println(rawAngle);             k++;           }           if (i >= stp) {             swing = false;           }           i++;         }       }       swing = true;       i = 0;       while (swing == true) { // SECOND         unsigned long currentMicros = micros();         if (currentMicros - previousMicros >= 10000) {           // save the last time you blinked the LED           previousMicros = currentMicros;           x = x1[motor];           calibrationAngle = a[motor] * x + b[motor];           servoWrite(motorPin , calibrationAngle);           rawAngle , motorPin = motorInfo(motor);           if ((i % 3) == 0) {             yi[k+1] = x;             xi[k] = rawAngle;             Serial.print("  |  | Real ang: ");             Serial.print(x);             Serial.print(" -> Servo ang: ");             Serial.print(calibrationAngle);             Serial.print(" Enc: ");             Serial.println(rawAngle);             k++;           }           if (i >= stp) {             swing = false;           }           i++;         }       }             Serial.println("  |  | Interpolation finished!");       /*-------Calculate linear interpolation of the encoder from 60 meassures done in swing------*/       double sx = 0;       double sy = 0;       double sx2 = 0;       double sy2 = 0;       double sxy = 0;       double xmean = 0;       double ymean = 0;       int n = 300;       for (int i = 0 ; i < n ; i++) {         sx += xi[i+10];         sy += yi[i+10];         sx2 += xi[i+10] * xi[i+10];         sy2 += yi[i+10] * yi[i+10];         sxy += xi[i+10] * yi[i+10];       }       ae[motor] = (n * sxy - sx * sy) / (n * sx2 - sx * sx); //sxy / sx2; //       be[motor] = (sy - ae[motor] * sx) / n; //ymean - ae[motor] * xmean;       Serial.println("  |  | Moving back to ZERO position."); // turn the motor back to middle position       swing = true;       i = 0;       while (swing == true) {         unsigned long currentMicros = micros();         if (currentMicros - previousMicros >= 10000) {           // save the last time you blinked the LED           previousMicros = currentMicros;           x = x1[motor] + float(i) * (90 - x1[motor]) / 60;           calibrationAngle = a[motor] * x + b[motor];           servoWrite(motorPin , calibrationAngle);           rawAngle , motorPin = motorInfo(motor);           eang = ae[motor] * rawAngle + be[motor];           if ((i % 4) == 0) {             Serial.print("  |  | Servo ang: ");             Serial.print(calibrationAngle);             Serial.print(" -> Real ang: ");             Serial.print(x);             Serial.print(" -> Encoder ang: ");             Serial.println(eang);           }           if (i >= 60) {             swing = false;           }           i++;         }       }       Serial.println("___|___|");       Serial.println("  | ");       Serial.println("___");       Serial.println("  | Calibration finished satisfactory. Results data:");       Serial.print("  | HIGH lim: "); Serial.print(highLim[motor]);       Serial.print("  LOW lim: "); Serial.println(lowLim[motor]);       Serial.print("  | angle 1: "); Serial.print(ang1[motor]);       Serial.print("  angle 2 "); Serial.println(ang2[motor]);       Serial.print("  | Regression Motor a: "); Serial.print(a[motor], 5);       Serial.print("                 b: "); Serial.println(b[motor], 5);       Serial.print("  | Regression Encoder a: "); Serial.print(ae[motor], 5);       Serial.print("                   b: "); Serial.println(be[motor], 5);       Serial.println("  |");       Serial.println("  | ______________________________________________________________");       Serial.println("  | |                                                            |");       Serial.println("  | |  This code won't be able to save the updated parameters   |");       Serial.println("  | |  once the robot is shutted down.                          |");       Serial.println("  | |                                                            |");       Serial.println("  | |  Please, write down the results                           |");       Serial.println("  | |  and save them in the definition of each variable.        |");       Serial.println("  | |_____________________________________________________________|");       Serial.println("  |");       Serial.println("___|");       Serial.println(" Select another motor.");       Serial.print("SELECTED MOTOR: ");       Serial.print(motorTag[motor]);       Serial.print(".    SELECTED RESOLUTION: ");       Serial.println(res);     }     interp = false;     secuence = false;   } } SAFE = false; Serial.println("Calibration killed"); } // END OF CALIBRATION