File content as of revision 0:4a219a0d6583:

#include "mbed.h"
#include "TS_DISCO_F429ZI.h"
#include "LCD_DISCO_F429ZI.h"
#include "Motor.h"
#include "PID.h"

LCD_DISCO_F429ZI lcd;
TS_DISCO_F429ZI ts;

PwmOut right_sk(PE_5);
PwmOut left_sk(PE_6);

DigitalOut right_en(PE_4); //Enable
DigitalOut right_fr(PE_3); //Direction
DigitalOut right_bk(PE_2); //Break

DigitalOut left_en(PD_2); //Enable
DigitalOut left_fr(PD_4); //Direction
DigitalOut left_bk(PD_5); //Break

 
//Serial pc(PG_14, PG_9, 115200); // tx, rx
//Serial pc(PC_12, PD_2, 115200); // tx, rx

DigitalOut led1(LED1);
char buffer[20];
char buffer2[20];

class Counter {
public:
    Counter(PinName pin) : _interrupt(pin) {        // create the InterruptIn on the pin specified to Counter
        _interrupt.fall(this, &Counter::increment); // attach increment function of this counter instance
    }
 
    void increment() {
        _count++;
    }
 
    long read() {
        return _count;
    }
    
    void zero(){
        _count = 0;
    }
 
private:
    InterruptIn _interrupt;
    volatile long _count;
};

 
Counter counterLeft(PC_13);
Counter counterRight(PC_12);



Motor leftMotor(PE_5, PB_3, PB_3);  //pwm, inB, inA
Motor rightMotor(PE_6, PB_4, PB_4); //pwm, inA, inB
PID leftPid(0.4, 0.1, 0.0, 0.01);  //Kc, Ti, Td
PID rightPid(0.4, 0.1, 0.0, 0.01); //Kc, Ti, Td

//-------------------------------

   


