interfacing 16x2 lcd display with pic microcontroller made in china

In the previous chapter, we have discussed how a character LCD is interfaced with a PIC microcontroller in 8-bit mode, where we used predefined characters stored in the LCD to display our data. In this article, we will learn more about the LCD and how we can create and use custom characters.

DDRAM or “Data Display Random Access Memory” is the working data buffer of the display. Each character on the display has a corresponding DDRAM location and the byte loaded in DDRAM controls which character is displayed.

CGROM or “Character Generation Read Only Memory” holds all the standard patterns for the 5 x 7 dot matrix characters. For instance, if you want to display character “A”, you would send ASCII code 65 (decimal) to the DDRAM. The display controller looks up the pattern of dots to display for this code in the CGROM and lights up the ones appropriate for “A”. The CGROM contents depend on the particular character set and model of display, US, Chinese etc. and cannot be changed.

CGRAM or “Character Generation Random Access Memory” allows the user to define special supplementary non-standard character types that are not in the CGROM. You can load your own dot pattern shapes and call these up for display.

For making custom patterns we need to write values to the CGRAM area defining which pixel to glow. These values are to be written in the CGRAM address starting from 0x40. CGRAM has a total of 64 Bytes. For LCD using 8×5 dots for each character, you can define a total of 8 user defined patterns (1 Byte for each row and 8 rows for each pattern).

Custom characters are assigned fixed display codes from 0 to 7 for pattern stored in the location pointed by CGRAM address 0x40, 0x48, 0x56… and so on. So, if the user wants to display second pattern (pattern stored at CGRAM address 0x48), simply call the data function with value 1 as an argument at a desired location in the LCD.

To display the sequence in the LCD, we need to specify the position on LCD and which pattern to display at the position. Provide adequate delay in between frames to observe the sequence distinctly.

In this tutorial i am going to print/display ASCII characters on 16×2 lcd using pic16f877 microcontroller. Lcd is interfaced with pic microcontroller in 8-bit mode. Code is written in c language. High tech c compiler is used to compile code and code is written in Mp-lab ide. Interfacing 16×2 lcd with pic microcontroller and displaying characters on lcd is very easy. Only the portion which is little bit complex is how to generate/display ASCII characters on lcd. Well you don’t need to generate ASCII characters they are already present in the 16×2 lcd controller (HD44780) Ram, like other characters and numbers. You just need to know how to invoke the ASCII characters to be displayed on the lcd screen.

If you are new in the field of microcontrollers and lcd’s and don’t know about lcd pin out, working and internal structure of lcd then please go through the following tutorials. They will clear you about the working of 16×2 lcd. You will learn how to display characters on lcd? Difference between commands and data send to lcd? It will also explain you about how to use lcd in 4-bit and 8-bit mode.

The circuit diagram of the project is given below. Port-B of Pic16f877 microcontroller is connected with data pins of 16×2 lcd. It means Port-B is used to send commands and data to 16×2 lcd. Lcd control signals(read/write, enable, register select) are provided using individual bits of Port-D. All the other connections are normal connections applying +5 volts to microcontroller and lcd. You can see the circuit diagram given below. Rs(Register select) is connected to Port-D Pin#6. En(Enable) is connected to Port-D Pin#7. Read/Write pin is grounded. Lcd will always remain in write state since we made the R/W pin ground.

Coming to the code portion, I first include the header file htc.h. If you are using high tech c compiler always include this library, this library is necessary to be included in every project that is going to be compiled with high tech c compiler. It contains the compilers directives etc. Then the frequency of the oscillator is defined which is 20 MHz. Then individual pins of Port-D are defined. These pins are used to provide control signals to lcd. delay() function is used to generate some arbitrary delay where necessary. lcdcmd() function is sending commands to lcd with control signals. display() function is sending data to lcd with control signals. lcdint() function is initializing our lcd(8-bit mode, display on ,cursor off etc).

The ascii character 0 is present at address 0x30. To go to address 0x00 negate 0x30 from 0x30. The instruction i=i-0x30; is doing the same job. First i contains 0x30 after executing i=i-0x30 i contains 0x00. Hence we are at starting address of ascii characters. Now increment the address one by one and display the ascii character associated with that address on the lcd screen.

