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@7:1aba48efb1c3, 2018-03-28 (annotated)

Committer:
dzoni
Date:
Wed Mar 28 09:32:05 2018 +0000
Revision:
7:1aba48efb1c3
Parent:
6:cc38171e6a4b
Child:
8:5ce5fe1ce503
Added PID controller to control motor speed (RPM). Compiles OK, ready for functional test.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dzoni 0:bd186184ef2a 1 #include "mbed.h"
dzoni 1:70c514e10598 2 #include "FastPWM.h"
dzoni 2:70918f7f8451 3 #include "FastIO.h"
dzoni 7:1aba48efb1c3 4 #include "PID.h"
dzoni 0:bd186184ef2a 5
dzoni 4:7cb8986200a7 6 #define MODSERIAL_DEFAULT_RX_BUFFER_SIZE 16
dzoni 4:7cb8986200a7 7 #define MODSERIAL_DEFAULT_TX_BUFFER_SIZE 64
dzoni 4:7cb8986200a7 8 #include "MODSERIAL.h"
dzoni 4:7cb8986200a7 9
dzoni 5:ec4d6e435822 10 #define IMPULSE_SENSOR_R_PIN (PA_9)
dzoni 5:ec4d6e435822 11 #define PWM_OUT_R_PIN (PA_6)
dzoni 4:7cb8986200a7 12
dzoni 3:f6c30ada5370 13 //------------------------------------
dzoni 3:f6c30ada5370 14 // Hyperterminal configuration
dzoni 3:f6c30ada5370 15 // 9600 bauds, 8-bit data, no parity
dzoni 3:f6c30ada5370 16 //------------------------------------
dzoni 4:7cb8986200a7 17 MODSERIAL pcLink(SERIAL_TX, SERIAL_RX);
dzoni 4:7cb8986200a7 18
dzoni 7:1aba48efb1c3 19 static const us_timestamp_t periodImpSens = 125000; // 125 msec
dzoni 7:1aba48efb1c3 20 static const us_timestamp_t periodLEDBlink = 100000; // 100 msec
dzoni 7:1aba48efb1c3 21 static const us_timestamp_t periodPWMWrite = 250000; // 250 msec
dzoni 7:1aba48efb1c3 22 static const us_timestamp_t periodRPMSetpoint = 10000000; // 10 sec
dzoni 4:7cb8986200a7 23
dzoni 7:1aba48efb1c3 24 static us_timestamp_t tStampImpSens = 0;
dzoni 7:1aba48efb1c3 25 static us_timestamp_t tStampLEDBlink = 0;
dzoni 7:1aba48efb1c3 26 static us_timestamp_t tStampPWMWrite = 0;
dzoni 7:1aba48efb1c3 27 static us_timestamp_t tStampRPMSetpoint = 0;
dzoni 7:1aba48efb1c3 28
dzoni 7:1aba48efb1c3 29 static us_timestamp_t tStamp = 0;
dzoni 4:7cb8986200a7 30
dzoni 4:7cb8986200a7 31 static unsigned int uiImpSens = 0U;
dzoni 5:ec4d6e435822 32 static unsigned int uiImpSensTemp = 0U;
dzoni 5:ec4d6e435822 33 static int iImpSensLastState = 0;
dzoni 7:1aba48efb1c3 34 static float fPwmDuty = 0.0;
dzoni 7:1aba48efb1c3 35 static float fRPMSetpoint = 0.0f;
dzoni 2:70918f7f8451 36
dzoni 5:ec4d6e435822 37 static void setup(void);
dzoni 5:ec4d6e435822 38 static void tskImpSens(void);
dzoni 5:ec4d6e435822 39 static void tskLEDBlink(void);
dzoni 5:ec4d6e435822 40 static void tskPWMWrite(void);
dzoni 7:1aba48efb1c3 41 static void tskRPMSetpoint(void);
dzoni 5:ec4d6e435822 42 static void tskBackground(void);
dzoni 5:ec4d6e435822 43
dzoni 7:1aba48efb1c3 44 FastPWM mypwm(PWM_OUT_R_PIN);
dzoni 7:1aba48efb1c3 45
dzoni 7:1aba48efb1c3 46 FastIn<IMPULSE_SENSOR_R_PIN> pinImpulseSensorIn;
dzoni 7:1aba48efb1c3 47 FastIn<USER_BUTTON> pinUserButtonIn;
dzoni 7:1aba48efb1c3 48
dzoni 7:1aba48efb1c3 49 PID pid_RPM_Right_motor(1.0f, 0.0f, 0.0f, (((float)periodPWMWrite)/1000000.0f));
dzoni 7:1aba48efb1c3 50
dzoni 7:1aba48efb1c3 51 DigitalOut myled(LED1);
dzoni 7:1aba48efb1c3 52
dzoni 7:1aba48efb1c3 53 Timer myTimer;
dzoni 7:1aba48efb1c3 54
dzoni 7:1aba48efb1c3 55
dzoni 5:ec4d6e435822 56 static inline void DO_TASK(us_timestamp_t tskPeriod, us_timestamp_t &tskTimer, us_timestamp_t timeStamp, void (*tskFunction)(void))
dzoni 5:ec4d6e435822 57 {
dzoni 5:ec4d6e435822 58 if (tskPeriod < (timeStamp - tskTimer))
dzoni 5:ec4d6e435822 59 {
dzoni 5:ec4d6e435822 60 tskTimer = timeStamp;
dzoni 5:ec4d6e435822 61 (*tskFunction)();
dzoni 5:ec4d6e435822 62 }
dzoni 5:ec4d6e435822 63 }
dzoni 5:ec4d6e435822 64
dzoni 5:ec4d6e435822 65 static inline void BACKGROUND(void (*tskFunction)(void))
dzoni 5:ec4d6e435822 66 {
dzoni 5:ec4d6e435822 67 (*tskFunction)();
dzoni 5:ec4d6e435822 68 }
dzoni 4:7cb8986200a7 69
dzoni 5:ec4d6e435822 70
dzoni 5:ec4d6e435822 71 int main(void)
dzoni 5:ec4d6e435822 72 {
dzoni 5:ec4d6e435822 73 setup();
dzoni 5:ec4d6e435822 74
dzoni 5:ec4d6e435822 75 while(1)
dzoni 5:ec4d6e435822 76 {
dzoni 5:ec4d6e435822 77 tStamp = myTimer.read_high_resolution_us();
dzoni 5:ec4d6e435822 78
dzoni 7:1aba48efb1c3 79 DO_TASK(periodLEDBlink, tStampLEDBlink, tStamp, &tskLEDBlink);
dzoni 7:1aba48efb1c3 80 DO_TASK(periodPWMWrite, tStampPWMWrite, tStamp, &tskPWMWrite);
dzoni 7:1aba48efb1c3 81 DO_TASK(periodImpSens, tStampImpSens, tStamp, &tskImpSens);
dzoni 7:1aba48efb1c3 82 DO_TASK(periodRPMSetpoint, tStampRPMSetpoint, tStamp, &tskRPMSetpoint);
dzoni 5:ec4d6e435822 83
dzoni 5:ec4d6e435822 84 BACKGROUND(&tskBackground);
dzoni 5:ec4d6e435822 85 }
dzoni 5:ec4d6e435822 86 }
dzoni 5:ec4d6e435822 87
dzoni 5:ec4d6e435822 88
dzoni 5:ec4d6e435822 89 void setup(void)
dzoni 5:ec4d6e435822 90 {
dzoni 4:7cb8986200a7 91 pcLink.baud(115200);
dzoni 4:7cb8986200a7 92 pcLink.format(8, SerialBase::None, 1);
dzoni 4:7cb8986200a7 93
dzoni 5:ec4d6e435822 94 mypwm.period_us(2000);
dzoni 5:ec4d6e435822 95 mypwm.write(0.5);
dzoni 0:bd186184ef2a 96
dzoni 4:7cb8986200a7 97 myTimer.start();
dzoni 7:1aba48efb1c3 98
dzoni 7:1aba48efb1c3 99 //Analog input from 0.0 to 100.0 impulses per measurement period
dzoni 7:1aba48efb1c3 100 pid_RPM_Right_motor.setInputLimits(0.0f, 100.0f);
dzoni 7:1aba48efb1c3 101
dzoni 7:1aba48efb1c3 102 //Pwm output from 0.0 to 1.0
dzoni 7:1aba48efb1c3 103 pid_RPM_Right_motor.setOutputLimits(0.0f, 1.0f);
dzoni 7:1aba48efb1c3 104
dzoni 7:1aba48efb1c3 105 //If there's a bias.
dzoni 7:1aba48efb1c3 106 pid_RPM_Right_motor.setBias(0.0f);
dzoni 7:1aba48efb1c3 107 pid_RPM_Right_motor.setMode(AUTO_MODE);
dzoni 7:1aba48efb1c3 108
dzoni 7:1aba48efb1c3 109 //We want the process variable to be 0.0 RPM
dzoni 7:1aba48efb1c3 110 pid_RPM_Right_motor.setSetPoint(0.0f);
dzoni 5:ec4d6e435822 111 }
dzoni 5:ec4d6e435822 112
dzoni 5:ec4d6e435822 113 void tskImpSens(void)
dzoni 5:ec4d6e435822 114 {
dzoni 5:ec4d6e435822 115 uiImpSens = uiImpSensTemp;
dzoni 5:ec4d6e435822 116 uiImpSensTemp = 0U;
dzoni 4:7cb8986200a7 117
dzoni 6:cc38171e6a4b 118 pcLink.printf("IMP: %u imp. \r", uiImpSens);
dzoni 5:ec4d6e435822 119 }
dzoni 4:7cb8986200a7 120
dzoni 5:ec4d6e435822 121 void tskLEDBlink(void)
dzoni 5:ec4d6e435822 122 {
dzoni 5:ec4d6e435822 123 myled = !myled;
dzoni 5:ec4d6e435822 124 }
dzoni 1:70c514e10598 125
dzoni 5:ec4d6e435822 126 void tskPWMWrite(void)
dzoni 5:ec4d6e435822 127 {
dzoni 7:1aba48efb1c3 128 // fPwmDuty = fPwmDuty + 0.1;
dzoni 4:7cb8986200a7 129
dzoni 7:1aba48efb1c3 130 // if (1.0 < fPwmDuty)
dzoni 7:1aba48efb1c3 131 // {
dzoni 7:1aba48efb1c3 132 // fPwmDuty = 0.0;
dzoni 7:1aba48efb1c3 133 // }
dzoni 7:1aba48efb1c3 134
dzoni 7:1aba48efb1c3 135 //Update the process variable.
dzoni 7:1aba48efb1c3 136 pid_RPM_Right_motor.setProcessValue((float)uiImpSens);
dzoni 7:1aba48efb1c3 137
dzoni 7:1aba48efb1c3 138 //Set the new output.
dzoni 7:1aba48efb1c3 139 fPwmDuty = pid_RPM_Right_motor.compute();
dzoni 7:1aba48efb1c3 140
dzoni 7:1aba48efb1c3 141 mypwm.write(fPwmDuty);
dzoni 7:1aba48efb1c3 142
dzoni 7:1aba48efb1c3 143 pcLink.printf("PWM: %.2f %%\tIMP: %u imp.\tSET: %.2f imp.\t\r", mypwm.read() * 100, uiImpSens, fRPMSetpoint);
dzoni 7:1aba48efb1c3 144
dzoni 7:1aba48efb1c3 145 // pcLink.printf("\r\nPWM: %.2f %% \r\n", mypwm.read() * 100);
dzoni 7:1aba48efb1c3 146 }
dzoni 7:1aba48efb1c3 147
dzoni 7:1aba48efb1c3 148 void tskRPMSetpoint(void)
dzoni 7:1aba48efb1c3 149 {
dzoni 7:1aba48efb1c3 150 fRPMSetpoint += 10.0f;
dzoni 7:1aba48efb1c3 151 if (100.0f < fRPMSetpoint)
dzoni 5:ec4d6e435822 152 {
dzoni 7:1aba48efb1c3 153 fRPMSetpoint = 0.0f;
dzoni 0:bd186184ef2a 154 }
dzoni 5:ec4d6e435822 155
dzoni 7:1aba48efb1c3 156 pid_RPM_Right_motor.setSetPoint(fRPMSetpoint);
dzoni 0:bd186184ef2a 157 }
dzoni 5:ec4d6e435822 158
dzoni 5:ec4d6e435822 159 void tskBackground(void)
dzoni 5:ec4d6e435822 160 {
dzoni 5:ec4d6e435822 161 // Impulse sensor - pulse counting
dzoni 5:ec4d6e435822 162 int iTemp = pinImpulseSensorIn.read();
dzoni 5:ec4d6e435822 163 if (iTemp != iImpSensLastState)
dzoni 5:ec4d6e435822 164 {
dzoni 5:ec4d6e435822 165 iImpSensLastState = iTemp;
dzoni 5:ec4d6e435822 166 uiImpSensTemp++;
dzoni 5:ec4d6e435822 167 }
dzoni 5:ec4d6e435822 168 }