240x240 tft display free sample

This is a quick video showing our new 1.3 inch TFT LCD. This is a small, full-color TFT. It"s controlled via 4-wire SPI. It has a ST7789H2 controller. This display runs off a single 3.3v supply which controls the logic and backlight.

This TFT kit comprises one of our smallest TFT displays and an adapter board that breaks the tail connections out to a simple 2x5 10-position header. The adapter board includes a backlight driver, so only a single 3.3v power input is required to bring up the display.

The adapter board is specifically designed for use with this display, so it fits directly behind the display with no PCB overlap. The display is a 1.3", full color, IPS display that looks incredibly sharp.

We want to use ER-TFT013-1 as auxiliary, internal display on our modules (suppose that you know that my company already use EastRising displays in our product), and we want to use serial interface.

The ST7789 TFT module contains a display controller with the same name: ST7789. It’s a color display that uses SPI interface protocol and requires 3, 4 or 5 control pins, it’s low cost and easy to use. This display is an IPS display, it comes in different sizes (1.3″, 1.54″ …) but all of them should have the same resolution of 240×240 pixel, this means it has 57600 pixels. This module works with 3.3V only and it doesn’t support 5V (not 5V tolerant).

The ST7789 display module shown in project circuit diagram has 7 pins: (from right to left): GND (ground), VCC, SCL (serial clock), SDA (serial data), RES (reset), DC (or D/C: data/command) and BLK (back light).

As mentioned above, the ST7789 TFT display controller works with 3.3V only (power supply and control lines). The display module is supplied with 3.3V (between VCC and GND) which comes from the Arduino board.

To connect the Arduino to the display module, I used voltage divider for each line which means there are 4 voltage dividers. Each voltage divider consists of 2.2k and 3.3k resistors, this drops the 5V into 3V which is sufficient.

The first library is a driver for the ST7789 TFT display which can be installed from Arduino IDE library manager (Sketch —> Include Library —> Manage Libraries …, in the search box write “st7789” and install the one from Adafruit).

This is a graphics library for the family of small colour TFT displays based on the ST7735 and ST7789 driver chips. These are really nice displays; bright, colourful, available in a variety of useful sizes, and available at low cost from suppliers like Adafruit, AliExpress, or Banggood:

This library allows you to plot points, draw lines, draw filled rectangles, and plot text with an optional scale factor. I"ve included a demo histogram-plotting program that adjusts itself to fit each of the displays I"ve supported.

Unlike most other TFT display libraries this one doesn"t require a memory buffer, allowing it to be run on any processor down to an ATtiny85. The displays are SPI and require four pins to drive the display, leaving one pin free on an ATtiny85 to interface to another device, such as a temperature sensor. If you need more pins choose a larger chip, such as the ATtiny84; see Using the library with other AVR chips at the end of the article for information about how to convert the code for different chips.

I"ve published a library for a colour OLED display in a previous article: Colour Graphics Library. The main difference between the colour TFT displays and the colour OLED displays is that the TFT displays are not self-illuminating, and so need a backlight; they therefore have a slightly higher power consumption. However, they are exceedingly cheap, and they are available in larger sizes than the colour OLED displays.

This library will work with displays based on the ST7735 which supports a maximum display size of 132 (H) x 162 (V), or the similar ST7789 which supports a maximum display size of 240 (H) x 320 (V).

The display driver interfaces to the displays with the longer side as the vertical dimension, which is why the rectangular displays are usually listed with the longer dimension second. My library allows you to rotate the image for any desired orientation.

All the Adafruit breakout boards for these displays include level-shifting circuitry, so they will work with either 5V or 3.3V microcontroller boards. They also include an SD card socket, if that"s of interest to you. The Adafruit boards have pullups on the backlight and reset pins, so the display will work if you leave these pins unconnected.

The pullup resistor from the display"s CS pin is optional; it holds the chip select high to prevent the display from being affected by the ISP signals while programming the ATtiny85.

The different displays are catered for by six constants which specify the size of the display, the offsets relative to the area supported by the display driver, whether the display is inverted, and the rotation value; for example:

Note that on some displays you may also have to change the xoff or yoff value when rotating the display. For example, to rotate the image on the 240x240 displays by 180° use the settings:

To check or adjust the values for each display I ran this program, which draws a one-pixel border around the display area, and plots an "F" to show the orientation:

The ATtiny85 and other AVR processors supports toggling of one or more bits in a port, so provided you set all the pins to their disabled state at startup, for speed the display access routines can simply toggle the appropriate pins to enable or disable them.

The InitDisplay() routine first defines the four display pins as outputs, and takes the SCK, DC, and CS pins high (inactive). It then sends the essential configuration commands to the display.

