Embedded Systems / Mbed OS ES_CW2_Advanced

New smaller slits code

Dependencies:   PID

Fork of ES_CW2_Starter by Edward Stott

main.cpp@4:a436ddb6e57e, 2017-03-18 (annotated)

Committer:
Iswanto
Date:
Sat Mar 18 15:10:48 2017 +0000
Revision:
4:a436ddb6e57e
Parent:
3:60009a5ed1dc
Child:
5:07b2e414d174
Cleaned up code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
estott 0:de4320f74764 1 #include "mbed.h"
Amudsen 3:60009a5ed1dc 2 #include "PID.h"
estott 0:de4320f74764 3
Iswanto 4:a436ddb6e57e 4
Iswanto 4:a436ddb6e57e 5
Iswanto 4:a436ddb6e57e 6 /////////////////////
Iswanto 4:a436ddb6e57e 7 // Pin definitions //
Iswanto 4:a436ddb6e57e 8 /////////////////////
Iswanto 4:a436ddb6e57e 9
estott 0:de4320f74764 10 //Photointerrupter input pins
estott 0:de4320f74764 11 #define I1pin D2
estott 2:4e88faab6988 12 #define I2pin D11
estott 2:4e88faab6988 13 #define I3pin D12
estott 2:4e88faab6988 14
Iswanto 4:a436ddb6e57e 15 DigitalIn I1(I1pin);
Iswanto 4:a436ddb6e57e 16 DigitalIn I2(I2pin);
Iswanto 4:a436ddb6e57e 17 DigitalIn I3(I3pin);
Iswanto 4:a436ddb6e57e 18
Iswanto 4:a436ddb6e57e 19 InterruptIn I1intr(I1pin);
Iswanto 4:a436ddb6e57e 20 InterruptIn I2intr(I2pin);
Iswanto 4:a436ddb6e57e 21 InterruptIn I3intr(I3pin);
Iswanto 4:a436ddb6e57e 22
estott 2:4e88faab6988 23 //Incremental encoder input pins
estott 2:4e88faab6988 24 #define CHA D7
estott 2:4e88faab6988 25 #define CHB D8
estott 0:de4320f74764 26
Iswanto 4:a436ddb6e57e 27 DigitalIn CHAR(CHA);
Iswanto 4:a436ddb6e57e 28 DigitalIn CHBR(CHB);
Iswanto 4:a436ddb6e57e 29
Iswanto 4:a436ddb6e57e 30 InterruptIn CHAintr(CHA);
Iswanto 4:a436ddb6e57e 31 InterruptIn CHBintr(CHB);
Iswanto 4:a436ddb6e57e 32
Iswanto 4:a436ddb6e57e 33 //Status LED
Iswanto 4:a436ddb6e57e 34 DigitalOut led1 (LED1);
Iswanto 4:a436ddb6e57e 35 DigitalOut led2 (LED2);
Iswanto 4:a436ddb6e57e 36 DigitalOut led3 (LED3);
Iswanto 4:a436ddb6e57e 37
estott 0:de4320f74764 38 //Motor Drive output pins //Mask in output byte
estott 2:4e88faab6988 39 #define L1Lpin D4 //0x01
estott 2:4e88faab6988 40 #define L1Hpin D5 //0x02
estott 2:4e88faab6988 41 #define L2Lpin D3 //0x04
estott 2:4e88faab6988 42 #define L2Hpin D6 //0x08
estott 2:4e88faab6988 43 #define L3Lpin D9 //0x10
estott 0:de4320f74764 44 #define L3Hpin D10 //0x20
estott 0:de4320f74764 45
Iswanto 4:a436ddb6e57e 46 PwmOut L1L(L1Lpin);
Iswanto 4:a436ddb6e57e 47 PwmOut L1H(L1Hpin);
Iswanto 4:a436ddb6e57e 48 PwmOut L2L(L2Lpin);
Iswanto 4:a436ddb6e57e 49 PwmOut L2H(L2Hpin);
Iswanto 4:a436ddb6e57e 50 PwmOut L3L(L3Lpin);
Iswanto 4:a436ddb6e57e 51 PwmOut L3H(L3Hpin);
Iswanto 4:a436ddb6e57e 52
estott 0:de4320f74764 53 //Mapping from sequential drive states to motor phase outputs
estott 0:de4320f74764 54 /*
estott 0:de4320f74764 55 State L1 L2 L3
estott 0:de4320f74764 56 0 H - L
estott 0:de4320f74764 57 1 - H L
estott 0:de4320f74764 58 2 L H -
estott 0:de4320f74764 59 3 L - H
estott 0:de4320f74764 60 4 - L H
estott 0:de4320f74764 61 5 H L -
estott 0:de4320f74764 62 6 - - -
estott 0:de4320f74764 63 7 - - -
estott 0:de4320f74764 64 */
Iswanto 4:a436ddb6e57e 65
estott 0:de4320f74764 66 //Drive state to output table
estott 0:de4320f74764 67 const int8_t driveTable[] = {0x12,0x18,0x09,0x21,0x24,0x06,0x00,0x00};
estott 2:4e88faab6988 68
estott 0:de4320f74764 69 //Mapping from interrupter inputs to sequential rotor states. 0x00 and 0x07 are not valid
estott 2:4e88faab6988 70 const int8_t stateMap[] = {0x07,0x05,0x03,0x04,0x01,0x00,0x02,0x07};
estott 2:4e88faab6988 71 //const int8_t stateMap[] = {0x07,0x01,0x03,0x02,0x05,0x00,0x04,0x07}; //Alternative if phase order of input or drive is reversed
estott 2:4e88faab6988 72
estott 2:4e88faab6988 73 //Phase lead to make motor spin
Amudsen 3:60009a5ed1dc 74 const int8_t lead = 2; //2 for forwards, -2 for backwards
estott 0:de4320f74764 75
estott 0:de4320f74764 76
Amudsen 3:60009a5ed1dc 77
Iswanto 4:a436ddb6e57e 78 //////////////////////
Iswanto 4:a436ddb6e57e 79 // Global variables //
Iswanto 4:a436ddb6e57e 80 //////////////////////
Amudsen 3:60009a5ed1dc 81
Iswanto 4:a436ddb6e57e 82 volatile float targetSpeed; // User defined instantaneous speed
Iswanto 4:a436ddb6e57e 83 volatile float revPerSec; // Instantaneous speed estimate
Iswanto 4:a436ddb6e57e 84 volatile float previousTime = 0; // Used to calculate revPerSec
Iswanto 4:a436ddb6e57e 85 volatile float previousSpeed = 0; // Previous value of revPerSec
Iswanto 4:a436ddb6e57e 86 volatile float dutyCycle; // PWM duty cycle between 0 and 1
Iswanto 4:a436ddb6e57e 87 volatile float timeMap[] = {0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 }; // Keep track of last state
Iswanto 4:a436ddb6e57e 88 Timer t; // To keep track of time
Iswanto 4:a436ddb6e57e 89 Serial pc(SERIAL_TX,SERIAL_RX); // Serial connection
Iswanto 4:a436ddb6e57e 90 PID controller(5.0, 0.0, 0.000001, 0.001); // Arguments are Kp, Ki, Kd and interval
estott 0:de4320f74764 91
Iswanto 4:a436ddb6e57e 92
Amudsen 3:60009a5ed1dc 93
Iswanto 4:a436ddb6e57e 94 ///////////////
Iswanto 4:a436ddb6e57e 95 // Functions //
Iswanto 4:a436ddb6e57e 96 ///////////////
Iswanto 4:a436ddb6e57e 97
Iswanto 4:a436ddb6e57e 98 // THREAD: Blink LED1 every 0.5 seconds
Iswanto 4:a436ddb6e57e 99 void blinkLED1(){
Amudsen 3:60009a5ed1dc 100 while(1){
Amudsen 3:60009a5ed1dc 101 led1 = 0;
Amudsen 3:60009a5ed1dc 102 wait(0.5);
Amudsen 3:60009a5ed1dc 103 led1 = 1;
Amudsen 3:60009a5ed1dc 104 wait(0.5);
Amudsen 3:60009a5ed1dc 105 }
Amudsen 3:60009a5ed1dc 106 }
Amudsen 3:60009a5ed1dc 107
Iswanto 4:a436ddb6e57e 108 // THREAD: Print instantaneous speed
Iswanto 4:a436ddb6e57e 109 void printStatus() {
Amudsen 3:60009a5ed1dc 110 while(1){
Iswanto 4:a436ddb6e57e 111 led3 = !led3;
Iswanto 4:a436ddb6e57e 112 //pc.printf("%f\n\r",revPerSec);
Iswanto 4:a436ddb6e57e 113 wait(2);
Amudsen 3:60009a5ed1dc 114 }
Amudsen 3:60009a5ed1dc 115 }
Amudsen 3:60009a5ed1dc 116
Iswanto 4:a436ddb6e57e 117 // THREAD: Control loop
Iswanto 4:a436ddb6e57e 118 void controlLoop(void){
Iswanto 4:a436ddb6e57e 119 while(true){
Iswanto 4:a436ddb6e57e 120 controller.setProcessValue(revPerSec);
Iswanto 4:a436ddb6e57e 121 dutyCycle = controller.compute();
Iswanto 4:a436ddb6e57e 122 wait(0.001);
Iswanto 4:a436ddb6e57e 123 }
Amudsen 3:60009a5ed1dc 124 }
Amudsen 3:60009a5ed1dc 125
Iswanto 4:a436ddb6e57e 126 // Measure speed using slits
Iswanto 4:a436ddb6e57e 127 void measureSpeedSlits(){
Iswanto 4:a436ddb6e57e 128 float currentTime = t.read();
Iswanto 4:a436ddb6e57e 129 revPerSec = 0.4*previousSpeed + 0.6*((0.0021367521)/(currentTime-previousTime));
Iswanto 4:a436ddb6e57e 130 previousTime = currentTime;
Iswanto 4:a436ddb6e57e 131 previousSpeed = revPerSec;
Iswanto 4:a436ddb6e57e 132 }
Iswanto 4:a436ddb6e57e 133
Iswanto 4:a436ddb6e57e 134 // Measure speed using photointerrupters
Iswanto 4:a436ddb6e57e 135 void measureSpeedPhoto(){
Amudsen 3:60009a5ed1dc 136 led3 = !led3;
Amudsen 3:60009a5ed1dc 137 float speedTime;
Amudsen 3:60009a5ed1dc 138 speedTime = t.read();
Amudsen 3:60009a5ed1dc 139 revPerSec = 1.0/(speedTime - timeMap[I1 + 2*I2 + 4*I3]);
Amudsen 3:60009a5ed1dc 140 timeMap[I1 + 2*I2 + 4*I3] = speedTime;
Iswanto 4:a436ddb6e57e 141 }
Amudsen 3:60009a5ed1dc 142
Iswanto 4:a436ddb6e57e 143 // Set motor states
Amudsen 3:60009a5ed1dc 144 void motorOut(int8_t driveState, float dutyCycle){
estott 2:4e88faab6988 145 //Lookup the output byte from the drive state.
estott 2:4e88faab6988 146 int8_t driveOut = driveTable[driveState & 0x07];
estott 2:4e88faab6988 147
estott 2:4e88faab6988 148 //Turn off first
Iswanto 4:a436ddb6e57e 149 if (~driveOut & 0x01) L1L.write(0); // = 0
Iswanto 4:a436ddb6e57e 150 if (~driveOut & 0x02) L1H.write(dutyCycle); // = 1;
Iswanto 4:a436ddb6e57e 151 if (~driveOut & 0x04) L2L.write(0); // = 0;
Iswanto 4:a436ddb6e57e 152 if (~driveOut & 0x08) L2H.write(dutyCycle); // = 1;
Iswanto 4:a436ddb6e57e 153 if (~driveOut & 0x10) L3L.write(0); // = 0;
Iswanto 4:a436ddb6e57e 154 if (~driveOut & 0x20) L3H.write(dutyCycle); // = 1;
estott 2:4e88faab6988 155
estott 2:4e88faab6988 156 //Then turn on
Iswanto 4:a436ddb6e57e 157 if (driveOut & 0x01) L1L.write(dutyCycle); // = 1;
Iswanto 4:a436ddb6e57e 158 if (driveOut & 0x02) L1H.write(0); // = 0;
Iswanto 4:a436ddb6e57e 159 if (driveOut & 0x04) L2L.write(dutyCycle); // = 1;
Iswanto 4:a436ddb6e57e 160 if (driveOut & 0x08) L2H.write(0); // = 0;
Iswanto 4:a436ddb6e57e 161 if (driveOut & 0x10) L3L.write(dutyCycle); // = 1;
Iswanto 4:a436ddb6e57e 162 if (driveOut & 0x20) L3H.write(0); // = 0;
Iswanto 4:a436ddb6e57e 163 }
estott 0:de4320f74764 164
Iswanto 4:a436ddb6e57e 165 //Convert photointerrupter inputs to a rotor state
estott 0:de4320f74764 166 inline int8_t readRotorState(){
estott 2:4e88faab6988 167 return stateMap[I1 + 2*I2 + 4*I3];
Iswanto 4:a436ddb6e57e 168 }
estott 0:de4320f74764 169
estott 0:de4320f74764 170 //Basic synchronisation routine
estott 2:4e88faab6988 171 int8_t motorHome() {
estott 0:de4320f74764 172 //Put the motor in drive state 0 and wait for it to stabilise
Iswanto 4:a436ddb6e57e 173 motorOut(0,1);
Amudsen 3:60009a5ed1dc 174 wait(3.0);
estott 0:de4320f74764 175
estott 0:de4320f74764 176 //Get the rotor state
estott 2:4e88faab6988 177 return readRotorState();
estott 0:de4320f74764 178 }
Iswanto 4:a436ddb6e57e 179
estott 0:de4320f74764 180
Iswanto 4:a436ddb6e57e 181
Iswanto 4:a436ddb6e57e 182 //////////
Iswanto 4:a436ddb6e57e 183 // Main //
Iswanto 4:a436ddb6e57e 184 //////////
Iswanto 4:a436ddb6e57e 185
estott 0:de4320f74764 186 int main() {
Amudsen 3:60009a5ed1dc 187
Iswanto 4:a436ddb6e57e 188 // Initialize threads
Iswanto 4:a436ddb6e57e 189 Thread status;
Iswanto 4:a436ddb6e57e 190 Thread controlLoopThread;
Iswanto 4:a436ddb6e57e 191 status.start(printStatus);
Iswanto 4:a436ddb6e57e 192 controlLoopThread.start(controlLoop);
Iswanto 4:a436ddb6e57e 193 t.start();
estott 2:4e88faab6988 194
Iswanto 4:a436ddb6e57e 195 // Set control loop parameters
Iswanto 4:a436ddb6e57e 196 controller.setInputLimits(0, 200);
Iswanto 4:a436ddb6e57e 197 controller.setOutputLimits(0.0, 1.0);
Iswanto 4:a436ddb6e57e 198 controller.setBias(0.0);
Iswanto 4:a436ddb6e57e 199 controller.setMode(0);
Iswanto 4:a436ddb6e57e 200 controller.setSetPoint(50.0);
Iswanto 4:a436ddb6e57e 201
Iswanto 4:a436ddb6e57e 202 //Initialize the serial port
estott 0:de4320f74764 203 Serial pc(SERIAL_TX, SERIAL_RX);
Iswanto 4:a436ddb6e57e 204 pc.printf("Device on \n\r");
Iswanto 4:a436ddb6e57e 205
Iswanto 4:a436ddb6e57e 206 int8_t orState = 0; //Rotot offset at motor state 0
estott 2:4e88faab6988 207 int8_t intState = 0;
estott 2:4e88faab6988 208 int8_t intStateOld = 0;
Iswanto 4:a436ddb6e57e 209 float PWM_period = 0.001f;
Iswanto 4:a436ddb6e57e 210 dutyCycle = 1;
Iswanto 4:a436ddb6e57e 211
Iswanto 4:a436ddb6e57e 212 L1L.period(PWM_period);
Iswanto 4:a436ddb6e57e 213 L1H.period(PWM_period);
Iswanto 4:a436ddb6e57e 214 L2L.period(PWM_period);
Iswanto 4:a436ddb6e57e 215 L2H.period(PWM_period);
Iswanto 4:a436ddb6e57e 216 L3L.period(PWM_period);
Iswanto 4:a436ddb6e57e 217 L3H.period(PWM_period);
Amudsen 3:60009a5ed1dc 218
Iswanto 4:a436ddb6e57e 219 // Slits interrupts
Iswanto 4:a436ddb6e57e 220 CHAintr.rise(&measureSpeedSlits);
Iswanto 4:a436ddb6e57e 221 CHBintr.rise(&measureSpeedSlits);
Iswanto 4:a436ddb6e57e 222 CHAintr.fall(&meeasureSpeedSlits);
Iswanto 4:a436ddb6e57e 223 CHBintr.fall(&measureSpeedSlits);
Iswanto 4:a436ddb6e57e 224
Iswanto 4:a436ddb6e57e 225 // USE FOR PHOTOINTERRUPTERS
Iswanto 4:a436ddb6e57e 226 // Photointerrupters interrupts
Iswanto 4:a436ddb6e57e 227 //I1intr.rise(&measureSpeedPhoto);
Iswanto 4:a436ddb6e57e 228 //I2intr.rise(&measureSpeedPhoto);
Iswanto 4:a436ddb6e57e 229 //I3intr.rise(&measureSpeedPhoto);
Iswanto 4:a436ddb6e57e 230 //I1intr.fall(&measureSpeedPhoto);
Iswanto 4:a436ddb6e57e 231 //I2intr.fall(&measureSpeedPhoto);
Iswanto 4:a436ddb6e57e 232 //I3intr.fall(&measureSpeedPhoto);
estott 0:de4320f74764 233
estott 0:de4320f74764 234 //Run the motor synchronisation
estott 2:4e88faab6988 235 orState = motorHome();
estott 0:de4320f74764 236
estott 0:de4320f74764 237 //Poll the rotor state and set the motor outputs accordingly to spin the motor
estott 1:184cb0870c04 238 while (1) {
Amudsen 3:60009a5ed1dc 239
Iswanto 4:a436ddb6e57e 240 // Read serial input
Iswanto 4:a436ddb6e57e 241 if (pc.readable()){
Iswanto 4:a436ddb6e57e 242 char buffer[128];
Iswanto 4:a436ddb6e57e 243 pc.gets(buffer, 6);
Iswanto 4:a436ddb6e57e 244 targetSpeed = atof(buffer);
Iswanto 4:a436ddb6e57e 245 pc.printf("Target speed: '%f'\n\r", targetSpeed);
Iswanto 4:a436ddb6e57e 246 controller.setSetPoint(targetSpeed);
Iswanto 4:a436ddb6e57e 247 orState = motorHome();
Iswanto 4:a436ddb6e57e 248 }
Amudsen 3:60009a5ed1dc 249
Iswanto 4:a436ddb6e57e 250 intState = readRotorState();
estott 2:4e88faab6988 251 if (intState != intStateOld) {
estott 2:4e88faab6988 252 intStateOld = intState;
Amudsen 3:60009a5ed1dc 253 motorOut((intState-orState+lead+6)%6, dutyCycle); //+6 to make sure the remainder is positive
estott 0:de4320f74764 254 }
estott 2:4e88faab6988 255 }
Iswanto 4:a436ddb6e57e 256 }