Notethat the ascii characters of the HD44780 controller differs from the standard ascii characters. The HD44780 controller contains ascii characters in the format given on the right side. Some addresses are also void. So don’t get confused when you see the characters like below displayed on your lcd screen.

Total digits in ram of 16×2 lcd are 256. So i decided to display them all. It contains ASCII, numeric, alphabet and special characters(Chinese). Some address are void so nothing will display on the lcd screen across these addresses.

Interfacing of 5V LCD with a 3.3V Controller like LPC1768 is little tricky to handle. This is the Article to explain how a 16x2 LCD is interfaced with LPC1768 in 4-bit mode. LCD in 4 bit, which means we are going to use 4 lines instead of 8 line which save 4 GPIOs which can be used for other purposes. For setting up the Environment for the development of ARM cortex M3 is well discussed in this article.

The LPC 1768 is ARM Cortex- M3 based Microcontrollers for embedded application features in low power consumption and a high level of integration. The ARM Cortex M3 is designed in a such way to enhance debug features and a higher level of system integration. It clocks at a CPU frequency of 100 MHz, and incorporates a 3-stage pipeline and uses a Harvard architecture with separate local instruction and data buses for third bus peripherals. The ARM Cortex- M3 CPU have an internal pre-fetch unit to support speculative branching. The peripheral components include 512KB of flash memory, 64kb of data memory, Ethernet MAC, USB OTG, 4 UART’s, 8-channel general purpose DMA controller, 2 SSP Controllers, 10-bit DAC, Quadrature encoder interface, SPI interface, 3 I2C bus interface, 2 input plus 2 outputs I2S bus interface, 4 general purpose timers, ultra-low power Real-Time Clock (RTC) with separate battery supply, and up to 70 general purpose I/O pins, 6-output general purpose PWM. The LPC1768/66/65/64 are pin-compatible with the 100-pin LPC236x ARM7-based Microcontroller series.

The Chinese LCD used here is JHD162A. It has a KS0066U/ HD44780U controller. It has a 16 pin interface device which consists of 2 rows with 16 characters each. The operating voltage is 5V. Also, it has an LED backlight. There are 2 modes of operation:

Once you have verified all the connections from MCU to HFC4050 to LCD Module, we can go ahead to display text on the LCD. But first, the LCD needs to be initialized properly. (According to the Datasheet) Before initializing the LCD, you will need to wait for a minimum time of about 15 milliseconds after the input voltage supply is stable and greater than 4.5 Volts.

The first step is to make sure that the RS and Enable are held LOW. Next is to input some commands to the LCD using the Data pins. These commands will not be executed until a pulse is supplied to the Enable pin. After supplying the command with a pulse, Enable has to be made High and then Low after a short delay. Then, the command is executed.

The LCD can be configured in 4-bit mode by sending the appropriate command which is called “Function set” to it. The Function set is a hexadecimal command of the LCD MPU unit, which selects working modes of the LCD. The “Function Set” is mentioned in following table:

According to the table, the value of Function Set for 4 –bit mode will be 0010 0000(0x20) because DL=0. The value “Function Set” for the LCD configuration 2 line (N=1), 5X7 dots (F=0) and 4-bit (DL=0) mode will be 0010 1000(0x28).

When the power supply is given to the LCD, it remains in 8-bit mode. Now, if 0x20 is sent, lower nibble will not be received by LCD because four data lines (D4-D7) are connected, so 0x02 is sent instead of 0x20. For more details about the LCD interfacing refer these Links.

In this session we will see how to interface 16×2 LCD to PIC18F4550 microcontroller which is of family PIC18F. You can get information of 16×2 LCD in the session

PIC18F4550 belongs to the PIC18F family; PIC18F4550 is an 8bit microcontroller and uses RISC architecture. PIC18F4550 has 40 pins in PDIP (dual in line package) and 44 pin in TQFP (Quad flat package).

32KB flash memory, 2048 bytes of SRAM (synchronous Random Access memory), EEPROM (Electrically Erasable Program Read Only Memory) of 256 bytes are embedded in the PIC18F4550.