The display memory stores 18 bits per pixel: 6 bits per colour. However, you can write to the display in three alternative modes, with 12, 16, or 18 bits per pixel. I chose the 16 bit mode, which assigns 5 bits to red, 6 bits to green, and 5 bits blue. It"s the most convenient one to work with as you simply send two bytes to define the colour of each pixel.

To clear the display the ClearDisplay() routine sends the appropriate number of zero bytes. The routine temporarily switches to 12-bit colour mode, which reduces the time to clear the display by 25%:

The library includes basic graphics routines for plotting points and drawing lines. These work on a conventional coordinate system with the origin at lower left. For example, on the 80x160 display:

My first version of PlotChar() plotted characters by calling PlotPoint() for each pixel. However, I then tried the following alternative approach which defines an area of the display using the CASET (Column Address Set) and RASET (Row Address Set) commands, and then sends a stream of the appropriate bytes to define the character. This turned out to be over three times faster!

14th January 2020: Tested the program with the Adafruit 1.3" 240x240 TFT display, and updated the program to correct a problem when rotating the image on that display.

Focus Displays offers a wide range of standard full color TFT displays. 64 million unique colors, high brightness, sharp contrast, -30C operating temperature, and fast response time are all good descriptions of a TFT display. This is why TFT technology is one of the most popular choices for a new product.

Thin Film Transistor (TFT) display technology can be seen in products such as laptop computers, cell phones, tablets, digital cameras, and many other products that require color. TFT’s are active matrix displays which offers exceptional viewing experiences especially when compared to other passive matrix technologies. The clarity on TFT displays is outstanding; and they possess a longer half-life than some types of OLEDs and range in sizes from less than an inch to over 15 inches.

CCFL’s are still available, but are becoming a legacy (obsolete) component. TFT displays equipped with a CCFL require higher MOQs (Minimum Order Quantities) than displays with LED backlights.

The majority of TFT displays contain a touch panel, or touch screen. The touch panel is a touch-sensitive transparent overlay mounted on the front of the display glass. Allowing for interaction between the user and the LCD display.

Some touch panels require an independent driver IC; which can be included in the TFT display module or placed on the customer’s Printed Circuit Board (PCB). Touch screens make use of coordinate systems to locate where the user touched the screen.

Resistive touch panels are the lowest cost option and are standard equipment on many TFT modules. They are more common on smaller TFT displays, but can still be incorporated on larger modules.

Contrast ratio, or static contrast ratio, is one way to measure the sharpness of the TFT LCD display. This ratio is the difference between the darkest black and the brightest white the display is able to produce. The higher the number on the left, the sharper the image. A typical contrast ratio for TFT may be 300:1. This number ratio means that the white is 300 times brighter than the black.

TFT LCD displays are measured in inches; this is the measurement of the diagonal distance across the glass. Common TFT sizes include: 1.77”, 2.4”, 2.8”, 3”, 4.3”, 5”, 5.7”, 5.8”, 7”, 10.2”, 12.1 and 15”.

As a general rule, the larger the size of the glass the higher the cost of the display, but there are exceptions to this rule. A larger display may be less expensive than a smaller display if the manufacture produces higher quantities of the larger displays. When selecting your color display, be sure to ask what the cost is for one size smaller and one size larger. It may be worth modifying your design requirements.

TFT resolution is the number of dots or pixels the display contains. It is measured by the number of dots along the horizontal (X axis) and the dots along the vertical (Y axis).

The higher the resolution, the more dots per square inch (DPI), the sharper the display will look. A higher resolution results in a higher cost. One reason for the increase in cost is that more driver chips are necessary to drive each segment.

Certain combinations of width and height are standardized and typically given a name and a letter representation that is descriptive of its dimensions. Popular names given to the TFT LCD displays resolution include:

Transmissive displays must have the backlight on at all times to read the display, but are not the best option in direct sunlight unless the backlight is 750 Nits or higher. A majority of TFT displays are Transmissive, but they will require more power to operate with a brighter backlight.

Transflective displays are readable with the backlight off provided there is enough ambient light. Transflective displays are more expensive than Transmissive also there may be a larger MOQ for Transflective. However, Transflective displays are the best option for direct sunlight.

Drivers update and refresh the pixels (Picture Elements) of a display. Each driver is assigned a set number of pixels. If there are more pixels than a single driver can handle, then an additional drivers are added.

A primary job of the driver is to refresh each pixel. In passive TFT displays, the pixel is refreshed and then allowed to slowly fade (aka decay) until refreshed again. The higher the refresh frequency, the sharper the displays contrast.

The controller does just what its name suggest. It controls the drivers. There is only one controller per display no matter how many drivers. A complex graphic display with several thousand pixels will contain one controller and several drivers.

