File content as of revision 0:d109c328c59a:

#include "mbed.h"
#include "TS_DISCO_F746NG.h"
#include "LCD_DISCO_F746NG.h"
#include "herkulex.h"


#define SERVO_ID 0x02
         
Herkulex sv(PC_6, PC_7, 115200);
DigitalOut led(LED1);

LCD_DISCO_F746NG lcd;
TS_DISCO_F746NG ts;

int main()
{
    TS_StateTypeDef TS_State;
    uint16_t x, y;
    uint8_t text[28];
    uint8_t nom[50];
    uint8_t status;
    uint8_t idx;
    uint8_t cleared = 0;
    uint8_t prev_nb_touches = 0;
    int init=0, init1=0;
    int position = 0; 
    float positiondeg = 0;
    

    lcd.DisplayStringAt(0, LINE(5), (uint8_t *)"HerkuleX Smart Robot Servo", CENTER_MODE);
    lcd.SetBackColor(LCD_COLOR_BLACK);
    lcd.SetTextColor(LCD_COLOR_WHITE);
    wait(1);
    
    status = ts.Init(lcd.GetXSize(), lcd.GetYSize());
    if (status != TS_OK) {
        lcd.Clear(LCD_COLOR_RED);
        lcd.SetBackColor(LCD_COLOR_RED);
        lcd.SetTextColor(LCD_COLOR_WHITE);
        lcd.DisplayStringAt(0, LINE(5), (uint8_t *)"TOUCHSCREEN INIT FAIL", CENTER_MODE);
    } else {
        lcd.Clear(LCD_COLOR_GREEN);
        lcd.SetBackColor(LCD_COLOR_GREEN);
        lcd.SetTextColor(LCD_COLOR_WHITE);
        lcd.DisplayStringAt(0, LINE(5), (uint8_t *)"TOUCHSCREEN INIT OK", CENTER_MODE);
    }
                    /**
                * @brief  Draws a rectangle.
                * @param  Xpos: X position
                * @param  Ypos: Y position
                * @param  Width: Rectangle width  
                * @param  Height: Rectangle height
                * @retval None
                */
        lcd.SetFont(&Font12);
        lcd.SetBackColor(LCD_COLOR_WHITE);
        lcd.SetTextColor(LCD_COLOR_WHITE);
    

    sv.clear(SERVO_ID);

    sv.leds(SERVO_ID, 4);

    sv.setTorque(SERVO_ID, TORQUE_ON);

    //sv.getStatus(SERVO_ID);

    //sv.getPos(SERVO_ID);

    wait(1);

    sv.positionControl(SERVO_ID, 21, 60, GLED_ON);
    wait(1);
    sv.getStatus(SERVO_ID);
    position = sv.getPos(SERVO_ID); 
    
    while(1) {
        
        //position = sv.getPos(SERVO_ID); 
        sprintf((char*)text, "position : %d", position);
        lcd.DisplayStringAt(200, LINE(10), (uint8_t *)&text, LEFT_MODE);
        
        positiondeg = float(position)*0.325;
        
        //display current position
        lcd.SetBackColor(LCD_COLOR_WHITE);
        lcd.SetTextColor(LCD_COLOR_BLACK);
        sprintf((char*)text, "current position: %d degrees", int(positiondeg));
        lcd.DisplayStringAt(10, LINE(21), (uint8_t *)&text, CENTER_MODE);
        
        //button Init
        lcd.SetTextColor(LCD_COLOR_BLACK);
        lcd.DrawRect(380,40,50,25);
        lcd.SetBackColor(LCD_COLOR_WHITE);
        lcd.SetTextColor(LCD_COLOR_BLACK);
        sprintf((char*)text,"Init");
        lcd.DisplayStringAt(392, LINE(4), (uint8_t *)&text, LEFT_MODE);

        init=init+1;
        
        /*sprintf((char*)text,"%d",init);
        lcd.DisplayStringAt(0, LINE(21), (uint8_t *)&text, LEFT_MODE);
        wait_ms(20);*/
        
        //button Uturn
        lcd.SetTextColor(LCD_COLOR_BLACK); //choisi la couleur du texte
        lcd.DrawRect(380,90,50,25);        //trace le rectangle
        lcd.SetBackColor(LCD_COLOR_WHITE); //choisi la couleur à l'arière du texte
        lcd.SetTextColor(LCD_COLOR_BLACK); //choisi la couleur du texte
        sprintf((char*)text,"U-turn");     //ecrit "Uturn"
        lcd.DisplayStringAt(385, LINE(8), (uint8_t *)&text, LEFT_MODE); 
                                           //choisi la ligne où ecrire
        
        //button +10
        lcd.SetTextColor(LCD_COLOR_BLACK);
        lcd.DrawRect(380,140,50,25);
        lcd.SetBackColor(LCD_COLOR_WHITE);
        lcd.SetTextColor(LCD_COLOR_BLACK);
        sprintf((char*)text,"+10");
        lcd.DisplayStringAt(391, LINE(12), (uint8_t *)&text, LEFT_MODE);
        
        //button -10
        lcd.SetTextColor(LCD_COLOR_BLACK);
        lcd.DrawRect(380,190,50,25);
        lcd.SetBackColor(LCD_COLOR_WHITE);
        lcd.SetTextColor(LCD_COLOR_BLACK);
        sprintf((char*)text,"-10");
        lcd.DisplayStringAt(391, LINE(16), (uint8_t *)&text, LEFT_MODE);
        
        //middle circle
        lcd.SetTextColor(LCD_COLOR_BLACK);
        lcd.DrawCircle(240, 136, 100);
        
        ts.GetState(&TS_State);
        if (TS_State.touchDetected) { //when finger detected
            // Clear lines corresponding to old touches coordinates
            if (TS_State.touchDetected < prev_nb_touches) {
                for (idx = (TS_State.touchDetected + 1); idx <= 5; idx++) {
                    lcd.ClearStringLine(idx);
                }
            }
            prev_nb_touches = TS_State.touchDetected;

            cleared = 0;

            //sprintf((char*)text, "Touches: %d", TS_State.touchDetected);
            //lcd.DisplayStringAt(0, LINE(0), (uint8_t *)&text, LEFT_MODE);

            for (idx = 0; idx < TS_State.touchDetected; idx++) {
                x = TS_State.touchX[idx];
                y = TS_State.touchY[idx];
                //sprintf((char*)text, "Touch %d: x=%d y=%d    ", idx+1, x, y);
                //lcd.DisplayStringAt(0, LINE(idx+1), (uint8_t *)&text, LEFT_MODE);
            }
            //filling button Init  
            if (TS_State.touchDetected&(380<x & x<430)&(40<y & y<65)) {      
                 lcd.FillRect(380, 40, 50, 25);
                 sv.positionControl(SERVO_ID, 21, 60, GLED_ON);
                 wait(1);
                 sv.getStatus(SERVO_ID);
                 position = sv.getPos(SERVO_ID);  
             }
             //filling button Uturn  
            if (TS_State.touchDetected&(380<x & x<430)&(90<y & y<115)) {      
                 lcd.FillRect(380,90,50,25);
                 position = sv.getPos(SERVO_ID); 
                 if( position <= 490)
                 {    sv.positionControl(SERVO_ID, 554 + position, 60, BLED_ON);
                     wait(1);
                     sv.getStatus(SERVO_ID);
                     position = sv.getPos(SERVO_ID);
                  } 
             }
             //filling button +10  
            if (TS_State.touchDetected&(380<x & x<430)&(140<y & y<165)) {      
                 lcd.FillRect(380,140,50,25);
                 position = sv.getPos(SERVO_ID); 
                 if( position <= 972)
                 {    sv.positionControl(SERVO_ID, 30 + position, 60, BLED_ON);
                     wait(1);
                     sv.getStatus(SERVO_ID);
                     position = sv.getPos(SERVO_ID);
                  } 
             }
             //filling button -10  
            if (TS_State.touchDetected&(380<x & x<430)&(190<y & y<215)) {      
                lcd.FillRect(380,190,50,25);
                position = sv.getPos(SERVO_ID); 
                 if( position >= 51)
                 {    sv.positionControl(SERVO_ID, position - 30, 60, BLED_ON);
                     wait(1);
                     sv.getStatus(SERVO_ID);
                     position = sv.getPos(SERVO_ID);
                  } 
             }
             
             //filling circle
             if (TS_State.touchDetected&(140<x & x<340)&(36<y & y<236)) {      
                lcd.FillCircle(240,136,100);

                sv.positionControl(SERVO_ID, 1002-(((x-140)*5)), 60, BLED_ON);
                wait(1);
                sv.getStatus(SERVO_ID);
                position = sv.getPos(SERVO_ID);
             }
                           
             //init=0;       
            
            
            //finger follower
            /*if (TS_State.touchDetected&(0<x & x<480)&(0<y & y<272)) {      
             lcd.FillCircle(x, y, 10);
            }*/
          
            //lcd.DrawPixel(TS_State.touchX[0], TS_State.touchY[0], LCD_COLOR_WHITE);
        } else { //when no finger detected
            if (!cleared) {
                lcd.Clear(LCD_COLOR_WHITE);
                sprintf((char*)text, "Alim Voltage: 7.4 V");
                lcd.DisplayStringAt(0, LINE(5), (uint8_t *)&text, LEFT_MODE);
                sprintf((char*)text, "Current MAX: 450 mA");
                lcd.DisplayStringAt(0, LINE(6), (uint8_t *)&text, LEFT_MODE);
                //title display
                lcd.DisplayStringAt(0, LINE(1), (uint8_t *)"HerkuleX Servo Controler", CENTER_MODE);
                //name display
                sprintf((char*)nom, "Yann MARTIN");
                lcd.DisplayStringAt(0, LINE(17), (uint8_t *)&nom, LEFT_MODE);
                sprintf((char*)nom, "Karl BISWESE");
                lcd.DisplayStringAt(0, LINE(18), (uint8_t *)&nom, LEFT_MODE);
                sprintf((char*)nom, "Genath NAGENTHIRAN");
                lcd.DisplayStringAt(0, LINE(19), (uint8_t *)&nom, LEFT_MODE);
                cleared = 1;
            }
            
        }
        
       
    }
}