It has 35 I/O pins for interfacing and communication with other peripherals, 13channel of 10bit analog to digital converters which are used for interfacing and communicating the analog peripherals (DC motor, LDR, etc.).

PIC18F4550 has SPI (serial peripheral interface) and i2c (inter integrated circuit) for master and slave modes. It has SPP (Streaming Parallel Port) for USB streaming transfer.

PIC18F4550 is embedded with 4 timer modules (timer0 to timer3), 2 comparator modules and 3 external interrupt. It has Dual Oscillator options allow microcontroller and USB module to run at different clock speeds. It can operate in 2.0V to 5.5V

The resistor R1 is used for giving the contrast to the LCD. The crystal oscillator of 12 MHz is connected to the OSC1 and OSC2 pins of Pic microcontroller PIC18F4550 for system clock. The capacitor C2 and C3 will act filters to the crystal oscillator. You can use different ports or pins for interfacing the LCD before going to different ports please check the data sheet whether the pins for general purpose or they are special function pins.

Interfacing LCD to PIC is not different from interfacing to 8051. The basic concept and gist of the programming is almost same. Visit the following link for more information

Only the pins, registers and architecture using for interfacing will be different. When we look at the program, functions like initialization, sending data to the LCD will be almost same.

In the pic programming also for initializing the LCD the R/W pin should be low for writing the data, Enable pins should be high and register select pin (RS) should be high for writing the data. For sending a command the RS should be low, R/W pin should be low and enable pin should be high.

Install MPLAB in your system and create a new project, in selecting device and family select PIC18F family and add PIC18F4550 controller to your project.

This is our sixth tutorial in our PIC Tutorial Series, in this tutorial we learn Interfacing of 16x2 LCD with PIC Microcontroller. In our previous tutorials we have learnt the basics of PIC using some LED blinking Programs and have also learnt How to use Timers in PIC Microcontroller. You can check here all the tutorials on Learning PIC Microcontrollers using MPLABX and XC8 compiler.

This tutorial will be an interesting one because we will learn How to Interface 16×2 LCD with PIC16F877A, check the detailed Video at the end this tutorial. Gone are the old days where we used LEDs for user indications. Let us see how we can make our projects look more cool and useful by using LCD displays. Also check our previous articles on Interfacing LCD with 8051, with Arduino, with Raspberry Pi, with AVR.

To make things easier we have made a small librarythat could make things easy while using this LCD with our PIC16F877A. The header file "MyLCD.h" is given here for download, which contains all the necessary function to drive the LCD using PIC MCU. Library code is well explained by comment lines but if you still have doubts reach us through the comment section. Also check this article for Basic LCD working and its Pinouts.