The TFT display (minus touch screen/backlight) alone will contain one controller/driver combination. These are built into the display so the design engineer does not need to locate the correct hardware.

If you do not see a Thin Film Transistor (TFT) Display module that meets your specifications, or you need a replacement TFT, we can build a custom TFT displays to meet your requirements. Custom TFTs require a one-time tooling fee and may require higher MOQs.

Ready to order samples for your TFT design? Contact one of our US-based technical support people today concerning your design requirements. Note: We can provide smaller quantities for samples and prototyping.

If none of these part numbers meet your requirements in terms of brightness, interface, or connection method, please email us at info@orientdisplay.com.

New functions have been added to draw smooth (antialiased) arcs, circles, and rounded rectangle outlines. New sketches are provided in the "Smooth Graphics" examples folder. Arcs can be drawn with or without anti-aliasing (which will then render faster). The arc ends can be straight or rounded. The arc drawing algorithm uses an optimised fixed point sqrt() function to improve performance on processors that do not have a hardware Floating Point Unit (e.g. RP2040). Here are two demo images, on the left smooth (anti-aliased) arcs with rounded ends, the image to the right is the same resolution (grabbed from the same 240x240 TFT) with the smoothing diasbled (no anti-aliasing):

An excellent new compatible library is available which can render TrueType fonts on a TFT screen (or into a sprite). This has been developed by takkaO, I have created a branch with some bug fixes here. The library provides access to compact font files, with fully scaleable anti-aliased glyphs. Left, middle and right justified text can also be printed to the screen. I have added TFT_eSPI specific examples to the OpenFontRender library and tested on RP2040 and ESP32 processors, the ESP8266 does not have sufficient RAM due to the glyph render complexity. Here is a demo screen where a single 12kbyte font file binary was used to render fully anti-aliased glyphs of gradually increasing size on a 320x480 TFT screen:

Support has been added in v2.4.70 for the RP2040 with 16 bit parallel displays. This has been tested and the screen update performance is very good (4ms to clear 320 x 480 screen with HC8357C). The use of the RP2040 PIO makes it easy to change the write cycle timing for different displays. DMA with 16 bit transfers is also supported.

Smooth fonts can now be rendered direct to the TFT with very little flicker for quickly changing values. This is achieved by a line-by-line and block-by-block update of the glyph area without drawing pixels twice. This is a "breaking" change for some sketches because a new true/false parameter is needed to render the background. The default is false if the parameter is missing, Examples:

New anti-aliased graphics functions to draw lines, wedge shaped lines, circles and rounded rectangles. Examples are included. Examples have also been added to display PNG compressed images (note: requires ~40kbytes RAM).

Frank Boesing has created an extension library for TFT_eSPI that allows a large range of ready-built fonts to be used. Frank"s library (adapted to permit rendering in sprites as well as TFT) can be downloaded here. More than 3300 additional Fonts are available here. The TFT_eSPI_ext library contains examples that demonstrate the use of the fonts.

Users of PowerPoint experienced with running macros may be interested in the pptm sketch generator here, this converts graphics and tables drawn in PowerPoint slides into an Arduino sketch that renders the graphics on a 480x320 TFT. This is based on VB macros created by Kris Kasprzak here.

The RP2040 8 bit parallel interface uses the PIO. The PIO now manages the "setWindow" and "block fill" actions, releasing the processor for other tasks when areas of the screen are being filled with a colour. The PIO can optionally be used for SPI interface displays if #define RP2040_PIO_SPI is put in the setup file. Touch screens and pixel read operations are not supported when the PIO interface is used.

An Arduino IDE compatible graphics and fonts library for 32 bit processors. The library is targeted at 32 bit processors, it has been performance optimised for RP2040, STM32, ESP8266 and ESP32 types, other processors may be used but will use the slower generic Arduino interface calls. The library can be loaded using the Arduino IDE"s Library Manager. Direct Memory Access (DMA) can be used with the ESP32, RP2040 and STM32 processors with SPI interface displays to improve rendering performance. DMA with a parallel interface (8 and 16 bit parallel) is only supported with the RP2040.

For other processors only SPI interface displays are supported and the slower Arduino SPI library functions are used by the library. Higher clock speed processors such as used for the Teensy 3.x and 4.x boards will still provide a very good performance with the generic Arduino SPI functions.

"Four wire" SPI and 8 bit parallel interfaces are supported. Due to lack of GPIO pins the 8 bit parallel interface is NOT supported on the ESP8266. 8 bit parallel interface TFTs (e.g. UNO format mcufriend shields) can used with the STM32 Nucleo 64/144 range or the UNO format ESP32 (see below for ESP32).

