Basic DC motor control test, rpm feedback by simple impulse signal, PID speed control.
Dependencies: FastPWM mbed FastIO MODSERIAL
main.cpp@11:4747badb2448, 2018-04-04 (annotated)
- Committer:
- dzoni
- Date:
- Wed Apr 04 05:58:25 2018 +0000
- Revision:
- 11:4747badb2448
- Parent:
- 10:c28d133a1408
After functionality test on actual HW. Fine tunning of PID controller parameters required. Functionality OK. Controller slow on start from zero speed.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
dzoni | 8:5ce5fe1ce503 | 1 | /** |
dzoni | 8:5ce5fe1ce503 | 2 | * @brief Basic DC motor control test, rpm feedback by simple impulse signal, PID speed control. |
dzoni | 8:5ce5fe1ce503 | 3 | * @file main.cpp |
dzoni | 8:5ce5fe1ce503 | 4 | * @author Jan Tetour <jan.tetour@gmail.com> |
dzoni | 8:5ce5fe1ce503 | 5 | * |
dzoni | 8:5ce5fe1ce503 | 6 | * Test application for STM32F4 for small DC motor control. Main specifications: |
dzoni | 8:5ce5fe1ce503 | 7 | * - DC motor controlled by PWM |
dzoni | 8:5ce5fe1ce503 | 8 | * - Motor driver used L298N |
dzoni | 8:5ce5fe1ce503 | 9 | * - RPM evaluated via simple impulse sensor |
dzoni | 8:5ce5fe1ce503 | 10 | * - Speed (RPM) controlled by PID controller |
dzoni | 8:5ce5fe1ce503 | 11 | */ |
dzoni | 8:5ce5fe1ce503 | 12 | |
dzoni | 0:bd186184ef2a | 13 | #include "mbed.h" |
dzoni | 8:5ce5fe1ce503 | 14 | |
dzoni | 1:70c514e10598 | 15 | #include "FastPWM.h" |
dzoni | 2:70918f7f8451 | 16 | #include "FastIO.h" |
dzoni | 8:5ce5fe1ce503 | 17 | |
dzoni | 7:1aba48efb1c3 | 18 | #include "PID.h" |
dzoni | 0:bd186184ef2a | 19 | |
dzoni | 8:5ce5fe1ce503 | 20 | // Define MODSERIAL buffer sizes |
dzoni | 4:7cb8986200a7 | 21 | #define MODSERIAL_DEFAULT_RX_BUFFER_SIZE 16 |
dzoni | 4:7cb8986200a7 | 22 | #define MODSERIAL_DEFAULT_TX_BUFFER_SIZE 64 |
dzoni | 4:7cb8986200a7 | 23 | #include "MODSERIAL.h" |
dzoni | 4:7cb8986200a7 | 24 | |
dzoni | 8:5ce5fe1ce503 | 25 | // Pin defintions |
dzoni | 8:5ce5fe1ce503 | 26 | #define IMPULSE_SENSOR_R_PIN (PA_9) |
dzoni | 8:5ce5fe1ce503 | 27 | #define PWM_OUT_R_PIN (PA_6) |
dzoni | 4:7cb8986200a7 | 28 | |
dzoni | 8:5ce5fe1ce503 | 29 | // Serial port definitions |
dzoni | 4:7cb8986200a7 | 30 | MODSERIAL pcLink(SERIAL_TX, SERIAL_RX); |
dzoni | 4:7cb8986200a7 | 31 | |
dzoni | 8:5ce5fe1ce503 | 32 | // Tasks timming definitions |
dzoni | 7:1aba48efb1c3 | 33 | static const us_timestamp_t periodImpSens = 125000; // 125 msec |
dzoni | 7:1aba48efb1c3 | 34 | static const us_timestamp_t periodLEDBlink = 100000; // 100 msec |
dzoni | 7:1aba48efb1c3 | 35 | static const us_timestamp_t periodPWMWrite = 250000; // 250 msec |
dzoni | 7:1aba48efb1c3 | 36 | static const us_timestamp_t periodRPMSetpoint = 10000000; // 10 sec |
dzoni | 4:7cb8986200a7 | 37 | |
dzoni | 7:1aba48efb1c3 | 38 | static us_timestamp_t tStampImpSens = 0; |
dzoni | 7:1aba48efb1c3 | 39 | static us_timestamp_t tStampLEDBlink = 0; |
dzoni | 7:1aba48efb1c3 | 40 | static us_timestamp_t tStampPWMWrite = 0; |
dzoni | 7:1aba48efb1c3 | 41 | static us_timestamp_t tStampRPMSetpoint = 0; |
dzoni | 7:1aba48efb1c3 | 42 | |
dzoni | 7:1aba48efb1c3 | 43 | static us_timestamp_t tStamp = 0; |
dzoni | 4:7cb8986200a7 | 44 | |
dzoni | 8:5ce5fe1ce503 | 45 | static Timer myTimer; |
dzoni | 8:5ce5fe1ce503 | 46 | |
dzoni | 8:5ce5fe1ce503 | 47 | // RPM Sensor module level variables |
dzoni | 4:7cb8986200a7 | 48 | static unsigned int uiImpSens = 0U; |
dzoni | 5:ec4d6e435822 | 49 | static unsigned int uiImpSensTemp = 0U; |
dzoni | 5:ec4d6e435822 | 50 | static int iImpSensLastState = 0; |
dzoni | 8:5ce5fe1ce503 | 51 | |
dzoni | 8:5ce5fe1ce503 | 52 | // PWM Generator module level variables |
dzoni | 8:5ce5fe1ce503 | 53 | static float fPwmDuty = 0.0f; |
dzoni | 8:5ce5fe1ce503 | 54 | |
dzoni | 8:5ce5fe1ce503 | 55 | // RPM Controller module level variables |
dzoni | 7:1aba48efb1c3 | 56 | static float fRPMSetpoint = 0.0f; |
dzoni | 2:70918f7f8451 | 57 | |
dzoni | 8:5ce5fe1ce503 | 58 | // LOCAL MODULE VARIABLES |
dzoni | 8:5ce5fe1ce503 | 59 | // I/O pins related |
dzoni | 8:5ce5fe1ce503 | 60 | FastPWM mypwm(PWM_OUT_R_PIN); |
dzoni | 8:5ce5fe1ce503 | 61 | FastIn<IMPULSE_SENSOR_R_PIN> pinImpulseSensorIn; |
dzoni | 8:5ce5fe1ce503 | 62 | FastIn<USER_BUTTON> pinUserButtonIn; |
dzoni | 8:5ce5fe1ce503 | 63 | DigitalOut myled(LED1); |
dzoni | 8:5ce5fe1ce503 | 64 | |
dzoni | 8:5ce5fe1ce503 | 65 | // Controllers |
dzoni | 11:4747badb2448 | 66 | PID pid_RPM_Right_motor(0.00175f, 0.00200f, 0.000f, (((float)periodPWMWrite)/1000000.0f)); |
dzoni | 8:5ce5fe1ce503 | 67 | |
dzoni | 8:5ce5fe1ce503 | 68 | // LOCAL FUNCTION DECLARATIONS |
dzoni | 8:5ce5fe1ce503 | 69 | // Task worker functions |
dzoni | 5:ec4d6e435822 | 70 | static void tskImpSens(void); |
dzoni | 5:ec4d6e435822 | 71 | static void tskLEDBlink(void); |
dzoni | 5:ec4d6e435822 | 72 | static void tskPWMWrite(void); |
dzoni | 7:1aba48efb1c3 | 73 | static void tskRPMSetpoint(void); |
dzoni | 5:ec4d6e435822 | 74 | static void tskBackground(void); |
dzoni | 5:ec4d6e435822 | 75 | |
dzoni | 8:5ce5fe1ce503 | 76 | // Inititalization |
dzoni | 8:5ce5fe1ce503 | 77 | static void setup(void); |
dzoni | 7:1aba48efb1c3 | 78 | |
dzoni | 8:5ce5fe1ce503 | 79 | // Task management functions |
dzoni | 5:ec4d6e435822 | 80 | static inline void DO_TASK(us_timestamp_t tskPeriod, us_timestamp_t &tskTimer, us_timestamp_t timeStamp, void (*tskFunction)(void)) |
dzoni | 5:ec4d6e435822 | 81 | { |
dzoni | 5:ec4d6e435822 | 82 | if (tskPeriod < (timeStamp - tskTimer)) |
dzoni | 5:ec4d6e435822 | 83 | { |
dzoni | 5:ec4d6e435822 | 84 | tskTimer = timeStamp; |
dzoni | 5:ec4d6e435822 | 85 | (*tskFunction)(); |
dzoni | 5:ec4d6e435822 | 86 | } |
dzoni | 5:ec4d6e435822 | 87 | } |
dzoni | 5:ec4d6e435822 | 88 | |
dzoni | 5:ec4d6e435822 | 89 | static inline void BACKGROUND(void (*tskFunction)(void)) |
dzoni | 5:ec4d6e435822 | 90 | { |
dzoni | 5:ec4d6e435822 | 91 | (*tskFunction)(); |
dzoni | 5:ec4d6e435822 | 92 | } |
dzoni | 4:7cb8986200a7 | 93 | |
dzoni | 8:5ce5fe1ce503 | 94 | // Main function definition |
dzoni | 5:ec4d6e435822 | 95 | int main(void) |
dzoni | 5:ec4d6e435822 | 96 | { |
dzoni | 5:ec4d6e435822 | 97 | setup(); |
dzoni | 5:ec4d6e435822 | 98 | |
dzoni | 5:ec4d6e435822 | 99 | while(1) |
dzoni | 5:ec4d6e435822 | 100 | { |
dzoni | 5:ec4d6e435822 | 101 | tStamp = myTimer.read_high_resolution_us(); |
dzoni | 5:ec4d6e435822 | 102 | |
dzoni | 7:1aba48efb1c3 | 103 | DO_TASK(periodLEDBlink, tStampLEDBlink, tStamp, &tskLEDBlink); |
dzoni | 7:1aba48efb1c3 | 104 | DO_TASK(periodImpSens, tStampImpSens, tStamp, &tskImpSens); |
dzoni | 7:1aba48efb1c3 | 105 | DO_TASK(periodRPMSetpoint, tStampRPMSetpoint, tStamp, &tskRPMSetpoint); |
dzoni | 10:c28d133a1408 | 106 | DO_TASK(periodPWMWrite, tStampPWMWrite, tStamp, &tskPWMWrite); |
dzoni | 5:ec4d6e435822 | 107 | |
dzoni | 5:ec4d6e435822 | 108 | BACKGROUND(&tskBackground); |
dzoni | 5:ec4d6e435822 | 109 | } |
dzoni | 5:ec4d6e435822 | 110 | } |
dzoni | 5:ec4d6e435822 | 111 | |
dzoni | 5:ec4d6e435822 | 112 | |
dzoni | 8:5ce5fe1ce503 | 113 | // LOCAL MODULE DEFINITIONS |
dzoni | 10:c28d133a1408 | 114 | /** |
dzoni | 10:c28d133a1408 | 115 | * @brief System initialization - called at the prologue of main(). |
dzoni | 10:c28d133a1408 | 116 | * @note None. |
dzoni | 10:c28d133a1408 | 117 | * |
dzoni | 10:c28d133a1408 | 118 | * Caries out system level initialization at the beginning of the main(). |
dzoni | 10:c28d133a1408 | 119 | */ |
dzoni | 5:ec4d6e435822 | 120 | void setup(void) |
dzoni | 5:ec4d6e435822 | 121 | { |
dzoni | 4:7cb8986200a7 | 122 | pcLink.baud(115200); |
dzoni | 4:7cb8986200a7 | 123 | pcLink.format(8, SerialBase::None, 1); |
dzoni | 4:7cb8986200a7 | 124 | |
dzoni | 11:4747badb2448 | 125 | mypwm.period_us(3500); |
dzoni | 11:4747badb2448 | 126 | mypwm.write(0.0); |
dzoni | 0:bd186184ef2a | 127 | |
dzoni | 4:7cb8986200a7 | 128 | myTimer.start(); |
dzoni | 7:1aba48efb1c3 | 129 | |
dzoni | 7:1aba48efb1c3 | 130 | //Analog input from 0.0 to 100.0 impulses per measurement period |
dzoni | 11:4747badb2448 | 131 | pid_RPM_Right_motor.setInputLimits(16.0f, 35.0f); |
dzoni | 7:1aba48efb1c3 | 132 | |
dzoni | 7:1aba48efb1c3 | 133 | //Pwm output from 0.0 to 1.0 |
dzoni | 11:4747badb2448 | 134 | pid_RPM_Right_motor.setOutputLimits(0.05f, 1.0f); |
dzoni | 7:1aba48efb1c3 | 135 | |
dzoni | 7:1aba48efb1c3 | 136 | //If there's a bias. |
dzoni | 7:1aba48efb1c3 | 137 | pid_RPM_Right_motor.setBias(0.0f); |
dzoni | 7:1aba48efb1c3 | 138 | pid_RPM_Right_motor.setMode(AUTO_MODE); |
dzoni | 7:1aba48efb1c3 | 139 | |
dzoni | 7:1aba48efb1c3 | 140 | //We want the process variable to be 0.0 RPM |
dzoni | 11:4747badb2448 | 141 | fRPMSetpoint = 16.0f; |
dzoni | 11:4747badb2448 | 142 | pid_RPM_Right_motor.setSetPoint(fRPMSetpoint); |
dzoni | 5:ec4d6e435822 | 143 | } |
dzoni | 5:ec4d6e435822 | 144 | |
dzoni | 8:5ce5fe1ce503 | 145 | // Task worker functions definitions |
dzoni | 8:5ce5fe1ce503 | 146 | /** |
dzoni | 8:5ce5fe1ce503 | 147 | * @brief RPM calculation. |
dzoni | 8:5ce5fe1ce503 | 148 | * @note Needs refactoring to implement #ifdef for 2 different implementation (with/without PID). |
dzoni | 8:5ce5fe1ce503 | 149 | * |
dzoni | 8:5ce5fe1ce503 | 150 | * Stores impulse count per measurement period and clears impulse counter. |
dzoni | 8:5ce5fe1ce503 | 151 | */ |
dzoni | 5:ec4d6e435822 | 152 | void tskImpSens(void) |
dzoni | 5:ec4d6e435822 | 153 | { |
dzoni | 5:ec4d6e435822 | 154 | uiImpSens = uiImpSensTemp; |
dzoni | 5:ec4d6e435822 | 155 | uiImpSensTemp = 0U; |
dzoni | 4:7cb8986200a7 | 156 | |
dzoni | 8:5ce5fe1ce503 | 157 | // pcLink.printf("IMP: %u imp. \r", uiImpSens); |
dzoni | 5:ec4d6e435822 | 158 | } |
dzoni | 4:7cb8986200a7 | 159 | |
dzoni | 8:5ce5fe1ce503 | 160 | /** |
dzoni | 8:5ce5fe1ce503 | 161 | * @brief User LED flashing. |
dzoni | 8:5ce5fe1ce503 | 162 | * |
dzoni | 8:5ce5fe1ce503 | 163 | * Implements User LED flashing. |
dzoni | 8:5ce5fe1ce503 | 164 | */ |
dzoni | 5:ec4d6e435822 | 165 | void tskLEDBlink(void) |
dzoni | 5:ec4d6e435822 | 166 | { |
dzoni | 5:ec4d6e435822 | 167 | myled = !myled; |
dzoni | 5:ec4d6e435822 | 168 | } |
dzoni | 1:70c514e10598 | 169 | |
dzoni | 8:5ce5fe1ce503 | 170 | /** |
dzoni | 8:5ce5fe1ce503 | 171 | * @brief Writes new duty cycle value into PWM generator. |
dzoni | 8:5ce5fe1ce503 | 172 | * @note Needs refactoring to implement #ifdef for 2 different implementation (with/without PID). |
dzoni | 8:5ce5fe1ce503 | 173 | * @warning Not finished. |
dzoni | 8:5ce5fe1ce503 | 174 | * |
dzoni | 8:5ce5fe1ce503 | 175 | * Calculates new dyty cycle and writes the value into PWM generator. |
dzoni | 8:5ce5fe1ce503 | 176 | */ |
dzoni | 5:ec4d6e435822 | 177 | void tskPWMWrite(void) |
dzoni | 5:ec4d6e435822 | 178 | { |
dzoni | 7:1aba48efb1c3 | 179 | // fPwmDuty = fPwmDuty + 0.1; |
dzoni | 4:7cb8986200a7 | 180 | |
dzoni | 7:1aba48efb1c3 | 181 | // if (1.0 < fPwmDuty) |
dzoni | 7:1aba48efb1c3 | 182 | // { |
dzoni | 7:1aba48efb1c3 | 183 | // fPwmDuty = 0.0; |
dzoni | 7:1aba48efb1c3 | 184 | // } |
dzoni | 7:1aba48efb1c3 | 185 | |
dzoni | 7:1aba48efb1c3 | 186 | //Update the process variable. |
dzoni | 7:1aba48efb1c3 | 187 | pid_RPM_Right_motor.setProcessValue((float)uiImpSens); |
dzoni | 7:1aba48efb1c3 | 188 | |
dzoni | 7:1aba48efb1c3 | 189 | //Set the new output. |
dzoni | 7:1aba48efb1c3 | 190 | fPwmDuty = pid_RPM_Right_motor.compute(); |
dzoni | 7:1aba48efb1c3 | 191 | |
dzoni | 7:1aba48efb1c3 | 192 | mypwm.write(fPwmDuty); |
dzoni | 7:1aba48efb1c3 | 193 | |
dzoni | 11:4747badb2448 | 194 | pcLink.printf("PWM: %.2f %%\tIMP: %.2f imp.\tSET: %.2f imp.\t\r", mypwm.read() * 100, (float)uiImpSens, fRPMSetpoint); |
dzoni | 7:1aba48efb1c3 | 195 | |
dzoni | 7:1aba48efb1c3 | 196 | // pcLink.printf("\r\nPWM: %.2f %% \r\n", mypwm.read() * 100); |
dzoni | 7:1aba48efb1c3 | 197 | } |
dzoni | 7:1aba48efb1c3 | 198 | |
dzoni | 8:5ce5fe1ce503 | 199 | /** |
dzoni | 8:5ce5fe1ce503 | 200 | * @brief Implementation of periodic change of RPM Setpoint. Simulates setpoint changes to asses dynamic behaviour. |
dzoni | 8:5ce5fe1ce503 | 201 | * @note For test purposes. |
dzoni | 8:5ce5fe1ce503 | 202 | * |
dzoni | 8:5ce5fe1ce503 | 203 | * Increases Setpoint value step by step with every invocation. |
dzoni | 8:5ce5fe1ce503 | 204 | */ |
dzoni | 7:1aba48efb1c3 | 205 | void tskRPMSetpoint(void) |
dzoni | 7:1aba48efb1c3 | 206 | { |
dzoni | 11:4747badb2448 | 207 | fRPMSetpoint += 2.0f; |
dzoni | 11:4747badb2448 | 208 | if (35.0f < fRPMSetpoint) |
dzoni | 5:ec4d6e435822 | 209 | { |
dzoni | 11:4747badb2448 | 210 | fRPMSetpoint = 16.0f; |
dzoni | 0:bd186184ef2a | 211 | } |
dzoni | 5:ec4d6e435822 | 212 | |
dzoni | 7:1aba48efb1c3 | 213 | pid_RPM_Right_motor.setSetPoint(fRPMSetpoint); |
dzoni | 0:bd186184ef2a | 214 | } |
dzoni | 5:ec4d6e435822 | 215 | |
dzoni | 8:5ce5fe1ce503 | 216 | /** |
dzoni | 8:5ce5fe1ce503 | 217 | * @brief Implementation of background task. Periodically called from main loop without delay. |
dzoni | 8:5ce5fe1ce503 | 218 | * @note These actions are candidates for refactoring to event based implementation. |
dzoni | 8:5ce5fe1ce503 | 219 | * @warning Initial implementation. Simple and easy. |
dzoni | 8:5ce5fe1ce503 | 220 | * |
dzoni | 8:5ce5fe1ce503 | 221 | * This function implements actions, which needs to be processed with high priority. |
dzoni | 8:5ce5fe1ce503 | 222 | * Is intended to be called in every pass of main loop. |
dzoni | 8:5ce5fe1ce503 | 223 | */ |
dzoni | 5:ec4d6e435822 | 224 | void tskBackground(void) |
dzoni | 5:ec4d6e435822 | 225 | { |
dzoni | 5:ec4d6e435822 | 226 | // Impulse sensor - pulse counting |
dzoni | 5:ec4d6e435822 | 227 | int iTemp = pinImpulseSensorIn.read(); |
dzoni | 5:ec4d6e435822 | 228 | if (iTemp != iImpSensLastState) |
dzoni | 5:ec4d6e435822 | 229 | { |
dzoni | 5:ec4d6e435822 | 230 | iImpSensLastState = iTemp; |
dzoni | 5:ec4d6e435822 | 231 | uiImpSensTemp++; |
dzoni | 5:ec4d6e435822 | 232 | } |
dzoni | 8:5ce5fe1ce503 | 233 | } |