Diff: main.cpp

Revision:

0:4a219a0d6583

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Sep 26 16:23:36 2017 +0000
@@ -0,0 +1,340 @@
+#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);
+                            }
+                            */
+             //}
+      
+
+}