The library supports some TFT displays designed for the Raspberry Pi (RPi) that are based on a ILI9486 or ST7796 driver chip with a 480 x 320 pixel screen. The ILI9486 RPi display must be of the Waveshare design and use a 16 bit serial interface based on the 74HC04, 74HC4040 and 2 x 74HC4094 logic chips. Note that due to design variations between these displays not all RPi displays will work with this library, so purchasing a RPi display of these types solely for use with this library is NOT recommended.

A "good" RPi display is the MHS-4.0 inch Display-B type ST7796 which provides good performance. This has a dedicated controller and can be clocked at up to 80MHz with the ESP32 (125MHz with overclocked RP2040, 55MHz with STM32 and 40MHz with ESP8266). The MHS-3.5 inch RPi ILI9486 based display is also supported, however the MHS ILI9341 based display of the same type does NOT work with this library.

Some displays permit the internal TFT screen RAM to be read, a few of the examples use this feature. The TFT_Screen_Capture example allows full screens to be captured and sent to a PC, this is handy to create program documentation.

The library supports Waveshare 2 and 3 colour ePaper displays using full frame buffers. This addition is relatively immature and thus only one example has been provided.

The library includes a "Sprite" class, this enables flicker free updates of complex graphics. Direct writes to the TFT with graphics functions are still available, so existing sketches do not need to be changed.

The "Animated_dial" example shows how dials can be created using a rotated Sprite for the needle. To run this example the TFT interface must support reading from the screen RAM (not all do). The dial rim and scale is a jpeg image, created using a paint program.

The XPT2046 touch screen controller is supported for SPI based displays only. The SPI bus for the touch controller is shared with the TFT and only an additional chip select line is needed. This support will eventually be deprecated when a suitable touch screen library is available.

The library supports SPI overlap on the ESP8266 so the TFT screen can share MOSI, MISO and SCLK pins with the program FLASH, this frees up GPIO pins for other uses. Only one SPI device can be connected to the FLASH pins and the chips select for the TFT must be on pin D3 (GPIO0).

Configuration of the library font selections, pins used to interface with the TFT and other features is made by editing the User_Setup.h file in the library folder, or by selecting your own configuration in the "User_Setup_Selet,h" file. Fonts and features can easily be enabled/disabled by commenting out lines.

It would be possible to compress the vlw font files but the rendering performance to a TFT is still good when storing the font file(s) in SPIFFS, LittleFS or FLASH arrays.

Anti-aliased fonts can also be drawn over a gradient background with a callback to fetch the background colour of each pixel. This pixel colour can be set by the gradient algorithm or by reading back the TFT screen memory (if reading the display is supported).

The common 8 bit "Mcufriend" shields are supported for the STM Nucleo 64/144 boards and ESP32 UNO style board. The STM32 "Blue/Black Pill" boards can also be used with 8 bit parallel displays.

Unfortunately the typical UNO/mcufriend TFT display board maps LCD_RD, LCD_CS and LCD_RST signals to the ESP32 analogue pins 35, 34 and 36 which are input only. To solve this I linked in the 3 spare pins IO15, IO33 and IO32 by adding wires to the bottom of the board as follows:

If the display board is fitted with a resistance based touch screen then this can be used by performing the modifications described here and the fork of the Adafruit library:

If you load a new copy of TFT_eSPI then it will overwrite your setups if they are kept within the TFT_eSPI folder. One way around this is to create a new folder in your Arduino library folder called "TFT_eSPI_Setups". You then place your custom setup.h files in there. After an upgrade simply edit the User_Setup_Select.h file to point to your custom setup file e.g.:

We"ve been looking for a display like this for a long time - it"s only 1.5" diagonal but has a high density 220 ppi, 240x240 pixel display with full-angle viewing.

It looks a lot like the 1.44" 128x128 display, but has 4x as many pixels and looks great at any angle. We"ve seen displays of this caliber used in smartwatches and small electronic devices but they"ve always been MIPI interface. Finally, we found one that is SPI and has a friendly display driver, so it works with any and all microcontrollers or microcomputers!

This lovely little display breakout is the best way to add a small, colorful and very bright display to any project. Since the display uses 4-wire SPI to communicate and has its own pixel-addressable frame buffer, it can be used with every kind of microcontroller. Even a very small one with low memory and few pins available! The 1.54" display has 240x240 16-bit full color pixels and is an IPS display, so the color looks great up to 80 degrees off axis in any direction. The TFT driver (ST7789) is very similar to the popular ST7735, and our Arduino library supports it well.

This breakout has the TFT display soldered on (it uses a delicate flex-circuit connector) as well as a ultra-low-dropout 3.3V regulator and a 3/5V level shifter so you can use it with 3.3V or 5V power and logic. There was a little space so Adafruit placed a microSD card holder so you can easily load full color bitmaps from a FAT16/FAT32 formatted microSD card. The microSD card is not included, but you can pick one up here.

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