teensy tft display quotation

The version of this library, in which we have worked for a couple of years, is 100% functional for teensy 3.6 and teensy 4. With my brother lightcalamar, and with the support of the good nopnop2002, we managed to respond to the question: What screen to use in teensy 4 or teensy 3.6 to give a decent use to the SDIO reader?

The cost of such screens is an interesting challenge at the beginning, however the investment is well worth it, by seeing what can be designed in the TFT. It hasn"t stopped surprising me since I had my first gameduino 2 on the worktable.

As if that were not enough, Greiman recently gave us the SdFat beta library, which is compatible with the SDIO reader of teensy 4, and that by tests we have proven that it also works in the teensy reader 3.6.

One thing people have done in the past is failing to notice the Project Options tab. You do have teensy set correctly in general settings so that when you create a new project it will default to target teensy, but if you created your project before setting this value it could be set to arduino inside your test project. So you will need to double check your project is set for teensy. Otherwise you will get compiler errors.

You can check this setting by opening your test project in the builder and clicking on the Project Options tab and it must show teensy for your target also.

Spice up your Arduino project with a beautiful large touchscreen display shield with built in MicroSD card connection. This TFT display is big (5" diagonal) bright (12 white-LED backlight) and colorfu 800x480 pixels with individual pixel control. As a bonus, this display has a optional resistive or capacitive touch panel with controller, attached by default

This display shield has a controller built into it with RAM buffering, so that almost no work is done by the microcontroller. You can connect more sensors, buttons and LEDs.

Spice up your Arduino project with a beautiful large touchscreen display shield with built in microSD card connection. This TFT display is big (7" diagonal) bright (18 white-LED backlight) and colorfu 1024x600 pixels with individual pixel control. As a bonus, this display has a optional capacitive touch panel attached on screen by default.

This display shield has a controller built into it with RAM buffering, so that almost no work is done by the microcontroller. You can connect more sensors, buttons and LEDs.

I sorry, the microSD with the ILI9341 nope at this moment. But with FT813 + gameduino library + SdFat_beta, work it surprisingly well in teensy 4. Later I share the library; It is work in process.

I have been working with a similar library: the ILI9488_t3. Until a few minutes ago, the XPT2046 touch chip did not work on the Teensy-4. The key is the file HW_ARM_defines.h inside of the path: URTouch\hardware\arm

EasyTFT board is a perfect choice for users or mikroElektronika boards who want to upgrade their GLCD with TFT display. It features connector compatible with GLCD 128x64 connectors, as well as touch panel connectors. Board also contains TFT Color Display MI0283QT-9A with 320x240px resolution, which is driven by

Arduino shields are meant to extend the capabilities of the Arduino, while also making initial development of a new device much easier for the user. In this case, our NHD-FT81x-SHIELD provides seamless connectivity and direct software compatibility for the user with any of our EVE2 TFT Modules and an Arduino. This shield has built-in logic level shifting, an on-board buck switching regulator, an audio power amplifier, and a microSD card reader for expandable data storage.

Choose from a wide selection of interface options or talk to our experts to select the best one for your project. We can incorporate HDMI, USB, SPI, VGA and more into your display to achieve your design goals.

Equip your display with a custom cut cover glass to improve durability. Choose from a variety of cover glass thicknesses and get optical bonding to protect against moisture and debris.

I have two different noise sources, one I assume is from the power supply but it is very low. I have a 2nd issue with a lot more noise (hum/buzz, video with noise below) coming from a TFT display connected via SPI. This is not my area and wanted to see if anyone has run into this before. I thought it was the backlight but its the chipset 3.3V that seems to be the cause it. I"m using the Teensy 4.0x audio hat but I had to solder jumpers from it to a breadboard to test. Removing the 5V and GND for the backlight does not change anything, as soon as the 3.3V is removed the noise stops, backlight on with no 3.3V chipset connected, no noise. Not sure how to go about filtering this? I added some extra caps on the 3.3V output from the teensy, no change. Tried driving with a separate 3.3V supply, no change, separate 3.3 volt with caps no change. Powered with a bench top regulated power supply , no change. A 2nd switch mode 5V power I had lying around did not help either. I have seen some PT8211 examples using output drivers, but I assume that will just boost the noise. It"s possible its noise from the breadboard, I gave everything the wiggle test but made no difference. I tried a 2nd 4.1 and a second brand new display, no change. Note, I have the trace cut so I can use the USB with the bench supply, disconnecting USB does not change anything. Open to any advice.

A step sequencer is one of the easiest kinds of electronic music “synthesizers” to learn how to use. Synthesizer is in quotes there because many step sequences are just playing prerecorded clips, and aren’t actually generating sounds in real time. That makes them ideal for certain applications, such as drum machines, because the musician just needs to pick a sound and choose which steps in a beat to play it on.The Teensy Beats Shield is a simple step sequenceryou can build yourself, and that is designed specifically to be hackable.

Teensy Beats Shield was created by Chris Miller after he noticed that most commercial step sequencers, like the Pocket Operator for example, are closed source and aren’t intended to be modified. Miller already hada Teensy 3.2laying around, and he decided to use that to build his ownstep sequencersince the Teensy has DSP (digital signal processing) capabilities. What he came up with is the Teensy Beats Shield, which is a complete step sequencer in a handheld, open source, hacker-friendly package.

You can interact with the Teensy Beats Shield through four rotary encoders and 20 buttons. The interface is displayed on a 2.4" TFT LCD touchscreen, and each of the 20 buttons has its own RGB LED to indicate when it’s active. Battery charging is handled by anAdafruit Powerboost 1000c, and a Teensy audio shield provides an amplified DAC headphone output. If you want to build your own Teensy Beats Shield the bare PCB is available onTindieand instructions are onHackaday.io.

CC3000 is troublesome, partly because it uses SPI_MODE1, partly because it uses the SPI port from within an interrupt. Adafruit’s CC3000 library has code to backup the AVR’s SPI registers, change them to MODE1, and then restore when it’s done, so the conflicting clock polarity isn’t (usually) an issue on AVR. But on Due, Teensy 3.1 and all other non-AVR chips, their specific SPI registers aren’t also in the code, so you can pretty easily end up with the SPI port left in the wrong mode.

Interrupts are also a huge problem. Using the CC3000 in simple blocking ways, where you fully complete all communication before you try to write to the display or read the touch screen or access the SD card tends to work. But if you use another device while the CC3000 generates an interrupt at just the wrong moment, it can run its SPI code while another device has chip select asserted, causing all sorts of terribly wrong results.

The touch controller on Adafruit’s displays comes in a couple different types, which need different SPI data modes, and some require very slow clock speeds. Again, they have AVR-only register save/restore, so usually you don’t get wrong settings into other libraries, but there’s no hardware specific code in those libs for non-AVR chips.

My recent work on SPI transactions, which will be in Teensyduino 1.20 (already in the latest release candidate and on github) and is planned for Arduino 1.5.8 (already in their github source and nightly builds) aims to solve both the settings and interrupt problems, in a hardware independent way. Adafruit has already merged my patches to their libs, at least for these most common ones, so they use the new SPI transaction stuff when compiled on those new versions.

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