int main()
{
    TS_StateTypeDef TS_State;
    uint16_t x, y;
    uint8_t text[30];
    uint8_t status;
  
    BSP_LCD_SetFont(&Font20);
  
    lcd.DisplayStringAt(0, LINE(5), (uint8_t *)"TOUCHSCREEN", CENTER_MODE);
    lcd.DisplayStringAt(0, LINE(6), (uint8_t *)"DEMO", CENTER_MODE);
    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", CENTER_MODE);
      lcd.DisplayStringAt(0, LINE(6), (uint8_t *)"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", CENTER_MODE);
      lcd.DisplayStringAt(0, LINE(6), (uint8_t *)"INIT OK", CENTER_MODE);
    }
    
    wait(1);
    lcd.Clear(LCD_COLOR_BLUE);
    lcd.SetBackColor(LCD_COLOR_BLUE);
    lcd.SetTextColor(LCD_COLOR_WHITE);
   
   /* 
    do1 = 0;
    do2 = 1;
    do3 = 1;
    //do1 = 0;
    pwm1.period(1.0/10000);      // 4 second period
    pwm1.write(0.50f);      // 50% duty cycle, relative to period
    
    while(1)
    {
      
      ts.GetState(&TS_State);      
      if (TS_State.TouchDetected)
      {
        x = TS_State.X;
        y = TS_State.Y;
        sprintf((char*)text, "x=%d y=%d   ", x, y);
        lcd.DisplayStringAt(0, LINE(0), (uint8_t *)&text, LEFT_MODE);
        if (x <= 120) { do2 = 0;} else { do2 = 1;}
        float k = float(y)/320.00f;
         pwm1.write(k); 
      }
      sprintf((char*)text, "c=%d   ", counter.read());
        lcd.DisplayStringAt(0, LINE(1), (uint8_t *)&text, LEFT_MODE);
      }
      */
      
    //right_en = 0;
    //left_en = 0; 
    right_bk = 1;
    right_en = 0;
    //right_fr = 1;
    //right_sk.period(0.00005);
    //right_sk.write(0.50f);
    left_bk = 1;
    left_en = 0;
    //left_fr = 1;
    //left_sk.period(0.00005);
    //left_sk.write(0.50f); 
    leftMotor.period(0.00005);  //Set motor PWM periods to 20KHz.
    rightMotor.period(0.00005);

    //Input and output limits have been determined
    //empirically with the specific motors being used.
    //Change appropriately for different motors.
    //Input  units: counts per second.
    //Output units: PwmOut duty cycle as %.
    //Default limits are for moving forward.
    leftPid.setInputLimits(0, 1500);
    leftPid.setOutputLimits(0.2, 0.9);
    leftPid.setMode(AUTO_MODE);
    rightPid.setInputLimits(0, 1500);
    rightPid.setOutputLimits(0.2, 0.9);
    rightPid.setMode(AUTO_MODE);

    
    long leftPulses      = 0; //How far the left wheel has travelled.
    long leftPrevPulses  = 0; //The previous reading of how far the left wheel
    //has travelled.
    float leftVelocity  = 0.0; //The velocity of the left wheel in pulses per
    //second.
    long rightPulses     = 0; //How far the right wheel has travelled.
    long rightPrevPulses = 0; //The previous reading of how far the right wheel
    //has travelled.
    float rightVelocity = 0.0; //The velocity of the right wheel in pulses per
    //second.
    long distance     = 3000; //Number of pulses to travel.
    //while(1){
     right_bk = 1;
    left_bk = 1;
    leftPulses      = 0; //How far the left wheel has travelled.
    leftPrevPulses  = 0; //The previous reading of how far the left wheel
    //has travelled.
    leftVelocity  = 0.0; //The velocity of the left wheel in pulses per
    //second.
    rightPulses     = 0; //How far the right wheel has travelled.
    rightPrevPulses = 0; //The previous reading of how far the right wheel
    //has travelled.
    rightVelocity = 0.0; //The velocity of the right wheel in pulses per
     counterLeft.zero();
     counterRight.zero();
    distance     = 3000; //Number of pulses to travel.
    right_fr = 1;
    left_fr = 0;
    
    //wait(5); //Wait a few seconds before we start moving.

    //Velocity to mantain in pulses per second.
    leftPid.setSetPoint(1000);
    rightPid.setSetPoint(1000);
    

    while ((leftPulses < distance) || (rightPulses < distance)) {

        //Get the current pulse count and subtract the previous one to
        //calculate the current velocity in counts per second.
        leftPulses = counterLeft.read();
        leftVelocity = (leftPulses - leftPrevPulses) / 0.01;
        leftPrevPulses = leftPulses;
        //Use the absolute value of velocity as the PID controller works
        //in the % (unsigned) domain and will get confused with -ve values.
        leftPid.setProcessValue(fabs(leftVelocity));
        leftMotor.speed(leftPid.compute());

        rightPulses = counterRight.read();
        rightVelocity = (rightPulses - rightPrevPulses) / 0.01;
        rightPrevPulses = rightPulses;
        rightPid.setProcessValue(fabs(rightVelocity));
        rightMotor.speed(rightPid.compute());

        wait(0.01);
        sprintf((char*)text, "left=%d   ", leftPulses);
        lcd.DisplayStringAt(0, LINE(0), (uint8_t *)&text, LEFT_MODE);
        sprintf((char*)text, "right=%d   ", rightPulses);
        lcd.DisplayStringAt(0, LINE(1), (uint8_t *)&text, LEFT_MODE);

    }

    leftMotor.brake();
    rightMotor.brake();
    
    
    
    
    
      right_bk = 0;
    left_bk = 0;
    
    
    /*
     right_bk = 1;
    left_bk = 1;
    leftPulses      = 0; //How far the left wheel has travelled.
    leftPrevPulses  = 0; //The previous reading of how far the left wheel
    //has travelled.
    leftVelocity  = 0.0; //The velocity of the left wheel in pulses per
    //second.
    rightPulses     = 0; //How far the right wheel has travelled.
    rightPrevPulses = 0; //The previous reading of how far the right wheel
    //has travelled.
    rightVelocity = 0.0; //The velocity of the right wheel in pulses per
     counterLeft.zero();
     counterRight.zero();
    distance     = 30000; //Number of pulses to travel.
    
    right_fr = 1;
    left_fr = 1;
    
    //wait(5); //Wait a few seconds before we start moving.

    //Velocity to mantain in pulses per second.
    leftPid.setSetPoint(100000);
    rightPid.setSetPoint(100000);
    

    while ((leftPulses < distance) || (rightPulses < distance)) {

        //Get the current pulse count and subtract the previous one to
        //calculate the current velocity in counts per second.
        leftPulses = counterLeft.read();
        leftVelocity = (leftPulses - leftPrevPulses) / 0.01;
        leftPrevPulses = leftPulses;
        //Use the absolute value of velocity as the PID controller works
        //in the % (unsigned) domain and will get confused with -ve values.
        leftPid.setProcessValue(fabs(leftVelocity));
        leftMotor.speed(leftPid.compute());

        rightPulses = counterRight.read();
        rightVelocity = (rightPulses - rightPrevPulses) / 0.01;
        rightPrevPulses = rightPulses;
        rightPid.setProcessValue(fabs(rightVelocity));
        rightMotor.speed(rightPid.compute());

        wait(0.01);
        sprintf((char*)text, "left=%d   ", leftPulses);
        lcd.DisplayStringAt(0, LINE(0), (uint8_t *)&text, LEFT_MODE);
        sprintf((char*)text, "right=%d   ", rightPulses);
        lcd.DisplayStringAt(0, LINE(1), (uint8_t *)&text, LEFT_MODE);

    }

    leftMotor.brake();
    rightMotor.brake();
      right_bk = 0;
    left_bk = 0;
    
    
    
    */
    
    
    
    //}
    
    
    
     /* while(1){
         sprintf((char*)text, "left=%d   ", counterLeft.read());
        lcd.DisplayStringAt(0, LINE(0), (uint8_t *)&text, LEFT_MODE);
        sprintf((char*)text, "right=%d   ", counterRight.read());
       lcd.DisplayStringAt(0, LINE(1), (uint8_t *)&text, LEFT_MODE);
         
         }*/
         
      //lcd.Clear(LCD_COLOR_BLUE);
      //lcd.SetBackColor(LCD_COLOR_BLUE);
      //lcd.SetTextColor(LCD_COLOR_WHITE);
      //wait(0.3);

    //led = 0.5f;          // shorthand for led.write()
    //led.pulsewidth(2);   // alternative to led.write, set duty cycle time in seconds
      
      
      /*gyro.GetXYZ(GyroBuffer);
      int ret = snprintf(buffer2, sizeof buffer2, "    %.1f     ", GyroBuffer[0]);
      lcd.DisplayStringAt(0, LINE(3), (uint8_t *)buffer2, CENTER_MODE);
      ret = snprintf(buffer2, sizeof buffer2, "    %.1f     ", GyroBuffer[1]);
      lcd.DisplayStringAt(0, LINE(4), (uint8_t *)buffer2, CENTER_MODE);
      ret = snprintf(buffer2, sizeof buffer2, "    %.1f     ", GyroBuffer[2]);
      lcd.DisplayStringAt(0, LINE(5), (uint8_t *)buffer2, CENTER_MODE);*/
      
      
      //wait(1);
        //pc.printf("Hello World!\n");
           // while(1) {
                                    
                   /* if (pc.readable())
                            {
                                pc.scanf("%s", &buffer);
                                pc.printf("%s\r\n", &buffer);
                            }
                            */
             //}
      

}