Revision 7:1aba48efb1c3, committed 2018-03-28

Comitter:

dzoni

Date:

Wed Mar 28 09:32:05 2018 +0000

Parent:

6:cc38171e6a4b

Child:

8:5ce5fe1ce503

Commit message:

Added PID controller to control motor speed (RPM). Compiles OK, ready for functional test.

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.lib	Wed Mar 28 09:32:05 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/aberk/code/PID/#125d04fbce1d

--- a/main.cpp	Tue Mar 27 21:24:35 2018 +0000
+++ b/main.cpp	Wed Mar 28 09:32:05 2018 +0000
@@ -1,6 +1,7 @@
 #include "mbed.h"
 #include "FastPWM.h"
 #include "FastIO.h"
+#include "PID.h"
 
 #define MODSERIAL_DEFAULT_RX_BUFFER_SIZE 16
 #define MODSERIAL_DEFAULT_TX_BUFFER_SIZE 64 
@@ -9,41 +10,49 @@
 #define IMPULSE_SENSOR_R_PIN (PA_9)
 #define PWM_OUT_R_PIN (PA_6)
 
-FastPWM mypwm(PWM_OUT_R_PIN);
-
-FastIn<IMPULSE_SENSOR_R_PIN> pinImpulseSensorIn;
-FastIn<USER_BUTTON> pinUserButtonIn;
-
-DigitalOut myled(LED1);
-
-Timer   myTimer;
-
 //------------------------------------
 // Hyperterminal configuration
 // 9600 bauds, 8-bit data, no parity
 //------------------------------------
 MODSERIAL pcLink(SERIAL_TX, SERIAL_RX);
 
-static const us_timestamp_t  periodImpSens  = 125000;   // 125 msec
-static const us_timestamp_t  periodLEDBlink = 100000;   // 100 msec
-static const us_timestamp_t  periodPWMWrite = 5000000;  // 5 sec
+static const us_timestamp_t periodImpSens  =      125000; // 125 msec
+static const us_timestamp_t periodLEDBlink =      100000; // 100 msec
+static const us_timestamp_t periodPWMWrite =      250000; // 250 msec
+static const us_timestamp_t periodRPMSetpoint = 10000000; // 10 sec
 
-static us_timestamp_t  tStampImpSens  = 0;
-static us_timestamp_t  tStampLEDBlink = 0;
-static us_timestamp_t  tStampPWMWrite = 0;
-static us_timestamp_t  tStamp = 0;
+static us_timestamp_t tStampImpSens  = 0;
+static us_timestamp_t tStampLEDBlink = 0;
+static us_timestamp_t tStampPWMWrite = 0;
+static us_timestamp_t tStampRPMSetpoint = 0;
+
+static us_timestamp_t tStamp = 0;
 
 static unsigned int uiImpSens = 0U;
 static unsigned int uiImpSensTemp = 0U;
 static int          iImpSensLastState = 0;
-static double       dPwmDuty = 0.0;
+static float       fPwmDuty = 0.0;
+static float fRPMSetpoint = 0.0f;
 
 static void setup(void);
 static void tskImpSens(void);
 static void tskLEDBlink(void);
 static void tskPWMWrite(void);
+static void tskRPMSetpoint(void);
 static void tskBackground(void);
 
+FastPWM mypwm(PWM_OUT_R_PIN);
+
+FastIn<IMPULSE_SENSOR_R_PIN> pinImpulseSensorIn;
+FastIn<USER_BUTTON> pinUserButtonIn;
+
+PID pid_RPM_Right_motor(1.0f, 0.0f, 0.0f, (((float)periodPWMWrite)/1000000.0f));
+
+DigitalOut myled(LED1);
+
+Timer   myTimer;
+
+
 static inline void DO_TASK(us_timestamp_t tskPeriod, us_timestamp_t &tskTimer, us_timestamp_t timeStamp, void (*tskFunction)(void))
 {
     if (tskPeriod < (timeStamp - tskTimer))
@@ -67,9 +76,10 @@
     {
         tStamp = myTimer.read_high_resolution_us();
  
-        DO_TASK(periodLEDBlink, tStampLEDBlink, tStamp, &tskLEDBlink);
-        DO_TASK(periodPWMWrite, tStampPWMWrite, tStamp, &tskPWMWrite);
-        DO_TASK(periodImpSens,  tStampImpSens,  tStamp, &tskImpSens);
+        DO_TASK(periodLEDBlink,     tStampLEDBlink,     tStamp, &tskLEDBlink);
+        DO_TASK(periodPWMWrite,     tStampPWMWrite,     tStamp, &tskPWMWrite);
+        DO_TASK(periodImpSens,      tStampImpSens,      tStamp, &tskImpSens);
+        DO_TASK(periodRPMSetpoint,  tStampRPMSetpoint,  tStamp, &tskRPMSetpoint);
 
         BACKGROUND(&tskBackground);
     }
@@ -85,6 +95,19 @@
     mypwm.write(0.5);
   
     myTimer.start();
+    
+    //Analog input from 0.0 to 100.0 impulses per measurement period
+    pid_RPM_Right_motor.setInputLimits(0.0f, 100.0f);
+    
+    //Pwm output from 0.0 to 1.0
+    pid_RPM_Right_motor.setOutputLimits(0.0f, 1.0f);
+    
+    //If there's a bias.
+    pid_RPM_Right_motor.setBias(0.0f);
+    pid_RPM_Right_motor.setMode(AUTO_MODE);
+    
+    //We want the process variable to be 0.0 RPM
+    pid_RPM_Right_motor.setSetPoint(0.0f);
 }
 
 void tskImpSens(void)
@@ -102,16 +125,35 @@
 
 void tskPWMWrite(void)
 {
-    dPwmDuty = dPwmDuty + 0.1;
+//    fPwmDuty = fPwmDuty + 0.1;
             
-    if (1.0 < dPwmDuty)
+//    if (1.0 < fPwmDuty)
+//    {
+//        fPwmDuty = 0.0;
+//    }
+
+    //Update the process variable.
+    pid_RPM_Right_motor.setProcessValue((float)uiImpSens);
+
+    //Set the new output.
+    fPwmDuty = pid_RPM_Right_motor.compute();
+
+    mypwm.write(fPwmDuty);
+
+    pcLink.printf("PWM: %.2f %%\tIMP: %u imp.\tSET: %.2f imp.\t\r", mypwm.read() * 100, uiImpSens, fRPMSetpoint);
+
+//    pcLink.printf("\r\nPWM: %.2f %%     \r\n", mypwm.read() * 100);
+}
+
+void tskRPMSetpoint(void)
+{
+    fRPMSetpoint += 10.0f;
+    if (100.0f < fRPMSetpoint)
     {
-        dPwmDuty = 0.0;
+        fRPMSetpoint = 0.0f;
     }
 
-    mypwm.write(dPwmDuty);
-
-    pcLink.printf("\r\nPWM: %.2f %%     \r\n", mypwm.read() * 100);
+    pid_RPM_Right_motor.setSetPoint(fRPMSetpoint);
 }
 
 void tskBackground(void)