Now, there are two ways to add this code into your program. You can either copy all the above lines of code in MyLCD.h and paste them before the void main(). Or you can download the header file using the link and add them to the header file of your project (#include " MyLCD.h ";). This can be done by right clicking on the header file and selecting Add existing Item and browsing to this header file.

Here I have copied and pasted the header file code into my main C file. So if you are using our code, then you don’t need to download and add the header file into your program, just use the complete Code given at the end of this Tutorial. Also note that this library will only support PIC16F series PIC Microcontroller.

void Lcd_Start():This function should be the first function that has to be called to start working with our LCD. We should call this function only once to avoid lag in the program.

void Lcd_Set_Cursor(x pos, y pos):Once started, our LCD is ready to take commands, we can instruct the LCD to set its cursor in you preferred location by using this function.  Suppose if, we need out cursor at 5th character of 1st row. Then the function will be void Lcd_Set_Cursor(1, 5)

Each time the Lcd_Print_Char(char data)is called, its respective character values is sent to the data-lines of the LCD. These characters reach the HD44780U in form of bits. Now this IC relates the bits to the character to be displayed by using its ROM memory as shown the below table. You can find bits for all the characters in the datasheet of HD44780U LCD Controller.

Now, since we are satisfied with our header file let’s build the circuit and test the program. Also check complete header file given in the link given above.

The hardware for this project is very simple. We are going to reuse the same PIC module that we used last time and connect the LCD module to our PIC using jumper wires.

I am on a tight budget since I have so many other hobbies as well but am getting into Arduinos and want to be able to output GPS location data for a fun project I have in mind. I want to confirm if it"s possible to get the cheap LCD I linked above working with an Arduino Pro or Uno and can I get it working with a GPS connected as well or do I need to go with the SparkFun LCD screen instead as it"s a little more ready to go?

I do have programming experience but mostly in MATLAB. I"ve looked at the Arduino sketches and don"t think it"s really too difficult so I don"t foresee any problems but it is my first microcontroller that I"m dealing with. I know a good deal about basic electronics as well so all the circuit stuff isn"t an issue.

This is LCD1602 Parallel LCD Display that provides a simple and cost-effective solution for adding a 16×2 White on RGB Liquid Crystal Display into your project. The display is 16 character by 2 line display that has a very clear and high contrast white text upon a Geen background/backlight. This is the great Green backlight LCD display. It is fantastic for Arduino-based projects. This LCD1602 Parallel LCD Display with Yellow Backlight is very easy to interface with Arduino or Other Microcontrollers. This display overcomes the drawback of LCD1602 Parallel LCD Display in which you’ll waste about 8 Pins on your Arduino for the display to get working. Luckily in this product, an I2C adapter is directly soldered right onto the pins of the display. So all you need to connect are the I2C pins, which show a good library and little coding. The I2C is a type of serial bus developed by Philips, which uses two bidirectional lines, called SDA (Serial Data Line) and SCL (Serial Clock Line). Both must be connected via pulled-up resistors. The usage voltages are standard as 5V and 3.3V.

If you already have the I2C adapter soldered onto the board like in this product, the wiring is quite easy. You should usually have only four pins to hook up. VCC and GND of course. The LCD display works with 5 Volts. So we go for the 5V Pin.

This product is known as 16×2 LCD Display with I2C/IIC interface Green Backlight, display, I2C, I2C Interface, LCD, LCD Display, LCD Display with I2C/IIC interface, LCD1602, Module, Sensors and Modules, 16×2 Green LCD Display With IIC/I2C Interface, 16X2 LCD Display with IIC/I2C interface – Green, 1602 16×2 LCD Display with I2C-IIC interface – Green Backlight, LCD1602 1602 LCD Module Green, LCD 16X2 Alphanumeric Display with I2C/IIC interface – Green Backlight, LCD Display, I2C Interface, I2C, LCD1602, Display for Arduino Uno, 1602 (16×2) LCD Display with I2C/IIC interface – Green Backlight.

Here is a simple project on how to build/generate/make custom characters in 16×2 lcd and then print/display them on lcd using microchip pic16f877 microcontroller. Character lcd contains a set of ascii characters and some Chinese characters in their controllers. We invoke the ascii characters present in the ram for displaying them on lcd. But if we want to display some special characters, symbols or similes we first have to make/declare them in the ram of lcd controller since they are not present in the ascii character set of the lcd. Then we can invoke them for displaying on the lcd when ever is required.

Building and displaying self made custom characters on lcd is not a very hard task. To carry out this task you must know about the internal structure of character lcd. The size of the lcd controller ram, registers of the lcd and CG-RAM(Character generated ram) of lcd. CG-RAM is the most important part of lcd for generating and displaying self made custom characters. CG-RAM is a whole big topic so it is kept in a separate post. CG-RAM is fully discussed in the tutorials below. I recommend you to please take the tutorials other wise you will not unable to understand the code below.

Code is written in c language. MP-LAB ide and High TECH C compiler is used to compile the code. First i included the header file htc.h. This file must be included in every project which is going to be compiled in High Tech c compiler. Then frequency of the crystal is defined which is 20 MHz. Lcd control pins are defined next. Then some character arrays are defined. These character arrays are actually the custom characters which we are going to display on lcd. Delay function is used to give some arbitrary delay where needed. lcdcmd() function is sending commands to lcd. display() function is displaying characters on lcd. lcdint() function is initializing our lcd. In the main function I am generating and then displaying character on lcd.

To understand the code you must first know the internal structure of lcd. The tutorials links given above are very helpful for understanding the working and internal structure of CG-RAM of lcd. If you didn’t take the tutorials. I recommend you to please go through them before going through the code below.

PIC18F452 is a 40 pin microcontroller each pin has its own functionality,which we will see in our next classes one by one. suppose I want to create a project on LED blinking, a simple project on PIC so what I have to do.First of all, I need the following things so that I may use them to work with PIC. I have discussed the Functions available in PIC18F452 Micrcontroller in the next post.

PIC Programmer ----> There are many PIC Programmers available in market ( Hall Road in Lahore ). You have to attach this Programmer with the PC through serial port (Parallel Port & USB Port PIC Programmer are also available). Now Put you PIC in the 40 pin socket of Programmer.

PICpgm Software / PICKit software ----> This software is used to burn thr program in the PIC .Its just like the same as burning a movie on DVD.Now open this software and browse to your program which you have made in MPLAB and then click on program PIC and your Program will be burnt on your PIC.

PIC Basic Circuit ---->PIC alone never works, we have to add a simple circuitry to make it work Below figure shows the simple PIC circuit.It is not much difficult and you can make it on a simple vero board with soldering.I have given its circuit below :

Now remove your PIC microcontrollers from the Programmer and add it in the hardware circuit and you will check that your PIC will give you the same results as you have programmed in it. In this tutorial,we have just take an overview of PIC Microcontrollers. Next we will check its Ports and whats the purpose of these ports. Till then take care ALLAH HAFIZ .... :))

