Embedded Systems / Mbed OS ES_CW2_Advanced

New smaller slits code

Dependencies:   PID

Fork of ES_CW2_Starter by Edward Stott

main.cpp@3:60009a5ed1dc, 2017-03-17 (annotated)

Committer:
Amudsen
Date:
Fri Mar 17 14:05:26 2017 +0000
Revision:
3:60009a5ed1dc
Parent:
2:4e88faab6988
Child:
4:a436ddb6e57e
Small slits with PID

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
estott 0:de4320f74764 4 //Photointerrupter input pins
estott 0:de4320f74764 5 #define I1pin D2
estott 2:4e88faab6988 6 #define I2pin D11
estott 2:4e88faab6988 7 #define I3pin D12
estott 2:4e88faab6988 8
estott 2:4e88faab6988 9 //Incremental encoder input pins
estott 2:4e88faab6988 10 #define CHA D7
estott 2:4e88faab6988 11 #define CHB D8
estott 0:de4320f74764 12
estott 0:de4320f74764 13 //Motor Drive output pins //Mask in output byte
estott 2:4e88faab6988 14 #define L1Lpin D4 //0x01
estott 2:4e88faab6988 15 #define L1Hpin D5 //0x02
estott 2:4e88faab6988 16 #define L2Lpin D3 //0x04
estott 2:4e88faab6988 17 #define L2Hpin D6 //0x08
estott 2:4e88faab6988 18 #define L3Lpin D9 //0x10
estott 0:de4320f74764 19 #define L3Hpin D10 //0x20
estott 0:de4320f74764 20
estott 0:de4320f74764 21 //Mapping from sequential drive states to motor phase outputs
estott 0:de4320f74764 22 /*
estott 0:de4320f74764 23 State L1 L2 L3
estott 0:de4320f74764 24 0 H - L
estott 0:de4320f74764 25 1 - H L
estott 0:de4320f74764 26 2 L H -
estott 0:de4320f74764 27 3 L - H
estott 0:de4320f74764 28 4 - L H
estott 0:de4320f74764 29 5 H L -
estott 0:de4320f74764 30 6 - - -
estott 0:de4320f74764 31 7 - - -
estott 0:de4320f74764 32 */
estott 0:de4320f74764 33 //Drive state to output table
estott 0:de4320f74764 34 const int8_t driveTable[] = {0x12,0x18,0x09,0x21,0x24,0x06,0x00,0x00};
estott 2:4e88faab6988 35
estott 0:de4320f74764 36 //Mapping from interrupter inputs to sequential rotor states. 0x00 and 0x07 are not valid
estott 2:4e88faab6988 37 const int8_t stateMap[] = {0x07,0x05,0x03,0x04,0x01,0x00,0x02,0x07};
estott 2:4e88faab6988 38 //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 39
estott 2:4e88faab6988 40 //Phase lead to make motor spin
Amudsen 3:60009a5ed1dc 41 const int8_t lead = 2; //2 for forwards, -2 for backwards
estott 0:de4320f74764 42
Amudsen 3:60009a5ed1dc 43 volatile float revTime;
Amudsen 3:60009a5ed1dc 44 volatile float revPerSec;
Amudsen 3:60009a5ed1dc 45 volatile float errorPrev = 0;
Amudsen 3:60009a5ed1dc 46 volatile float dutyCycle;
Amudsen 3:60009a5ed1dc 47 volatile float targetSpeed;
Amudsen 3:60009a5ed1dc 48 volatile float acc;
Amudsen 3:60009a5ed1dc 49
Amudsen 3:60009a5ed1dc 50 volatile float timeMap[] = {0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 };
Amudsen 3:60009a5ed1dc 51
Amudsen 3:60009a5ed1dc 52 volatile float prevTime = 0;
Amudsen 3:60009a5ed1dc 53 volatile float prevSpeed = 0;
estott 0:de4320f74764 54 //Status LED
estott 0:de4320f74764 55 DigitalOut led1(LED1);
Amudsen 3:60009a5ed1dc 56 DigitalOut led2 (LED2);
Amudsen 3:60009a5ed1dc 57 DigitalOut led3 (LED3);
Amudsen 3:60009a5ed1dc 58
Amudsen 3:60009a5ed1dc 59 InterruptIn CHAintr(CHA);
Amudsen 3:60009a5ed1dc 60 InterruptIn CHBintr(CHB);
Amudsen 3:60009a5ed1dc 61
Amudsen 3:60009a5ed1dc 62 DigitalIn CHAR(CHA);
Amudsen 3:60009a5ed1dc 63 DigitalIn CHBR(CHB);
estott 0:de4320f74764 64
estott 0:de4320f74764 65 //Photointerrupter inputs
Amudsen 3:60009a5ed1dc 66 InterruptIn I1intr(I1pin);
Amudsen 3:60009a5ed1dc 67 InterruptIn I2intr(I2pin);
Amudsen 3:60009a5ed1dc 68 InterruptIn I3intr(I3pin);
Amudsen 3:60009a5ed1dc 69
estott 2:4e88faab6988 70 DigitalIn I1(I1pin);
estott 2:4e88faab6988 71 DigitalIn I2(I2pin);
estott 2:4e88faab6988 72 DigitalIn I3(I3pin);
estott 0:de4320f74764 73
Amudsen 3:60009a5ed1dc 74
Amudsen 3:60009a5ed1dc 75 PID controller(5.0, 0.0, 0.000001, 0.001);
Amudsen 3:60009a5ed1dc 76 Ticker tick;
Amudsen 3:60009a5ed1dc 77
Amudsen 3:60009a5ed1dc 78 Timer t;
Amudsen 3:60009a5ed1dc 79
Amudsen 3:60009a5ed1dc 80 Serial pc(SERIAL_TX,SERIAL_RX);
estott 0:de4320f74764 81 //Motor Drive outputs
Amudsen 3:60009a5ed1dc 82 PwmOut L1L(L1Lpin);
Amudsen 3:60009a5ed1dc 83 PwmOut L1H(L1Hpin);
Amudsen 3:60009a5ed1dc 84 PwmOut L2L(L2Lpin);
Amudsen 3:60009a5ed1dc 85 PwmOut L2H(L2Hpin);
Amudsen 3:60009a5ed1dc 86 PwmOut L3L(L3Lpin);
Amudsen 3:60009a5ed1dc 87 PwmOut L3H(L3Hpin);
estott 0:de4320f74764 88
estott 0:de4320f74764 89 //Set a given drive state
Amudsen 3:60009a5ed1dc 90
Amudsen 3:60009a5ed1dc 91 void blinkLED2(){
Amudsen 3:60009a5ed1dc 92 while(1){
Amudsen 3:60009a5ed1dc 93 led1 = 0;
Amudsen 3:60009a5ed1dc 94 wait(0.5);
Amudsen 3:60009a5ed1dc 95 led1 = 1;
Amudsen 3:60009a5ed1dc 96 wait(0.5);
Amudsen 3:60009a5ed1dc 97 }
Amudsen 3:60009a5ed1dc 98 }
Amudsen 3:60009a5ed1dc 99
Amudsen 3:60009a5ed1dc 100 void blinkLED3(){
Amudsen 3:60009a5ed1dc 101 while(1){
Amudsen 3:60009a5ed1dc 102 led3 = 0;
Amudsen 3:60009a5ed1dc 103 wait(0.1);
Amudsen 3:60009a5ed1dc 104 led3 = 1;
Amudsen 3:60009a5ed1dc 105 wait(0.1);
Amudsen 3:60009a5ed1dc 106 }
Amudsen 3:60009a5ed1dc 107 }
Amudsen 3:60009a5ed1dc 108
Amudsen 3:60009a5ed1dc 109 void mesSpeedSlits(){
Amudsen 3:60009a5ed1dc 110 float currTime = t.read();
Amudsen 3:60009a5ed1dc 111 revPerSec = 0.4*prevSpeed + 0.6*((0.0021367521)/(currTime-prevTime));
Amudsen 3:60009a5ed1dc 112 prevTime = currTime;
Amudsen 3:60009a5ed1dc 113 prevSpeed = revPerSec;
estott 0:de4320f74764 114
Amudsen 3:60009a5ed1dc 115 }
Amudsen 3:60009a5ed1dc 116
Amudsen 3:60009a5ed1dc 117 void mesRot(){
Amudsen 3:60009a5ed1dc 118 led3 = !led3;
Amudsen 3:60009a5ed1dc 119 float speedTime;
Amudsen 3:60009a5ed1dc 120 speedTime = t.read();
Amudsen 3:60009a5ed1dc 121 revPerSec = 1.0/(speedTime - timeMap[I1 + 2*I2 + 4*I3]);
Amudsen 3:60009a5ed1dc 122 timeMap[I1 + 2*I2 + 4*I3] = speedTime;
Amudsen 3:60009a5ed1dc 123 }
Amudsen 3:60009a5ed1dc 124
Amudsen 3:60009a5ed1dc 125 void controlLoop(void){
Amudsen 3:60009a5ed1dc 126 while(true){
Amudsen 3:60009a5ed1dc 127 //float error = targetSpeed - revPerSec;
Amudsen 3:60009a5ed1dc 128 //float errorDer = (error - errorPrev);
Amudsen 3:60009a5ed1dc 129 //float k = 5.0;
Amudsen 3:60009a5ed1dc 130 //float kd = 1;
Amudsen 3:60009a5ed1dc 131 //dutyCycle = k*error + kd*errorDer;
Amudsen 3:60009a5ed1dc 132 controller.setProcessValue(revPerSec);
Amudsen 3:60009a5ed1dc 133 //Set the new output.
Amudsen 3:60009a5ed1dc 134 dutyCycle = controller.compute();
Amudsen 3:60009a5ed1dc 135 //errorPrev = error; //remeber error
Amudsen 3:60009a5ed1dc 136 wait(0.001);
Amudsen 3:60009a5ed1dc 137 }
Amudsen 3:60009a5ed1dc 138 }
Amudsen 3:60009a5ed1dc 139
Amudsen 3:60009a5ed1dc 140 void decrease(){
Amudsen 3:60009a5ed1dc 141 dutyCycle -= 0.05;
Amudsen 3:60009a5ed1dc 142 pc.printf("current value: %f", dutyCycle);
Amudsen 3:60009a5ed1dc 143 if (dutyCycle < 0.2){
Amudsen 3:60009a5ed1dc 144 dutyCycle = 1;
Amudsen 3:60009a5ed1dc 145 }
Amudsen 3:60009a5ed1dc 146 }
Amudsen 3:60009a5ed1dc 147
Amudsen 3:60009a5ed1dc 148
Amudsen 3:60009a5ed1dc 149 void motorOut(int8_t driveState, float dutyCycle){
Amudsen 3:60009a5ed1dc 150 //float dutyCycle = 1.0f;
estott 2:4e88faab6988 151 //Lookup the output byte from the drive state.
estott 2:4e88faab6988 152 int8_t driveOut = driveTable[driveState & 0x07];
estott 2:4e88faab6988 153
estott 2:4e88faab6988 154 //Turn off first
Amudsen 3:60009a5ed1dc 155 if (~driveOut & 0x01) L1L.write(0); // = 0
Amudsen 3:60009a5ed1dc 156 if (~driveOut & 0x02) L1H.write(dutyCycle);// = 1;
Amudsen 3:60009a5ed1dc 157 if (~driveOut & 0x04) L2L.write(0); // = 0;
Amudsen 3:60009a5ed1dc 158 if (~driveOut & 0x08) L2H.write(dutyCycle);// = 1;
Amudsen 3:60009a5ed1dc 159 if (~driveOut & 0x10) L3L.write(0);// = 0;
Amudsen 3:60009a5ed1dc 160 if (~driveOut & 0x20) L3H.write(dutyCycle);// = 1;
estott 2:4e88faab6988 161
estott 2:4e88faab6988 162 //Then turn on
Amudsen 3:60009a5ed1dc 163 if (driveOut & 0x01) L1L.write(dutyCycle);// = 1;
Amudsen 3:60009a5ed1dc 164 if (driveOut & 0x02) L1H.write(0); // = 0;
Amudsen 3:60009a5ed1dc 165 if (driveOut & 0x04) L2L.write(dutyCycle);// = 1;
Amudsen 3:60009a5ed1dc 166 if (driveOut & 0x08) L2H.write(0);// = 0;
Amudsen 3:60009a5ed1dc 167 if (driveOut & 0x10) L3L.write(dutyCycle);// = 1;
Amudsen 3:60009a5ed1dc 168 if (driveOut & 0x20) L3H.write(0);// = 0;
estott 0:de4320f74764 169 }
estott 0:de4320f74764 170
estott 2:4e88faab6988 171 //Convert photointerrupter inputs to a rotor state
estott 0:de4320f74764 172 inline int8_t readRotorState(){
estott 2:4e88faab6988 173 return stateMap[I1 + 2*I2 + 4*I3];
estott 0:de4320f74764 174 }
estott 0:de4320f74764 175
estott 0:de4320f74764 176 //Basic synchronisation routine
estott 2:4e88faab6988 177 int8_t motorHome() {
estott 0:de4320f74764 178 //Put the motor in drive state 0 and wait for it to stabilise
Amudsen 3:60009a5ed1dc 179 motorOut(0, 1);
Amudsen 3:60009a5ed1dc 180 wait(3.0);
estott 0:de4320f74764 181
estott 0:de4320f74764 182 //Get the rotor state
estott 2:4e88faab6988 183 return readRotorState();
estott 0:de4320f74764 184 }
estott 0:de4320f74764 185
Amudsen 3:60009a5ed1dc 186 void printStatus() {
Amudsen 3:60009a5ed1dc 187 while(1){
Amudsen 3:60009a5ed1dc 188 led3 = !led3;
Amudsen 3:60009a5ed1dc 189 //pc.printf("%f\n\r",revPerSec);
Amudsen 3:60009a5ed1dc 190 wait(2);
Amudsen 3:60009a5ed1dc 191 }
Amudsen 3:60009a5ed1dc 192 }
estott 0:de4320f74764 193 //Main
estott 0:de4320f74764 194 int main() {
Amudsen 3:60009a5ed1dc 195
Amudsen 3:60009a5ed1dc 196 Thread th1;
Amudsen 3:60009a5ed1dc 197 Thread controlLoopThread;
estott 2:4e88faab6988 198 int8_t orState = 0; //Rotot offset at motor state 0
estott 2:4e88faab6988 199
Amudsen 3:60009a5ed1dc 200 controller.setInputLimits(0, 200);
Amudsen 3:60009a5ed1dc 201 //Pwm output from 0.0 to 1.0
Amudsen 3:60009a5ed1dc 202 controller.setOutputLimits(0.0, 1.0);
Amudsen 3:60009a5ed1dc 203 //If there's a bias.
Amudsen 3:60009a5ed1dc 204 controller.setBias(0.0);
Amudsen 3:60009a5ed1dc 205 controller.setMode(0);
estott 0:de4320f74764 206 //Initialise the serial port
estott 0:de4320f74764 207 Serial pc(SERIAL_TX, SERIAL_RX);
estott 2:4e88faab6988 208 int8_t intState = 0;
estott 2:4e88faab6988 209 int8_t intStateOld = 0;
Amudsen 3:60009a5ed1dc 210 float per = 0.001f;
Amudsen 3:60009a5ed1dc 211 controller.setSetPoint(50.0);
Amudsen 3:60009a5ed1dc 212
Amudsen 3:60009a5ed1dc 213 L1L.period(per);
Amudsen 3:60009a5ed1dc 214 L1H.period(per);
Amudsen 3:60009a5ed1dc 215 L2L.period(per);
Amudsen 3:60009a5ed1dc 216 L2H.period(per);
Amudsen 3:60009a5ed1dc 217 L3L.period(per);
Amudsen 3:60009a5ed1dc 218 L3H.period(per);
Amudsen 3:60009a5ed1dc 219 dutyCycle = 1;
Amudsen 3:60009a5ed1dc 220 pc.printf("Device on \n\r");
estott 0:de4320f74764 221
estott 0:de4320f74764 222 //Run the motor synchronisation
estott 2:4e88faab6988 223 orState = motorHome();
estott 2:4e88faab6988 224 //orState is subtracted from future rotor state inputs to align rotor and motor states
Amudsen 3:60009a5ed1dc 225 //th2.set_priority(osPriorityRealtime);
Amudsen 3:60009a5ed1dc 226 //th1.start(blinkLED);
Amudsen 3:60009a5ed1dc 227 //th2.start(blinkLED3);
estott 0:de4320f74764 228
Amudsen 3:60009a5ed1dc 229 //define interrupts
Amudsen 3:60009a5ed1dc 230 //I1intr.rise(&mesRot); //start photoInterrupt
Amudsen 3:60009a5ed1dc 231 //I2intr.rise(&mesRot);
Amudsen 3:60009a5ed1dc 232 //I3intr.rise(&mesRot);
Amudsen 3:60009a5ed1dc 233 //I1intr.fall(&mesRot);
Amudsen 3:60009a5ed1dc 234 //I2intr.fall(&mesRot);
Amudsen 3:60009a5ed1dc 235 //I3intr.fall(&mesRot);
Amudsen 3:60009a5ed1dc 236
Amudsen 3:60009a5ed1dc 237 CHAintr.rise(&mesSpeedSlits);
Amudsen 3:60009a5ed1dc 238 CHBintr.rise(&mesSpeedSlits);
Amudsen 3:60009a5ed1dc 239 CHAintr.fall(&mesSpeedSlits);
Amudsen 3:60009a5ed1dc 240 CHBintr.fall(&mesSpeedSlits);
Amudsen 3:60009a5ed1dc 241
Amudsen 3:60009a5ed1dc 242 th1.start(printStatus);
Amudsen 3:60009a5ed1dc 243 controlLoopThread.start(controlLoop); //start conrol unit thread
Amudsen 3:60009a5ed1dc 244 t.start();
Amudsen 3:60009a5ed1dc 245 //tick.attach(&decrease, 10);
estott 0:de4320f74764 246 //Poll the rotor state and set the motor outputs accordingly to spin the motor
estott 1:184cb0870c04 247 while (1) {
estott 2:4e88faab6988 248 intState = readRotorState();
Amudsen 3:60009a5ed1dc 249 if (pc.readable()){
Amudsen 3:60009a5ed1dc 250 char buffer[128];
Amudsen 3:60009a5ed1dc 251
Amudsen 3:60009a5ed1dc 252 pc.gets(buffer, 6);
Amudsen 3:60009a5ed1dc 253 targetSpeed = atof(buffer);
Amudsen 3:60009a5ed1dc 254 //pc.printf("I got '%s'\n\r", buffer);
Amudsen 3:60009a5ed1dc 255 pc.printf("Also in float '%f'\n\r", targetSpeed);
Amudsen 3:60009a5ed1dc 256 controller.setSetPoint(targetSpeed);
Amudsen 3:60009a5ed1dc 257 orState = motorHome();
Amudsen 3:60009a5ed1dc 258 }
Amudsen 3:60009a5ed1dc 259
estott 2:4e88faab6988 260 if (intState != intStateOld) {
estott 2:4e88faab6988 261 intStateOld = intState;
Amudsen 3:60009a5ed1dc 262 motorOut((intState-orState+lead+6)%6, dutyCycle); //+6 to make sure the remainder is positive
estott 0:de4320f74764 263 }
estott 2:4e88faab6988 264 }
estott 0:de4320f74764 265 }
estott 0:de4320f74764 266
Amudsen 3:60009a5ed1dc 267
Amudsen 3:60009a5ed1dc 268 // hi