Basic DC motor control test, rpm feedback by simple impulse signal, PID speed control.
Dependencies: FastPWM mbed FastIO MODSERIAL
Diff: main.cpp
- Revision:
- 7:1aba48efb1c3
- Parent:
- 6:cc38171e6a4b
- Child:
- 8:5ce5fe1ce503
--- 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)