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@8:5ce5fe1ce503, 2018-03-28 (annotated)

Committer:
dzoni
Date:
Wed Mar 28 13:18:59 2018 +0000
Revision:
8:5ce5fe1ce503
Parent:
7:1aba48efb1c3
Child:
10:c28d133a1408
Doxygen comments added. Ready for function test.

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