In this tutorial, you will learn to interface anLCD with a pic microcontroller. It is very simple and easy to understand the project for beginners and is commonly used in several electronic products. LCD (Liquid Crystal Display)provides a user-friendly interface and can be very useful for debugging purposes. After completion of this tutorial, you will be able to display data on an LCD using MPLAB XC8 Compiler and Mikro C compiler. We will provide examples with two Compilers such as MPLAB XC8 Compiler and Mikro C for PIC.

The reason LCD is more popular than LED, Seven Segment displays. Because we can display characters, numbers and custom characters with ease ( Just by easily programming a module).

First of all, to interface LCD with a pic microcontroller, we used GPIO pins.  GPIO pins are general-purpose input-output pins. Because we send control and data signals to LCD through these I/O pins.  Therefore, you should know how to use digital input-output pins of the pic microcontroller. To know about GPIO pins programming, check these tutorials:

It can work in two modes, 4-bit and 8-bit. In this tutorial, we have used the 4-bit mode which uses only 4 data lines, thus saving pins of the microcontroller. So It is recommended to use LCD in four bits mode to save pins of the microcontroller for other applications.

As you can see in this diagram, if we use 8-bit mode interfacing, we will need to use 11 pins of pic microcontroller. On the other hand, if we use 4-bit mode, we need only 6 GPIO pins. Therefore, it is recommended to use 4-bit mode interfacing.  The only difference between  4-bit and 8-bit is that data transfer speed is faster for 8-bit mode. However, it doesn’t make any major difference.

A variable resistor is used to adjust the contrast of 5×8 dot pixels according to background light. Therefore, if you are not able to see anything on LCD after programming, the maximum changes are that you need to adjust contrast with the variable resistor. This contrast register makes adjust to the voltage applied on the VEE pin.

For MPLAB XC8 Compiler, we will use the PIC18F4550 microcontroller. For MikroC Pro for PIC, we will use the PIC16F877A microcontroller. In the case of MPLAB XC8, we will develop our own LCD library. Because the XC8 compiler does not provide built-in libraries. In the contrary, MikroC Pro provides libraries for all modules such as LCD, Keypad, ADC Module, UART module.

In this section, we will see how to write example code for 16×2 LCD interfacing with PIC18F4550 microcontroller. Although, you can use see code with other Pic microcontrollers also.

As we mentioned earlier, we can use the 8-bit mode and 4-bit mode interfacing. But due to the efficient use of MCU pins, we will be using 4-bit Mode. To interface LCD, we follow these steps:

In this circuit, we used the PORTB of PIC18F4550. But you can use any PORT. To do this, we need to change the pin assignment inside the code. I will show you how to assign pins for LCD in the next section.

