Embedded Systems / Mbed OS Eds_code

New stuff

Fork of ES_CW2_Starter by Edward Stott

main.cpp@3:620fbdd7b6c6, 2017-03-24 (annotated)

Committer:
Bachoru
Date:
Fri Mar 24 00:48:47 2017 +0000
Revision:
3:620fbdd7b6c6
Parent:
2:4e88faab6988 
New stuff

Who changed what in which revision?

UserRevisionLine numberNew contents of line
estott 0:de4320f74764 1 #include "mbed.h"
Bachoru 3:620fbdd7b6c6 2
Bachoru 3:620fbdd7b6c6 3 /////////////////////
Bachoru 3:620fbdd7b6c6 4 // Pin definitions //
Bachoru 3:620fbdd7b6c6 5 /////////////////////
estott 0:de4320f74764 6
estott 0:de4320f74764 7 //Photointerrupter input pins
estott 0:de4320f74764 8 #define I1pin D2
estott 2:4e88faab6988 9 #define I2pin D11
estott 2:4e88faab6988 10 #define I3pin D12
estott 2:4e88faab6988 11
Bachoru 3:620fbdd7b6c6 12 DigitalIn I1(I1pin);
Bachoru 3:620fbdd7b6c6 13 DigitalIn I2(I2pin);
Bachoru 3:620fbdd7b6c6 14 DigitalIn I3(I3pin);
Bachoru 3:620fbdd7b6c6 15
Bachoru 3:620fbdd7b6c6 16 InterruptIn I1intr(I1pin);
Bachoru 3:620fbdd7b6c6 17 InterruptIn I2intr(I2pin);
Bachoru 3:620fbdd7b6c6 18 InterruptIn I3intr(I3pin);
Bachoru 3:620fbdd7b6c6 19
estott 2:4e88faab6988 20 //Incremental encoder input pins
estott 2:4e88faab6988 21 #define CHA D7
estott 2:4e88faab6988 22 #define CHB D8
estott 0:de4320f74764 23
Bachoru 3:620fbdd7b6c6 24 DigitalIn CHAR(CHA);
Bachoru 3:620fbdd7b6c6 25 DigitalIn CHBR(CHB);
Bachoru 3:620fbdd7b6c6 26
Bachoru 3:620fbdd7b6c6 27 InterruptIn CHAintr(CHA);
Bachoru 3:620fbdd7b6c6 28 InterruptIn CHBintr(CHB);
Bachoru 3:620fbdd7b6c6 29
Bachoru 3:620fbdd7b6c6 30 //Status LED
Bachoru 3:620fbdd7b6c6 31 DigitalOut led1 (LED1);
Bachoru 3:620fbdd7b6c6 32 DigitalOut led2 (LED2);
Bachoru 3:620fbdd7b6c6 33 DigitalOut led3 (LED3);
Bachoru 3:620fbdd7b6c6 34
estott 0:de4320f74764 35 //Motor Drive output pins //Mask in output byte
estott 2:4e88faab6988 36 #define L1Lpin D4 //0x01
estott 2:4e88faab6988 37 #define L1Hpin D5 //0x02
estott 2:4e88faab6988 38 #define L2Lpin D3 //0x04
estott 2:4e88faab6988 39 #define L2Hpin D6 //0x08
estott 2:4e88faab6988 40 #define L3Lpin D9 //0x10
estott 0:de4320f74764 41 #define L3Hpin D10 //0x20
estott 0:de4320f74764 42
Bachoru 3:620fbdd7b6c6 43 PwmOut L1L(L1Lpin);
Bachoru 3:620fbdd7b6c6 44 PwmOut L1H(L1Hpin);
Bachoru 3:620fbdd7b6c6 45 PwmOut L2L(L2Lpin);
Bachoru 3:620fbdd7b6c6 46 PwmOut L2H(L2Hpin);
Bachoru 3:620fbdd7b6c6 47 PwmOut L3L(L3Lpin);
Bachoru 3:620fbdd7b6c6 48 PwmOut L3H(L3Hpin);
Bachoru 3:620fbdd7b6c6 49
estott 0:de4320f74764 50 //Mapping from sequential drive states to motor phase outputs
estott 0:de4320f74764 51 /*
estott 0:de4320f74764 52 State L1 L2 L3
estott 0:de4320f74764 53 0 H - L
estott 0:de4320f74764 54 1 - H L
estott 0:de4320f74764 55 2 L H -
estott 0:de4320f74764 56 3 L - H
estott 0:de4320f74764 57 4 - L H
estott 0:de4320f74764 58 5 H L -
estott 0:de4320f74764 59 6 - - -
estott 0:de4320f74764 60 7 - - -
estott 0:de4320f74764 61 */
Bachoru 3:620fbdd7b6c6 62
estott 0:de4320f74764 63 //Drive state to output table
estott 0:de4320f74764 64 const int8_t driveTable[] = {0x12,0x18,0x09,0x21,0x24,0x06,0x00,0x00};
estott 2:4e88faab6988 65
estott 0:de4320f74764 66 //Mapping from interrupter inputs to sequential rotor states. 0x00 and 0x07 are not valid
estott 2:4e88faab6988 67 const int8_t stateMap[] = {0x07,0x05,0x03,0x04,0x01,0x00,0x02,0x07};
Bachoru 3:620fbdd7b6c6 68 const int8_t stateMapReversed[] = {0x07,0x01,0x03,0x02,0x05,0x00,0x04,0x07}; //Alternative if phase order of input or drive is reversed
estott 2:4e88faab6988 69
estott 2:4e88faab6988 70 //Phase lead to make motor spin
Bachoru 3:620fbdd7b6c6 71 volatile int8_t lead = 2; //2 for forwards, -2 for backwards
Bachoru 3:620fbdd7b6c6 72
Bachoru 3:620fbdd7b6c6 73
Bachoru 3:620fbdd7b6c6 74
Bachoru 3:620fbdd7b6c6 75 //////////////////////
Bachoru 3:620fbdd7b6c6 76 // Global variables //
Bachoru 3:620fbdd7b6c6 77 //////////////////////
estott 0:de4320f74764 78
Bachoru 3:620fbdd7b6c6 79 volatile float targetSpeed; // User defined instantaneous speed
Bachoru 3:620fbdd7b6c6 80 volatile float targetPosition;
Bachoru 3:620fbdd7b6c6 81 volatile float revPerSec = 0; // Instantaneous speed estimate
Bachoru 3:620fbdd7b6c6 82 volatile float slitPosition = 0;
Bachoru 3:620fbdd7b6c6 83 volatile float photoPosition = 0;
Bachoru 3:620fbdd7b6c6 84 volatile float positionEstimate = 0;
Bachoru 3:620fbdd7b6c6 85 volatile float previousTime = 0; // Used to calculate revPerSec
Bachoru 3:620fbdd7b6c6 86 volatile float previousSpeed = 0; // Previous value of revPerSec
Bachoru 3:620fbdd7b6c6 87 volatile float errorPrev = 0.0; //Previous value of error
Bachoru 3:620fbdd7b6c6 88 volatile float integralError = 0.0; //Integral of the error
Bachoru 3:620fbdd7b6c6 89 volatile float dutyCycle; // PWM duty cycle between 0 and 1
Bachoru 3:620fbdd7b6c6 90 volatile float timeMap[] = {0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 }; // Keep track of last state
Bachoru 3:620fbdd7b6c6 91 Timer timer; // To keep track of time
Bachoru 3:620fbdd7b6c6 92 Serial pc(SERIAL_TX,SERIAL_RX); // Serial connection
Bachoru 3:620fbdd7b6c6 93
estott 0:de4320f74764 94
Bachoru 3:620fbdd7b6c6 95 ///////////////
Bachoru 3:620fbdd7b6c6 96 // Functions //
Bachoru 3:620fbdd7b6c6 97 ///////////////
Bachoru 3:620fbdd7b6c6 98
Bachoru 3:620fbdd7b6c6 99 float parseNumber (int position, int *new_pos, char *inPut){
Bachoru 3:620fbdd7b6c6 100 char number[8];
Bachoru 3:620fbdd7b6c6 101 int count = position;
Bachoru 3:620fbdd7b6c6 102
Bachoru 3:620fbdd7b6c6 103 while((inPut[count] > '0' && inPut[count] < '9') || inPut[count] == '-' || inPut[count] == '.')
Bachoru 3:620fbdd7b6c6 104 {
Bachoru 3:620fbdd7b6c6 105 count ++;
Bachoru 3:620fbdd7b6c6 106 }
Bachoru 3:620fbdd7b6c6 107 for (int i = position; i < count; i++)
Bachoru 3:620fbdd7b6c6 108 {
Bachoru 3:620fbdd7b6c6 109 number[i-position] = inPut[i];
Bachoru 3:620fbdd7b6c6 110 }
Bachoru 3:620fbdd7b6c6 111 number[count] = '\0';
Bachoru 3:620fbdd7b6c6 112 *new_pos = count;
Bachoru 3:620fbdd7b6c6 113 return atof(number);
Bachoru 3:620fbdd7b6c6 114 }
Bachoru 3:620fbdd7b6c6 115 void parseTones(char *inPut)
Bachoru 3:620fbdd7b6c6 116 {
Bachoru 3:620fbdd7b6c6 117 int count = 1;
Bachoru 3:620fbdd7b6c6 118 while (inPut[count] != 13)
Bachoru 3:620fbdd7b6c6 119 {
Bachoru 3:620fbdd7b6c6 120 //Depends on the format of the tone output
Bachoru 3:620fbdd7b6c6 121 }
Bachoru 3:620fbdd7b6c6 122 }
estott 0:de4320f74764 123
Bachoru 3:620fbdd7b6c6 124 // THREAD: Parse input
Bachoru 3:620fbdd7b6c6 125 void parseRegex(char *inPut, float *speed, float *rev){
Bachoru 3:620fbdd7b6c6 126
Bachoru 3:620fbdd7b6c6 127 int pos_pointer = 0;
Bachoru 3:620fbdd7b6c6 128 switch (inPut[0])
Bachoru 3:620fbdd7b6c6 129 {
Bachoru 3:620fbdd7b6c6 130 case 'R':
Bachoru 3:620fbdd7b6c6 131 //printf("%d\r\n",pos_pointer);
Bachoru 3:620fbdd7b6c6 132 *rev = parseNumber(1, &pos_pointer, inPut);
Bachoru 3:620fbdd7b6c6 133 //printf("%d\r\n",pos_pointer);
Bachoru 3:620fbdd7b6c6 134 if (inPut[pos_pointer] == 'V'){
Bachoru 3:620fbdd7b6c6 135 //both R and V defined
Bachoru 3:620fbdd7b6c6 136 *speed = parseNumber(pos_pointer+1, &pos_pointer, inPut);
Bachoru 3:620fbdd7b6c6 137 // printf("%d\r\n",pos_pointer);
Bachoru 3:620fbdd7b6c6 138 }
Bachoru 3:620fbdd7b6c6 139 else if (inPut[pos_pointer] != '\r')
Bachoru 3:620fbdd7b6c6 140 {
Bachoru 3:620fbdd7b6c6 141 pc.printf("Invalid syntax");
Bachoru 3:620fbdd7b6c6 142 }
Bachoru 3:620fbdd7b6c6 143
Bachoru 3:620fbdd7b6c6 144 break;
Bachoru 3:620fbdd7b6c6 145 case 'V':
Bachoru 3:620fbdd7b6c6 146 *speed = parseNumber(1, &pos_pointer, inPut);
Bachoru 3:620fbdd7b6c6 147 break;
Bachoru 3:620fbdd7b6c6 148 case 'T':
Bachoru 3:620fbdd7b6c6 149 parseTones(inPut);
Bachoru 3:620fbdd7b6c6 150 break;
Bachoru 3:620fbdd7b6c6 151 default:
Bachoru 3:620fbdd7b6c6 152 pc.printf("Invalid syntax");
Bachoru 3:620fbdd7b6c6 153 //wrong format
Bachoru 3:620fbdd7b6c6 154 break;
Bachoru 3:620fbdd7b6c6 155 }
Bachoru 3:620fbdd7b6c6 156 pc.printf("Revolutions entered: %f, Desired speed: %f\n\r", rev, speed);
Bachoru 3:620fbdd7b6c6 157
Bachoru 3:620fbdd7b6c6 158 }
Bachoru 3:620fbdd7b6c6 159
Bachoru 3:620fbdd7b6c6 160 // THREAD: Print instantaneous speed
Bachoru 3:620fbdd7b6c6 161 void printStatus() {
Bachoru 3:620fbdd7b6c6 162 while(1){
Bachoru 3:620fbdd7b6c6 163 led3 = !led3;
Bachoru 3:620fbdd7b6c6 164 pc.printf("%f\n\r",revPerSec);
Bachoru 3:620fbdd7b6c6 165 wait(2);
Bachoru 3:620fbdd7b6c6 166 }
Bachoru 3:620fbdd7b6c6 167 }
estott 0:de4320f74764 168
Bachoru 3:620fbdd7b6c6 169 // THREAD: Control loop
Bachoru 3:620fbdd7b6c6 170 void controlSpeed(float *targetSpeed){
Bachoru 3:620fbdd7b6c6 171 while(true){
Bachoru 3:620fbdd7b6c6 172 float error = *targetSpeed - revPerSec;
Bachoru 3:620fbdd7b6c6 173 float errorDer = (error - errorPrev);
Bachoru 3:620fbdd7b6c6 174 integralError += error;
Bachoru 3:620fbdd7b6c6 175 float k = 0.4;
Bachoru 3:620fbdd7b6c6 176 float kd = 0.0;
Bachoru 3:620fbdd7b6c6 177 float ki = 0.00015;
Bachoru 3:620fbdd7b6c6 178 dutyCycle = k*error + ki*integralError;
Bachoru 3:620fbdd7b6c6 179
Bachoru 3:620fbdd7b6c6 180 errorPrev = error; //remeber error
Bachoru 3:620fbdd7b6c6 181 wait(0.01);
Bachoru 3:620fbdd7b6c6 182 }
Bachoru 3:620fbdd7b6c6 183 }
Bachoru 3:620fbdd7b6c6 184
Bachoru 3:620fbdd7b6c6 185 // THREAD: Control loop
Bachoru 3:620fbdd7b6c6 186 void controlPosition(float *targetPosition){
Bachoru 3:620fbdd7b6c6 187 while(true){
Bachoru 3:620fbdd7b6c6 188 positionEstimate = photoPosition + slitPosition;
Bachoru 3:620fbdd7b6c6 189 float error = *targetPosition - positionEstimate;
Bachoru 3:620fbdd7b6c6 190 float errorDer = (error - errorPrev);
Bachoru 3:620fbdd7b6c6 191 float k = 1.0;
Bachoru 3:620fbdd7b6c6 192 float kd = 240.0;
Bachoru 3:620fbdd7b6c6 193 float limit = 0.5;
Bachoru 3:620fbdd7b6c6 194 if (error < 50.0){k = 10.0; limit = 0.2;}
Bachoru 3:620fbdd7b6c6 195 dutyCycle = k*error + kd*errorDer;
Bachoru 3:620fbdd7b6c6 196 if (dutyCycle < 0) {lead = -2; led1 = 0;} else {lead = 2; led1 = 1;}
Bachoru 3:620fbdd7b6c6 197 if (dutyCycle > limit) {dutyCycle = limit;}
Bachoru 3:620fbdd7b6c6 198 errorPrev = error; //remeber error
Bachoru 3:620fbdd7b6c6 199 wait(0.001);
Bachoru 3:620fbdd7b6c6 200 }
Bachoru 3:620fbdd7b6c6 201 }
Bachoru 3:620fbdd7b6c6 202
Bachoru 3:620fbdd7b6c6 203 // Measure speed using slits
Bachoru 3:620fbdd7b6c6 204 void measureSpeedSlits(){
Bachoru 3:620fbdd7b6c6 205 float currentTime = timer.read();
Bachoru 3:620fbdd7b6c6 206 revPerSec = 0.4*previousSpeed + 0.6*((0.0021367521)/(currentTime-previousTime));
Bachoru 3:620fbdd7b6c6 207 previousTime = currentTime;
Bachoru 3:620fbdd7b6c6 208 previousSpeed = revPerSec;
Bachoru 3:620fbdd7b6c6 209 }
Bachoru 3:620fbdd7b6c6 210
Bachoru 3:620fbdd7b6c6 211 void measurePositionSlits(){
Bachoru 3:620fbdd7b6c6 212 slitPosition += 0.0021367521;
Bachoru 3:620fbdd7b6c6 213 }
Bachoru 3:620fbdd7b6c6 214
Bachoru 3:620fbdd7b6c6 215 // Measure speed using photointerrupters
Bachoru 3:620fbdd7b6c6 216 void measureSpeedPhoto(){
Bachoru 3:620fbdd7b6c6 217 led3 = !led3;
Bachoru 3:620fbdd7b6c6 218 float speedTime;
Bachoru 3:620fbdd7b6c6 219 speedTime = timer.read();
Bachoru 3:620fbdd7b6c6 220 revPerSec = 1.0/(speedTime - timeMap[I1 + 2*I2 + 4*I3]);
Bachoru 3:620fbdd7b6c6 221 timeMap[I1 + 2*I2 + 4*I3] = speedTime;
Bachoru 3:620fbdd7b6c6 222 }
Bachoru 3:620fbdd7b6c6 223
Bachoru 3:620fbdd7b6c6 224 void measurePositionPhoto(){
Bachoru 3:620fbdd7b6c6 225 photoPosition += 0.16666666;
Bachoru 3:620fbdd7b6c6 226 slitPosition = 0.0;
Bachoru 3:620fbdd7b6c6 227 }
estott 0:de4320f74764 228
Bachoru 3:620fbdd7b6c6 229 // Set motor states
Bachoru 3:620fbdd7b6c6 230 void motorOut(int8_t driveState, float dutyCycle){
estott 2:4e88faab6988 231 //Lookup the output byte from the drive state.
estott 2:4e88faab6988 232 int8_t driveOut = driveTable[driveState & 0x07];
estott 2:4e88faab6988 233
estott 2:4e88faab6988 234 //Turn off first
Bachoru 3:620fbdd7b6c6 235 if (~driveOut & 0x01) L1L.write(0); // = 0
Bachoru 3:620fbdd7b6c6 236 if (~driveOut & 0x02) L1H.write(dutyCycle); // = 1;
Bachoru 3:620fbdd7b6c6 237 if (~driveOut & 0x04) L2L.write(0); // = 0;
Bachoru 3:620fbdd7b6c6 238 if (~driveOut & 0x08) L2H.write(dutyCycle); // = 1;
Bachoru 3:620fbdd7b6c6 239 if (~driveOut & 0x10) L3L.write(0); // = 0;
Bachoru 3:620fbdd7b6c6 240 if (~driveOut & 0x20) L3H.write(dutyCycle); // = 1;
estott 2:4e88faab6988 241
estott 2:4e88faab6988 242 //Then turn on
Bachoru 3:620fbdd7b6c6 243 if (driveOut & 0x01) L1L.write(dutyCycle); // = 1;
Bachoru 3:620fbdd7b6c6 244 if (driveOut & 0x02) L1H.write(0); // = 0;
Bachoru 3:620fbdd7b6c6 245 if (driveOut & 0x04) L2L.write(dutyCycle); // = 1;
Bachoru 3:620fbdd7b6c6 246 if (driveOut & 0x08) L2H.write(0); // = 0;
Bachoru 3:620fbdd7b6c6 247 if (driveOut & 0x10) L3L.write(dutyCycle); // = 1;
Bachoru 3:620fbdd7b6c6 248 if (driveOut & 0x20) L3H.write(0); // = 0;
Bachoru 3:620fbdd7b6c6 249 }
estott 0:de4320f74764 250
Bachoru 3:620fbdd7b6c6 251 //Convert photointerrupter inputs to a rotor state
estott 0:de4320f74764 252 inline int8_t readRotorState(){
Bachoru 3:620fbdd7b6c6 253 if (lead > 0){return stateMap[I1 + 2*I2 + 4*I3];}
Bachoru 3:620fbdd7b6c6 254 else {return stateMapReversed[I1 + 2*I2 + 4*I3];}
Bachoru 3:620fbdd7b6c6 255 //return stateMap[I1 + 2*I2 + 4*I3];
Bachoru 3:620fbdd7b6c6 256 }
estott 0:de4320f74764 257
estott 0:de4320f74764 258 //Basic synchronisation routine
estott 2:4e88faab6988 259 int8_t motorHome() {
estott 0:de4320f74764 260 //Put the motor in drive state 0 and wait for it to stabilise
Bachoru 3:620fbdd7b6c6 261 motorOut(0,1);
Bachoru 3:620fbdd7b6c6 262 wait(7.0);
estott 0:de4320f74764 263
estott 0:de4320f74764 264 //Get the rotor state
estott 2:4e88faab6988 265 return readRotorState();
estott 0:de4320f74764 266 }
Bachoru 3:620fbdd7b6c6 267
Bachoru 3:620fbdd7b6c6 268
Bachoru 3:620fbdd7b6c6 269 //////////
Bachoru 3:620fbdd7b6c6 270 // Main //
Bachoru 3:620fbdd7b6c6 271 //////////
Bachoru 3:620fbdd7b6c6 272
Bachoru 3:620fbdd7b6c6 273 int main() {
estott 0:de4320f74764 274
Bachoru 3:620fbdd7b6c6 275 // Initialize threads
Bachoru 3:620fbdd7b6c6 276 Thread speedControlThread(osPriorityNormal, DEFAULT_STACK_SIZE/4);
Bachoru 3:620fbdd7b6c6 277 Thread positionControlThread(osPriorityNormal, DEFAULT_STACK_SIZE/4);
Bachoru 3:620fbdd7b6c6 278 Thread playTones(osPriorityNormal, DEFAULT_STACK_SIZE/4);
Bachoru 3:620fbdd7b6c6 279
Bachoru 3:620fbdd7b6c6 280 timer.start();
Bachoru 3:620fbdd7b6c6 281
Bachoru 3:620fbdd7b6c6 282 //Initialize the serial port
Bachoru 3:620fbdd7b6c6 283 Serial pc(SERIAL_TX, SERIAL_RX);
Bachoru 3:620fbdd7b6c6 284 pc.printf("Device on \n\r");
Bachoru 3:620fbdd7b6c6 285
estott 2:4e88faab6988 286 int8_t orState = 0; //Rotot offset at motor state 0
estott 2:4e88faab6988 287 int8_t intState = 0;
estott 2:4e88faab6988 288 int8_t intStateOld = 0;
Bachoru 3:620fbdd7b6c6 289
Bachoru 3:620fbdd7b6c6 290 float PWM_period = 0.001f;
Bachoru 3:620fbdd7b6c6 291
Bachoru 3:620fbdd7b6c6 292 L1L.period(PWM_period);
Bachoru 3:620fbdd7b6c6 293 L1H.period(PWM_period);
Bachoru 3:620fbdd7b6c6 294 L2L.period(PWM_period);
Bachoru 3:620fbdd7b6c6 295 L2H.period(PWM_period);
Bachoru 3:620fbdd7b6c6 296 L3L.period(PWM_period);
Bachoru 3:620fbdd7b6c6 297 L3H.period(PWM_period);
Bachoru 3:620fbdd7b6c6 298
Bachoru 3:620fbdd7b6c6 299 // Slits interrupts
Bachoru 3:620fbdd7b6c6 300 CHAintr.rise(&measurePositionSlits);
Bachoru 3:620fbdd7b6c6 301 CHBintr.rise(&measurePositionSlits);
Bachoru 3:620fbdd7b6c6 302 CHAintr.fall(&measurePositionSlits);
Bachoru 3:620fbdd7b6c6 303 CHBintr.fall(&measurePositionSlits);
estott 0:de4320f74764 304
estott 0:de4320f74764 305 //Run the motor synchronisation
estott 2:4e88faab6988 306 orState = motorHome();
Bachoru 3:620fbdd7b6c6 307
Bachoru 3:620fbdd7b6c6 308 // USE FOR PHOTOINTERRUPTERS
Bachoru 3:620fbdd7b6c6 309 // Photointerrupters interrupts
Bachoru 3:620fbdd7b6c6 310 I1intr.rise(&measurePositionPhoto);
Bachoru 3:620fbdd7b6c6 311 I2intr.rise(&measurePositionPhoto);
Bachoru 3:620fbdd7b6c6 312 I3intr.rise(&measurePositionPhoto);
Bachoru 3:620fbdd7b6c6 313 I1intr.fall(&measurePositionPhoto);
Bachoru 3:620fbdd7b6c6 314 I2intr.fall(&measurePositionPhoto);
Bachoru 3:620fbdd7b6c6 315 I3intr.fall(&measurePositionPhoto);
estott 0:de4320f74764 316
estott 0:de4320f74764 317 //Poll the rotor state and set the motor outputs accordingly to spin the motor
estott 1:184cb0870c04 318 while (1) {
Bachoru 3:620fbdd7b6c6 319
Bachoru 3:620fbdd7b6c6 320 // Read serial input
Bachoru 3:620fbdd7b6c6 321 if (pc.readable()){
Bachoru 3:620fbdd7b6c6 322 speedControlThread.terminate();
Bachoru 3:620fbdd7b6c6 323 positionControlThread.terminate();
Bachoru 3:620fbdd7b6c6 324
Bachoru 3:620fbdd7b6c6 325 int c = 0;
Bachoru 3:620fbdd7b6c6 326 char input[50];
Bachoru 3:620fbdd7b6c6 327 char tmp;
Bachoru 3:620fbdd7b6c6 328 float rev = 0;
Bachoru 3:620fbdd7b6c6 329 float speed = 0;
Bachoru 3:620fbdd7b6c6 330
Bachoru 3:620fbdd7b6c6 331 while((tmp = pc.getc()) != '\r'){
Bachoru 3:620fbdd7b6c6 332 input[c] = tmp;
Bachoru 3:620fbdd7b6c6 333 pc.printf("%c",tmp);
Bachoru 3:620fbdd7b6c6 334 c++;
Bachoru 3:620fbdd7b6c6 335 }
Bachoru 3:620fbdd7b6c6 336
Bachoru 3:620fbdd7b6c6 337 input[c] = '\r';
Bachoru 3:620fbdd7b6c6 338 parseRegex(input, &speed, &rev);
Bachoru 3:620fbdd7b6c6 339 orState = motorHome();
Bachoru 3:620fbdd7b6c6 340 intState = 0;
Bachoru 3:620fbdd7b6c6 341 intStateOld = 0;
Bachoru 3:620fbdd7b6c6 342 integralError = 0.0;
Bachoru 3:620fbdd7b6c6 343
Bachoru 3:620fbdd7b6c6 344 if (rev == 0.0 && speed != 0.0){
Bachoru 3:620fbdd7b6c6 345 speedControlThread.start(callback(controlSpeed, &speed));
Bachoru 3:620fbdd7b6c6 346 }
Bachoru 3:620fbdd7b6c6 347
Bachoru 3:620fbdd7b6c6 348 else if (rev != 0.0 && speed == 0.0){
Bachoru 3:620fbdd7b6c6 349 slitPosition = 0.0;
Bachoru 3:620fbdd7b6c6 350 photoPosition = 0.0;
Bachoru 3:620fbdd7b6c6 351 positionEstimate = 0.0;
Bachoru 3:620fbdd7b6c6 352 positionControlThread.start(callback(controlPosition, &rev));
Bachoru 3:620fbdd7b6c6 353 }
Bachoru 3:620fbdd7b6c6 354 }
Bachoru 3:620fbdd7b6c6 355
estott 2:4e88faab6988 356 intState = readRotorState();
estott 2:4e88faab6988 357 if (intState != intStateOld) {
estott 2:4e88faab6988 358 intStateOld = intState;
Bachoru 3:620fbdd7b6c6 359 motorOut((intState-orState+2+6)%6, dutyCycle); //+6 to make sure the remainder is positive
estott 0:de4320f74764 360 }
estott 2:4e88faab6988 361 }
Bachoru 3:620fbdd7b6c6 362 }