Honza T. / Mbed 2 deprecated DC_motor_control_test

Basic DC motor control test, rpm feedback by simple impulse signal, PID speed control.

Dependencies:   FastPWM mbed FastIO MODSERIAL

main.cpp@10:c28d133a1408, 2018-04-03 (annotated)

Committer:
dzoni
Date:
Tue Apr 03 12:49:11 2018 +0000
Revision:
10:c28d133a1408
Parent:
8:5ce5fe1ce503
Child:
11:4747badb2448
Doxy docs.

Who changed what in which revision?

UserRevisionLine numberNew 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 8:5ce5fe1ce503 66 PID pid_RPM_Right_motor(1.0f, 0.0f, 0.0f, (((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 5:ec4d6e435822 125 mypwm.period_us(2000);
dzoni 5:ec4d6e435822 126 mypwm.write(0.5);
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 7:1aba48efb1c3 131 pid_RPM_Right_motor.setInputLimits(0.0f, 100.0f);
dzoni 7:1aba48efb1c3 132
dzoni 7:1aba48efb1c3 133 //Pwm output from 0.0 to 1.0
dzoni 7:1aba48efb1c3 134 pid_RPM_Right_motor.setOutputLimits(0.0f, 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 7:1aba48efb1c3 141 pid_RPM_Right_motor.setSetPoint(0.0f);
dzoni 5:ec4d6e435822 142 }
dzoni 5:ec4d6e435822 143
dzoni 8:5ce5fe1ce503 144 // Task worker functions definitions
dzoni 8:5ce5fe1ce503 145 /**
dzoni 8:5ce5fe1ce503 146 * @brief RPM calculation.
dzoni 8:5ce5fe1ce503 147 * @note Needs refactoring to implement #ifdef for 2 different implementation (with/without PID).
dzoni 8:5ce5fe1ce503 148 *
dzoni 8:5ce5fe1ce503 149 * Stores impulse count per measurement period and clears impulse counter.
dzoni 8:5ce5fe1ce503 150 */
dzoni 5:ec4d6e435822 151 void tskImpSens(void)
dzoni 5:ec4d6e435822 152 {
dzoni 5:ec4d6e435822 153 uiImpSens = uiImpSensTemp;
dzoni 5:ec4d6e435822 154 uiImpSensTemp = 0U;
dzoni 4:7cb8986200a7 155
dzoni 8:5ce5fe1ce503 156 // pcLink.printf("IMP: %u imp. \r", uiImpSens);
dzoni 5:ec4d6e435822 157 }
dzoni 4:7cb8986200a7 158
dzoni 8:5ce5fe1ce503 159 /**
dzoni 8:5ce5fe1ce503 160 * @brief User LED flashing.
dzoni 8:5ce5fe1ce503 161 *
dzoni 8:5ce5fe1ce503 162 * Implements User LED flashing.
dzoni 8:5ce5fe1ce503 163 */
dzoni 5:ec4d6e435822 164 void tskLEDBlink(void)
dzoni 5:ec4d6e435822 165 {
dzoni 5:ec4d6e435822 166 myled = !myled;
dzoni 5:ec4d6e435822 167 }
dzoni 1:70c514e10598 168
dzoni 8:5ce5fe1ce503 169 /**
dzoni 8:5ce5fe1ce503 170 * @brief Writes new duty cycle value into PWM generator.
dzoni 8:5ce5fe1ce503 171 * @note Needs refactoring to implement #ifdef for 2 different implementation (with/without PID).
dzoni 8:5ce5fe1ce503 172 * @warning Not finished.
dzoni 8:5ce5fe1ce503 173 *
dzoni 8:5ce5fe1ce503 174 * Calculates new dyty cycle and writes the value into PWM generator.
dzoni 8:5ce5fe1ce503 175 */
dzoni 5:ec4d6e435822 176 void tskPWMWrite(void)
dzoni 5:ec4d6e435822 177 {
dzoni 7:1aba48efb1c3 178 // fPwmDuty = fPwmDuty + 0.1;
dzoni 4:7cb8986200a7 179
dzoni 7:1aba48efb1c3 180 // if (1.0 < fPwmDuty)
dzoni 7:1aba48efb1c3 181 // {
dzoni 7:1aba48efb1c3 182 // fPwmDuty = 0.0;
dzoni 7:1aba48efb1c3 183 // }
dzoni 7:1aba48efb1c3 184
dzoni 7:1aba48efb1c3 185 //Update the process variable.
dzoni 7:1aba48efb1c3 186 pid_RPM_Right_motor.setProcessValue((float)uiImpSens);
dzoni 7:1aba48efb1c3 187
dzoni 7:1aba48efb1c3 188 //Set the new output.
dzoni 7:1aba48efb1c3 189 fPwmDuty = pid_RPM_Right_motor.compute();
dzoni 7:1aba48efb1c3 190
dzoni 7:1aba48efb1c3 191 mypwm.write(fPwmDuty);
dzoni 7:1aba48efb1c3 192
dzoni 7:1aba48efb1c3 193 pcLink.printf("PWM: %.2f %%\tIMP: %u imp.\tSET: %.2f imp.\t\r", mypwm.read() * 100, uiImpSens, fRPMSetpoint);
dzoni 7:1aba48efb1c3 194
dzoni 7:1aba48efb1c3 195 // pcLink.printf("\r\nPWM: %.2f %% \r\n", mypwm.read() * 100);
dzoni 7:1aba48efb1c3 196 }
dzoni 7:1aba48efb1c3 197
dzoni 8:5ce5fe1ce503 198 /**
dzoni 8:5ce5fe1ce503 199 * @brief Implementation of periodic change of RPM Setpoint. Simulates setpoint changes to asses dynamic behaviour.
dzoni 8:5ce5fe1ce503 200 * @note For test purposes.
dzoni 8:5ce5fe1ce503 201 *
dzoni 8:5ce5fe1ce503 202 * Increases Setpoint value step by step with every invocation.
dzoni 8:5ce5fe1ce503 203 */
dzoni 7:1aba48efb1c3 204 void tskRPMSetpoint(void)
dzoni 7:1aba48efb1c3 205 {
dzoni 7:1aba48efb1c3 206 fRPMSetpoint += 10.0f;
dzoni 7:1aba48efb1c3 207 if (100.0f < fRPMSetpoint)
dzoni 5:ec4d6e435822 208 {
dzoni 7:1aba48efb1c3 209 fRPMSetpoint = 0.0f;
dzoni 0:bd186184ef2a 210 }
dzoni 5:ec4d6e435822 211
dzoni 7:1aba48efb1c3 212 pid_RPM_Right_motor.setSetPoint(fRPMSetpoint);
dzoni 0:bd186184ef2a 213 }
dzoni 5:ec4d6e435822 214
dzoni 8:5ce5fe1ce503 215 /**
dzoni 8:5ce5fe1ce503 216 * @brief Implementation of background task. Periodically called from main loop without delay.
dzoni 8:5ce5fe1ce503 217 * @note These actions are candidates for refactoring to event based implementation.
dzoni 8:5ce5fe1ce503 218 * @warning Initial implementation. Simple and easy.
dzoni 8:5ce5fe1ce503 219 *
dzoni 8:5ce5fe1ce503 220 * This function implements actions, which needs to be processed with high priority.
dzoni 8:5ce5fe1ce503 221 * Is intended to be called in every pass of main loop.
dzoni 8:5ce5fe1ce503 222 */
dzoni 5:ec4d6e435822 223 void tskBackground(void)
dzoni 5:ec4d6e435822 224 {
dzoni 5:ec4d6e435822 225 // Impulse sensor - pulse counting
dzoni 5:ec4d6e435822 226 int iTemp = pinImpulseSensorIn.read();
dzoni 5:ec4d6e435822 227 if (iTemp != iImpSensLastState)
dzoni 5:ec4d6e435822 228 {
dzoni 5:ec4d6e435822 229 iImpSensLastState = iTemp;
dzoni 5:ec4d6e435822 230 uiImpSensTemp++;
dzoni 5:ec4d6e435822 231 }
dzoni 8:5ce5fe1ce503 232 }