These lines define which pins of the pic microcontroller should connect with LCD. For instance, in this example, we used the PORTD of PIC18F4550 microcontroller. Connect RD0-RD3 pins with D4-D7 pins of LCD respectively and other pins with RW, EN, RS and Power pins. But you can change PORT to other PORT of PIC microcontroller also by changing the PORT name with these commands.

LCDWriteNibble() function is used to write a nibble. Nibble is basically a half a byte. Because we are using LCD in four bits mode. Therefore, we need to send 8-bit commands/data in four bits chunks. This function writes the specified nibble to the LCD.

Because we will use 4-bit mode, data and commands transfer in 4-bits format. Even it requires at least 8-bit to display a character. To resolve this issue, we send data in a 4-bits format two times.

void LCDPutChar(char ch): Writes a character to LCD at current cursor position. This function displays the specified ASCII character at the current position on the LCD.

LCDGoto(char pos, char ln): This function positions the cursor at the specified line and column. Column and line at which the cursor should be positioned between 0-15 for pos (column) and 0-1 for ln(row).

In last section, we have seen how to display ASCII characters or string. But in almost all practical projects, we need to display, integer, float values. This code displays the counter value which increments from 0-9 after every one second. This is the main function of code only. Because the rest of the code is same as the previous program example.

In this section, we will see how to interface LCD with pic microcontroller and programming examples using MikroC for pic. MikroC pro has a built-in library.

We have used 16×2 LCD which means there are 2 rows and 16 characters in each row. So we can display a total of 32 characters at a time in two rows with 16 characters in each row.

This is the main command which prints our text on LCD. It also gives the privilege to specify the position of the text. In the horizontal direction, we count rows number and in a vertical direction, we count the column number. In above command,

However if your string is longer than the number of characters that could be displayed in a row from the starting position, the last characters will not be displayed. E.g. Lcd_Out (1, 6 “LCD Interface”);will display text in row 1 starting from column position 6 and will display only LCD Interfacethe rest of the characters will not be displayed as there is no room for them.

void Lcd_Out_Cp(char *text);will start printing the text from the current cursor position. For example after printing Lcd_Out (1, 1, “LCD”);if you write Lcd_Out_Cp(“Hello”);it will display “Hello”at a position from column position of 4 in row 1.

void Lcd_Chr(char row, char column, char out_char);allows only single characters to be displayed at specified positions. E.g. Lcd_Chr(2, 5, ‘A’); will print only A on column 5 row 2.

void Lcd_Chr_Cp(char out_char); allows to print only single character from current cursor position like after Lcd_Chr(2, 5, ‘A’);if your writeLcd_Chr_Cp(‘B’);it will be printed at row 2 column 6.

To interface LCD withPIC16F877A and display the text ‘LCD INTERFACE’ on it. LCDs come in different sizes and shapes. For this project, we have selected a 16×2 character, alphanumeric LCD. It contains 2 rows of 16 character.

When using PIC microcontroller, the mikroC compiler has a built-in LCD library that supports the commands to carry out LCD initialization. The library consists of a number of functions to control LCDs with 4-bit data interface.

The main program first clears the LCD screen and then displays “LCD INTERFACE” in the first row of LCD. The LCD pin directions are all set as outputs. The RS pin of LCD is set to 1, which indicates that the information received from DB4-DB7 is a valid text to be printed on LCD screen. The EN pin is also set to 1 which indicates that data is send to the LCD.

Programmed LCDs are vastly used for industrial as well as commercial applications. LCDs are used in UPSs or inverters, where voltage and current readings are displayed on the screen. Instructions to be followed are displayed on an LCD screen in airports, banks, hospitals, etc. If you still have any issue after reading this article, feel free to comment on this post with your issues.

Thearduino LCD Keypad shieldis developed for Arduino compatible boards, to provide a user-friendly interface that allows users to go through the menu, make selections etc. It consists of a 1602 white character blue backlight LCD. The keypad consists of 5 keys select, up, right, down and left. To save the digital IO pins, the keypad interface uses only one ADC channel. The key value is read through a 5-stage voltage divider.

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

ERM19264_UC1609_TEXT, Library for ERM19264-5 v3 LCD (UC1609C controller) for the Arduino eco-system. This is a light weight, text only version of the main ERM19264_UC1609 library.