arduino mega 2560 3.2 tft lcd si5351 free sample

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 mechanismAfter 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 servosAnother 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

Very nice, JuanCarlos! You must have a ton of work in fabricating your PCB. I"m making a similar TFT display using the Mega Pro Mini (see board) to keep the footprint smaller, but using an encoder for the menuing system instead of push buttons. I"m also trying to bring out all of the I/O pins to make hacking it a bit easier.

This library enables you to use ISR-based PWM channels on AVR ATmega164, ATmega324, ATmega644, ATmega1284 with MCUdude MightyCore, to create and output PWM any GPIO pin

This library enables you to use Hardware-based PWM channels on Arduino AVR ATtiny-based boards (ATtiny3217, etc.), using megaTinyCore, to create and output PWM to pins.

This library enables you to use ISR-based PWM channels on Arduino AVR ATtiny-based boards (ATtiny3217, etc.), using megaTinyCore, to create and output PWM any GPIO pin.

Small low-level classes and functions for Arduino: incrementMod(), decToBcd(). strcmp_PP(), PrintStr, PrintStrN, printPad{N}To(), printIntAsFloat(), TimingStats, formUrlEncode(), FCString, KString, hashDjb2(), binarySearch(), linearSearch(), isSorted(), reverse(), and so on.

Cyclic Redundancy Check (CRC) algorithms (crc8, crc16ccitt, crc32) programmatically converted from C99 code generated by pycrc (https://pycrc.org) to Arduino C++ using namespaces and PROGMEM flash memory.

Various sorting algorithms for Arduino, including Bubble Sort, Insertion Sort, Selection Sort, Shell Sort (3 versions), Comb Sort (4 versions), Quick Sort (3 versions).

Date, time, timezone classes for Arduino supporting the full IANA TZ Database to convert epoch seconds to date and time components in different time zones.

Clock classes for Arduino that provides an auto-incrementing count of seconds since a known epoch which can be synchronized from external sources such as an NTP server, a DS3231 RTC chip, or an STM32 RTC chip.

Useful Arduino utilities which are too small as separate libraries, but complex enough to be shared among multiple projects, and often have external dependencies to other libraries.

Fast and compact software I2C implementations (SimpleWireInterface, SimpleWireFastInterface) on Arduino platforms. Also provides adapter classes to allow the use of third party I2C libraries using the same API.

Enables Bluetooth® Low Energy connectivity on the Arduino MKR WiFi 1010, Arduino UNO WiFi Rev.2, Arduino Nano 33 IoT, Arduino Nano 33 BLE and Nicla Sense ME.

Fully Asynchronous UDP Library for RASPBERRY_PI_PICO_W using CYW43439 WiFi with arduino-pico core. The library is easy to use and includes support for Unicast, Broadcast and Multicast environments.

The last hope for the desperate AVR programmer. A small (344 bytes) Arduino library to have real program traces and to find the place where your program hangs.

This library enables you to use Hardware-based PWM channels on AVR-based boards, such as Nano, UNO, Mega, Leonardo, 32u4, etc., to create and output PWM.

This library enables you to use ISR-based PWM channels on AVR-based boards, such as Mega-2560, UNO,Nano, Leonardo, etc., to create and output PWM any GPIO pin.

Simple WiFiManager for Blynk with MultiWiFi Credentials, for Mega, SAM DUE, SAMD21, SAMD51, nRF52, STM32F/L/H/G/WB/MP1, Teensy, RP2040-based RASPBERRY_PI_PICO, etc. boards running ESP8266/ESP32-AT shields. Configuration data saved in EEPROM, EEPROM-emulated FlashStorage_STM32 or FlashStorage_SAMD, SAM-DUE DueFlashStorage or nRF52/TP2040 LittleFS.

Simple WiFiManager for Blynk and Mega, UNO WiFi Rev2, Teensy, SAM DUE, SAMD21, SAMD51, STM32, nRF52, RP2040-based boards, etc. using WiFiNINA shields, configuration data saved in EEPROM, FlashStorage_SAMD, FlashStorage_STM32, DueFlashStorage, nRF52/RP2040 LittleFS

An Arduino library that takes input in degrees and output a string or integer for the 4, 8, 16, or 32 compass headings (like North, South, East, and West).

DDNS Update Client Library for SAM DUE, nRF52, SAMD21/SAMD51, STM32F/L/H/G/WB/MP1, AVR Mega, megaAVR, Teensy, RP2040-based RASPBERRY_PI_PICO, WT32_ETH01, Portenta_H7, etc. besides ESP8266/ESP32, using ESP8266-AT/ESP32-AT WiFi, WiFiNINA, Ethernet W5x00, ENC28J60, LAN8742A or Teensy NativeEthernet

Directly interface Arduino, esp8266, and esp32 to DSC PowerSeries and Classic security systems for integration with home automation, remote control apps, notifications on alarm events, and emulating DSC panels to connect DSC keypads.

This library enables you to use Hardware-based PWM channels on Arduino AVRDx-based boards (AVR128Dx, AVR64Dx, AVR32Dx, etc.), using DxCore, to create and output PWM.

This library enables you to use ISR-based PWM channels on Arduino AVRDx-based boards (AVR128Dx, AVR64Dx, AVR32Dx, etc.), using DxCore, to create and output PWM any GPIO pin.

Small and easy to use Arduino library for using push buttons at INT0/pin2 and / or any PinChangeInterrupt pin.Functions for long and double press detection are included.Just connect buttons between ground and any pin of your Arduino - that"s itNo call of begin() or polling function like update() required. No blocking debouncing delay.

Arduino library for controlling standard LEDs in an easy way. EasyLed provides simple logical methods like led.on(), led.toggle(), led.flash(), led.isOff() and more.

OpenTherm Library to control Central Heating (CH), HVAC (Heating, Ventilation, Air Conditioning) or Solar systems by creating a thermostat using Arduino IDE and ESP32 / ESP8266 hardware.

WizFi360/ESP8266/ESP32-AT library for Arduino providing an easy-to-use way to control WizFi360/ESP8266-AT/ESP32-AT WiFi shields using AT-commands. For AVR, Teensy, SAM DUE, SAMD21, SAMD51, STM32, nRF52, SIPEED_MAIX_DUINO and RP2040-based (Nano_RP2040_Connect, RASPBERRY_PI_PICO, etc.) boards using WizFi360/ESP8266/ESP32 AT-command shields.

Light-Weight WiFi/Credentials Manager for AVR Mega, SAM DUE, SAMD, nRF52, STM32, RP2040-based Nano_RP2040_connect, RASPBERRY_PI_PICO boards, etc. using WizFi360/ESP8266/ESP32-AT-command shields. Powerful-yet-simple-to-use feature to enable adding dynamic custom parameters.

ezTime - pronounced "Easy Time" - is a very easy to use Arduino time and date library that provides NTP network time lookups, extensive timezone support, formatted time and date strings, user events, millisecond precision and more.

A library for implementing fixed-point in-place Fast Fourier Transform on Arduino. It sacrifices precision and instead it is way faster than floating-point implementations.

FTP Client for Generic boards such as AVR Mega, megaAVR, Portenta_H7, Teensy, SAM DUE, SAMD21, SAMD51, STM32F/L/H/G/WB/MP1, nRF52, RP2040-based (Nano-RP2040-Connect, RASPBERRY_PI_PICO, RP2040W, etc.), ESP32/ESP8266 using Ethernet

The GCodeParser library is a lightweight G-Code parser for the Arduino using only a single character buffer to first collect a line of code (also called a "block") from a serial or file input and then parse that line into a code block and comments.

Arduino library for the Flysky/Turnigy RC iBUS protocol - servo (receive) and sensors/telemetry (send) using hardware UART (AVR, ESP32 and STM32 architectures)

An Arduino library to control the Iowa Scaled Engineering I2C-IRSENSE ( https://www.iascaled.com/store/I2C-IRSENSE ) reflective infrared proximity sensor.

Convinient way to map a push-button to a keyboard key. This library utilize the ability of 32u4-based Arduino-compatible boards to emulate USB-keyboard.

This library allows you to easily create light animations from an Arduino board or an ATtiny microcontroller (traffic lights, chaser, shopkeeper sign, etc.)

LiquidCrystal fork for displays based on HD44780. Uses the IOAbstraction library to work with i2c, PCF8574, MCP23017, Shift registers, Arduino pins and ports interchangably.

This library enables you to use ISR-based PWM channels on RP2040-based boards, such as Nano_RP2040_Connect, RASPBERRY_PI_PICO, with Arduino-mbed (mbed_nano or mbed_rp2040) core to create and output PWM any GPIO pin.

Arduino library for MCP4728 quad channel, 12-bit voltage output Digital-to-Analog Convertor with non-volatile memory and I2C compatible Serial Interface

mDNS Library for ESP32, ESP8266, nRF52, SAMD21, SAMD51, SAM DUE, STM32F/L/H/G/WB/MP1, Portenta_H7, AVR Mega, RP2040-based boards, etc. using Ethernet W5x00, ESP WiFi, WiFiNINA or ESP8266-AT shields

This library enables you to use Hardware-based PWM channels on megaAVR-based boards, such as UNO WiFi Rev2, AVR_Nano_Every, etc., to create and output PWM.

This library enables you to use ISR-based PWM channels on an Arduino megaAVR board, such as UNO WiFi Rev2, AVR_Nano_Every, etc., to create and output PWM any GPIO pin.

Replace Arduino methods with mocked versions and let you develop code without the hardware. Run parallel hardware and system development for greater efficiency.

A library package for ARDUINO acting as ModBus slave communicating through UART-to-RS485 converter. Originally written by Geabong github user. Improved by Łukasz Ślusarczyk.

Connects to MySQL or MariaDB using ESP8266/ESP32, WT32_ETH01 (ESP32 + LAN8720A), nRF52, SAMD21/SAMD51, STM32F/L/H/G/WB/MP1, Teensy, SAM DUE, Mega, RP2040-based boards, Portenta_H7, etc. with W5x00, ENC28J60 Ethernet, Teensy 4.1 NativeEthernet/QNEthernet, WiFiNINA modules/shields or Portenta_H7 WiFi/Ethernet. W5x00 can use Ethernet_Generic library. ENC28J60 can use either EthernetENC or UIPEthernet Library.

This library enables you to use ISR-based PWM channels on an nRF52-based board using Arduino-mbed mbed_nano core such as Nano-33-BLE to create and output PWM any GPIO pin.

This library enables you to use ISR-based PWM channels on an nRF52-based board using Adafruit_nRF52_Arduino core such as Itsy-Bitsy nRF52840 to create and output PWM any GPIO pin.

An Arduino library for the Nano 33 BLE Sense that leverages Mbed OS to automatically place sensor measurements in a ring buffer that can be integrated into programs in a simple manner.

his library enables you to use Hardware-based PWM channels on RP2040-based boards, such as Nano_RP2040_Connect, RASPBERRY_PI_PICO, with either Arduino-mbed (mbed_nano or mbed_rp2040) or arduino-pico core to create and output PWM to any GPIO pin.

This library enables you to use SPI SD cards with RP2040-based boards such as Nano_RP2040_Connect, RASPBERRY_PI_PICO using either RP2040 Arduino-mbed or arduino-pico core.

This library enables you to use ISR-based PWM channels on RP2040-based boards, such as ADAFRUIT_FEATHER_RP2040, RASPBERRY_PI_PICO, etc., with arduino-pico core to create and output PWM any GPIO pin.

The most powerful and popular available library for using 7/14/16 segment display, supporting daisy chaining so you can control mass amounts from your Arduino!

Provides methods to retrieve instant and peak values from the ADC input. The Arduino library SensorWLED splits the input from a varying analog signal from the ADC into components, i.e., provides the capability of a sample-and-hold circuit.

Enables smooth servo movement. Linear as well as other (Cubic, Circular, Bounce, etc.) ease movements for servos are provided. The Arduino Servo library or PCA9685 servo expanders are supported.

Enables reading and writing on SD card using SD card slot connected to the SDIO/SDMMC-hardware of the STM32 MCU. For slots connected to SPI-hardware use the standard Arduino SD library.

Menu library for Arduino with IoT capabilities that supports many input and display devices with a designer UI, code generator, CLI, and strong remote control capability.

Adds tcUnicode UTF-8 support to Adafruit_GFX, U8G2, tcMenu, and TFT_eSPI graphics libraries with a graphical font creation utility available. Works with existing libraries

A library for creating Tickers which can call repeating functions. Replaces delay() with non-blocking functions. Recommanded for ESP and Arduino boards with mbed behind.

This library enables you to use Interrupt from Hardware Timers on an Arduino, Adafruit or Sparkfun AVR board, such as Nano, UNO, Mega, Leonardo, YUN, Teensy, Feather_32u4, Feather_328P, Pro Micro, etc.

This library enables you to use Interrupt from Hardware Timers on supported Arduino boards such as AVR, Mega-AVR, ESP8266, ESP32, SAMD, SAM DUE, nRF52, STM32F/L/H/G/WB/MP1, Teensy, Nano-33-BLE, RP2040-based boards, etc.

Really tiny library to basic RTC functionality on Arduino. DS1307, DS3231 and DS3232 RTCs are supported. See https://github.com/Naguissa/uEEPROMLib for EEPROM support. Temperature, Alarms, SQWG, Power lost and RAM support.

Monochrome LCD, OLED and eInk Library. Display controller: SSD1305, SSD1306, SSD1309, SSD1312, SSD1316, SSD1318, SSD1320, SSD1322, SSD1325, SSD1327, SSD1329, SSD1606, SSD1607, SH1106, SH1107, SH1108, SH1122, T6963, RA8835, LC7981, PCD8544, PCF8812, HX1230, UC1601, UC1604, UC1608, UC1610, UC1611, UC1617, UC1638, UC1701, ST7511, ST7528, ST7565, ST7567, ST7571, ST7586, ST7588, ST75160, ST75256, ST75320, NT7534, ST7920, IST3020, IST3088, IST7920, LD7032, KS0108, KS0713, HD44102, T7932, SED1520, SBN1661, IL3820, MAX7219, GP1287, GP1247, GU800. Interfaces: I2C, SPI, Parallel.

True color TFT and OLED library, Up to 18 Bit color depth. Supported display controller: ST7735, ILI9163, ILI9325, ILI9341, ILI9486,LD50T6160, PCF8833, SEPS225, SSD1331, SSD1351, HX8352C.

RFC6455-based WebSockets Server and Client for Arduino boards, such as nRF52, Portenta_H7, SAMD21, SAMD51, STM32F/L/H/G/WB/MP1, Teensy, SAM DUE, RP2040-based boards, besides ESP8266/ESP32 (ESP32, ESP32_S2, ESP32_S3 and ESP32_C3) and WT32_ETH01. Ethernet shields W5100, W5200, W5500, ENC28J60, Teensy 4.1 NativeEthernet/QNEthernet or Portenta_H7 WiFi/Ethernet. Supporting websocket only mode for Socket.IO. Ethernet_Generic library is used as default for W5x00. Now supporting RP2040W

Light-Weight MultiWiFi/Credentials Manager for AVR Mega, Teensy, SAM DUE, SAMD21, SAMD51, STM32F/L/H/G/WB/MP1, nRF52, RP2040-based (Nano RP2040 Connect, RASPBERRY_PI_PICO) boards, etc. using u-blox WiFiNINA / WiFi101 modules/shields. Powerful-yet-simple-to-use feature to enable adding dynamic custom parameters.

Enables network connection (local and Internet) and WiFiStorage for SAM DUE, SAMD21, SAMD51, Teensy, AVR (328P, 32u4, 16u4, etc.), Mega, STM32F/L/H/G/WB/MP1, nRF52, NINA_B302_ublox, NINA_B112_ublox, RP2040-based boards, etc. in addition to Arduino MKR WiFi 1010, Arduino MKR VIDOR 4000, Arduino UNO WiFi Rev.2, Nano 33 IoT, Nano RP2040 Connect. Now with fix of severe limitation to permit sending much larger data than total 4K and using new WiFi101_Generic library

Simple WiFiWebServer, HTTP Client and WebSocket Client library for AVR Mega, megaAVR, Portenta_H7, Teensy, SAM DUE, SAMD21, SAMD51, STM32F/L/H/G/WB/MP1, nRF52, RP2040-based (Nano-RP2040-Connect, RASPBERRY_PI_PICO, RASPBERRY_PI_PICO_W, ESP32/ESP8266, etc.) boards using WiFi, such as WiFiNINA, WiFi101, CYW43439, U-Blox W101, W102, ESP8266/ESP32-AT modules/shields, with functions similar to those of ESP8266/ESP32 WebServer libraries.

Universal Timer with 1 millisecond resolution, based on system uptime (i.e. Arduino: millis() function or STM32: HAL_GetTick() function), supporting OOP principles.