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@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?

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 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 }