Edward Stott / Mbed OS ES_CW2_selftest

Self-test routine for Embedded Systems Coursework 2 Hardware

main.cpp@6:f95233916b61, 2019-02-28 (annotated)

Committer:
estott
Date:
Thu Feb 28 09:30:42 2019 +0000
Revision:
6:f95233916b61
Parent:
5:a01230d95321 
Removed old PWM mode

Who changed what in which revision?

UserRevisionLine numberNew contents of line
estott 0:de4320f74764 1 #include "mbed.h"
estott 0:de4320f74764 2
estott 0:de4320f74764 3 //Photointerrupter input pins
estott 4:1cb32cb438ee 4 #define I1pin D3
estott 4:1cb32cb438ee 5 #define I2pin D6
estott 4:1cb32cb438ee 6 #define I3pin D5
estott 2:4e88faab6988 7
estott 2:4e88faab6988 8 //Incremental encoder input pins
estott 4:1cb32cb438ee 9 #define CHApin D12
estott 4:1cb32cb438ee 10 #define CHBpin D11
estott 0:de4320f74764 11
estott 0:de4320f74764 12 //Motor Drive output pins //Mask in output byte
estott 4:1cb32cb438ee 13 #define L1Lpin D1 //0x01
estott 4:1cb32cb438ee 14 #define L1Hpin A3 //0x02
estott 4:1cb32cb438ee 15 #define L2Lpin D0 //0x04
estott 4:1cb32cb438ee 16 #define L2Hpin A6 //0x08
estott 4:1cb32cb438ee 17 #define L3Lpin D10 //0x10
estott 4:1cb32cb438ee 18 #define L3Hpin D2 //0x20
estott 4:1cb32cb438ee 19
estott 4:1cb32cb438ee 20 #define PWMpin D9
estott 4:1cb32cb438ee 21
estott 4:1cb32cb438ee 22 //Motor current sense
estott 4:1cb32cb438ee 23 #define MCSPpin A1
estott 4:1cb32cb438ee 24 #define MCSNpin A0
estott 0:de4320f74764 25
estott 0:de4320f74764 26 //Mapping from sequential drive states to motor phase outputs
estott 0:de4320f74764 27 /*
estott 0:de4320f74764 28 State L1 L2 L3
estott 0:de4320f74764 29 0 H - L
estott 0:de4320f74764 30 1 - H L
estott 0:de4320f74764 31 2 L H -
estott 0:de4320f74764 32 3 L - H
estott 0:de4320f74764 33 4 - L H
estott 0:de4320f74764 34 5 H L -
estott 0:de4320f74764 35 6 - - -
estott 0:de4320f74764 36 7 - - -
estott 0:de4320f74764 37 */
estott 0:de4320f74764 38 //Drive state to output table
estott 0:de4320f74764 39 const int8_t driveTable[] = {0x12,0x18,0x09,0x21,0x24,0x06,0x00,0x00};
estott 2:4e88faab6988 40
estott 0:de4320f74764 41 //Mapping from interrupter inputs to sequential rotor states. 0x00 and 0x07 are not valid
estott 2:4e88faab6988 42 const int8_t stateMap[] = {0x07,0x05,0x03,0x04,0x01,0x00,0x02,0x07};
estott 2:4e88faab6988 43 //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 44
estott 2:4e88faab6988 45 //Phase lead to make motor spin
estott 3:569b35e2a602 46 const int8_t lead = 2; //2 for forwards, -2 for backwards
estott 0:de4320f74764 47
estott 4:1cb32cb438ee 48 const int32_t MIN_CURRENT = 100000;
estott 4:1cb32cb438ee 49
estott 4:1cb32cb438ee 50 const int32_t PWM_PRD = 2000;
estott 4:1cb32cb438ee 51
estott 6:f95233916b61 52 const uint32_t ENC_TEST_PRE = 10000;
estott 6:f95233916b61 53 const uint32_t ENC_TEST_DUR = 20000;
estott 4:1cb32cb438ee 54
estott 4:1cb32cb438ee 55 const bool EXT_PWM = true;
estott 4:1cb32cb438ee 56
estott 4:1cb32cb438ee 57 float dc=0.0;
estott 4:1cb32cb438ee 58
estott 0:de4320f74764 59 //Status LED
estott 0:de4320f74764 60 DigitalOut led1(LED1);
estott 0:de4320f74764 61
estott 0:de4320f74764 62 //Photointerrupter inputs
estott 4:1cb32cb438ee 63 InterruptIn I1(I1pin);
estott 2:4e88faab6988 64 DigitalIn I2(I2pin);
estott 2:4e88faab6988 65 DigitalIn I3(I3pin);
estott 0:de4320f74764 66
estott 0:de4320f74764 67 //Motor Drive outputs
estott 0:de4320f74764 68 DigitalOut L1L(L1Lpin);
estott 4:1cb32cb438ee 69 DigitalOut L2L(L2Lpin);
estott 4:1cb32cb438ee 70 DigitalOut L3L(L3Lpin);
estott 0:de4320f74764 71 DigitalOut L1H(L1Hpin);
estott 0:de4320f74764 72 DigitalOut L2H(L2Hpin);
estott 0:de4320f74764 73 DigitalOut L3H(L3Hpin);
estott 0:de4320f74764 74
estott 4:1cb32cb438ee 75 AnalogIn MCSP(MCSPpin);
estott 4:1cb32cb438ee 76 AnalogIn MCSN(MCSNpin);
estott 4:1cb32cb438ee 77
estott 4:1cb32cb438ee 78 PwmOut MotorPWM(PWMpin);
estott 4:1cb32cb438ee 79
estott 4:1cb32cb438ee 80 //Encoder inputs
estott 4:1cb32cb438ee 81 InterruptIn CHA(CHApin);
estott 4:1cb32cb438ee 82 InterruptIn CHB(CHBpin);
estott 4:1cb32cb438ee 83
estott 5:a01230d95321 84 //Set a given drive state
estott 0:de4320f74764 85 void motorOut(int8_t driveState){
estott 0:de4320f74764 86
estott 2:4e88faab6988 87 //Lookup the output byte from the drive state.
estott 2:4e88faab6988 88 int8_t driveOut = driveTable[driveState & 0x07];
estott 2:4e88faab6988 89
estott 2:4e88faab6988 90 //Turn off first
estott 2:4e88faab6988 91 if (~driveOut & 0x01) L1L = 0;
estott 2:4e88faab6988 92 if (~driveOut & 0x02) L1H = 1;
estott 2:4e88faab6988 93 if (~driveOut & 0x04) L2L = 0;
estott 2:4e88faab6988 94 if (~driveOut & 0x08) L2H = 1;
estott 2:4e88faab6988 95 if (~driveOut & 0x10) L3L = 0;
estott 2:4e88faab6988 96 if (~driveOut & 0x20) L3H = 1;
estott 4:1cb32cb438ee 97
estott 2:4e88faab6988 98 //Then turn on
estott 2:4e88faab6988 99 if (driveOut & 0x01) L1L = 1;
estott 2:4e88faab6988 100 if (driveOut & 0x02) L1H = 0;
estott 2:4e88faab6988 101 if (driveOut & 0x04) L2L = 1;
estott 2:4e88faab6988 102 if (driveOut & 0x08) L2H = 0;
estott 2:4e88faab6988 103 if (driveOut & 0x10) L3L = 1;
estott 2:4e88faab6988 104 if (driveOut & 0x20) L3H = 0;
estott 0:de4320f74764 105 }
estott 0:de4320f74764 106
estott 2:4e88faab6988 107 //Convert photointerrupter inputs to a rotor state
estott 0:de4320f74764 108 inline int8_t readRotorState(){
estott 2:4e88faab6988 109 return stateMap[I1 + 2*I2 + 4*I3];
estott 0:de4320f74764 110 }
estott 0:de4320f74764 111
estott 4:1cb32cb438ee 112 uint32_t revCount,encCount,maxEncCount,minEncCount,badEdges,maxBadEdges;
estott 4:1cb32cb438ee 113 uint32_t encState,totalEncCount;
estott 4:1cb32cb438ee 114 void photoISR() {
estott 4:1cb32cb438ee 115
estott 4:1cb32cb438ee 116 if (encCount > maxEncCount) maxEncCount = encCount;
estott 4:1cb32cb438ee 117 if (encCount < minEncCount) minEncCount = encCount;
estott 4:1cb32cb438ee 118 if (badEdges > maxBadEdges) maxBadEdges = badEdges;
estott 4:1cb32cb438ee 119
estott 4:1cb32cb438ee 120 revCount++;
estott 4:1cb32cb438ee 121 totalEncCount += encCount;
estott 4:1cb32cb438ee 122 encCount = 0;
estott 4:1cb32cb438ee 123 badEdges = 0;
estott 4:1cb32cb438ee 124 }
estott 0:de4320f74764 125
estott 4:1cb32cb438ee 126 void encISR0() {
estott 4:1cb32cb438ee 127 if (encState == 3) encCount++;
estott 4:1cb32cb438ee 128 else badEdges++;
estott 4:1cb32cb438ee 129 encState = 0;
estott 4:1cb32cb438ee 130 }
estott 4:1cb32cb438ee 131 void encISR1() {
estott 4:1cb32cb438ee 132 if (encState == 0) encCount++;
estott 4:1cb32cb438ee 133 else badEdges++;
estott 4:1cb32cb438ee 134 encState = 1;
estott 4:1cb32cb438ee 135 }
estott 4:1cb32cb438ee 136 void encISR2() {
estott 4:1cb32cb438ee 137 if (encState == 1) encCount++;
estott 4:1cb32cb438ee 138 else badEdges++;
estott 4:1cb32cb438ee 139 encState = 2;
estott 4:1cb32cb438ee 140 }
estott 4:1cb32cb438ee 141 void encISR3() {
estott 4:1cb32cb438ee 142 if (encState == 2) encCount++;
estott 4:1cb32cb438ee 143 else badEdges++;
estott 4:1cb32cb438ee 144 encState = 3;
estott 4:1cb32cb438ee 145 }
estott 4:1cb32cb438ee 146
estott 0:de4320f74764 147
estott 0:de4320f74764 148 //Main
estott 0:de4320f74764 149 int main() {
estott 2:4e88faab6988 150 int8_t orState = 0; //Rotot offset at motor state 0
estott 3:569b35e2a602 151 int8_t intState = 0;
estott 3:569b35e2a602 152 int8_t intStateOld = 0;
estott 2:4e88faab6988 153
estott 0:de4320f74764 154 //Initialise the serial port
estott 4:1cb32cb438ee 155 RawSerial pc(USBTX, USBRX);
estott 0:de4320f74764 156 pc.printf("Hello\n\r");
estott 0:de4320f74764 157
estott 4:1cb32cb438ee 158 MotorPWM.period_us(PWM_PRD);
estott 4:1cb32cb438ee 159 MotorPWM.pulsewidth_us(PWM_PRD);
estott 4:1cb32cb438ee 160
estott 4:1cb32cb438ee 161 Timer testTimer;
estott 4:1cb32cb438ee 162 testTimer.start();
estott 4:1cb32cb438ee 163
estott 4:1cb32cb438ee 164 pc.printf("Testing drive and photointerrupters\n\r");
estott 4:1cb32cb438ee 165
estott 4:1cb32cb438ee 166 while (testTimer.read_ms() < 1000) {}
estott 4:1cb32cb438ee 167
estott 4:1cb32cb438ee 168 //Cycle through states to catch rotor
estott 4:1cb32cb438ee 169 for (int i=5;i>=-1;i--) {
estott 4:1cb32cb438ee 170 motorOut(i);
estott 4:1cb32cb438ee 171 testTimer.reset();
estott 4:1cb32cb438ee 172 while (testTimer.read_ms() < 1000) {}
estott 4:1cb32cb438ee 173 }
estott 4:1cb32cb438ee 174
estott 4:1cb32cb438ee 175 orState = readRotorState();
estott 4:1cb32cb438ee 176 pc.printf("PI origin %d\n\r",orState);
estott 4:1cb32cb438ee 177
estott 6:f95233916b61 178 //Test each state
estott 4:1cb32cb438ee 179 bool drivePass = true;
estott 4:1cb32cb438ee 180 bool PIPass = true;
estott 4:1cb32cb438ee 181 for (int i=0;i<6;i++) {
estott 4:1cb32cb438ee 182 motorOut(i);
estott 4:1cb32cb438ee 183 testTimer.reset();
estott 4:1cb32cb438ee 184 while (testTimer.read_ms() < 1000) {}
estott 4:1cb32cb438ee 185
estott 4:1cb32cb438ee 186 int32_t motorCurrent = (MCSP.read_u16()-MCSN.read_u16())*56; //Conversion to microamps
estott 4:1cb32cb438ee 187 printf("Drive State %d: PI input %d, Current %dmA\n\r",i,readRotorState(),motorCurrent/1000);
estott 4:1cb32cb438ee 188
estott 4:1cb32cb438ee 189 int8_t stateOffset = (readRotorState() - i + 6)%6;
estott 4:1cb32cb438ee 190 if (stateOffset != orState){
estott 4:1cb32cb438ee 191 printf(" Unexpected PI input\n\r");
estott 4:1cb32cb438ee 192 PIPass = false;
estott 4:1cb32cb438ee 193 }
estott 4:1cb32cb438ee 194 if (motorCurrent < MIN_CURRENT){
estott 4:1cb32cb438ee 195 printf(" Drive current too low\n\r");
estott 4:1cb32cb438ee 196 drivePass = false;
estott 4:1cb32cb438ee 197 }
estott 4:1cb32cb438ee 198
estott 4:1cb32cb438ee 199 }
estott 4:1cb32cb438ee 200 if (drivePass == false) {
estott 4:1cb32cb438ee 201 printf("Motor current fail\n\r");
estott 4:1cb32cb438ee 202 while (1) {}}
estott 4:1cb32cb438ee 203
estott 4:1cb32cb438ee 204 if (PIPass == false) {
estott 4:1cb32cb438ee 205 printf("Disc stuck or PI sensor fault\n\r");
estott 4:1cb32cb438ee 206 while (1) {}}
estott 4:1cb32cb438ee 207
estott 6:f95233916b61 208
estott 6:f95233916b61 209 printf("Finding maximum velocity\n\r");
estott 6:f95233916b61 210 I1.rise(&photoISR);
estott 4:1cb32cb438ee 211 testTimer.reset();
estott 4:1cb32cb438ee 212 intStateOld = readRotorState();
estott 4:1cb32cb438ee 213 motorOut((readRotorState()-orState+lead+6)%6); //+6 to make sure the remainder is positive
estott 4:1cb32cb438ee 214 int32_t velCount = 0;
estott 4:1cb32cb438ee 215 int32_t maxVel = 0;
estott 4:1cb32cb438ee 216 uint8_t findMax = 1;
estott 6:f95233916b61 217 revCount = 0;
estott 4:1cb32cb438ee 218
estott 4:1cb32cb438ee 219 while (findMax){
estott 4:1cb32cb438ee 220 intState = readRotorState();
estott 4:1cb32cb438ee 221 if (intState != intStateOld) {
estott 4:1cb32cb438ee 222 intStateOld = intState;
estott 4:1cb32cb438ee 223 motorOut((intState-orState+lead+6)%6); //+6 to make sure the remainder is positive
estott 6:f95233916b61 224 //if (intState == 0) velCount++;
estott 4:1cb32cb438ee 225 }
estott 4:1cb32cb438ee 226
estott 4:1cb32cb438ee 227 if (testTimer.read_ms() >= 1000){
estott 4:1cb32cb438ee 228 testTimer.reset();
estott 6:f95233916b61 229 if (revCount > maxVel) {
estott 6:f95233916b61 230 maxVel = revCount;
estott 4:1cb32cb438ee 231 } else {
estott 4:1cb32cb438ee 232 findMax = 0;
estott 4:1cb32cb438ee 233 }
estott 6:f95233916b61 234 revCount = 0;
estott 4:1cb32cb438ee 235 }
estott 4:1cb32cb438ee 236 }
estott 4:1cb32cb438ee 237
estott 4:1cb32cb438ee 238 printf("Maximum Velocity %d rps\n\r",maxVel);
estott 0:de4320f74764 239
estott 6:f95233916b61 240 //Poll the rotor state and set the motor outputs accordingly to spin the motor
estott 6:f95233916b61 241 float testDC = 1.0;
estott 4:1cb32cb438ee 242 for (dc=1.0; dc>0; dc-= 0.1){
estott 4:1cb32cb438ee 243 printf("Testing at %0.1f duty cycle...",dc);
estott 4:1cb32cb438ee 244 MotorPWM.write(dc);
estott 4:1cb32cb438ee 245 findMax = 1;
estott 4:1cb32cb438ee 246 testTimer.reset();
estott 6:f95233916b61 247 revCount = 0;
estott 4:1cb32cb438ee 248 while (findMax) {
estott 4:1cb32cb438ee 249 intState = readRotorState();
estott 4:1cb32cb438ee 250 if (intState != intStateOld) {
estott 4:1cb32cb438ee 251 intStateOld = intState;
estott 6:f95233916b61 252 motorOut((intState-orState+lead+6)%6);
estott 4:1cb32cb438ee 253 }
estott 4:1cb32cb438ee 254
estott 4:1cb32cb438ee 255 if (testTimer.read_ms() >= 1000){
estott 4:1cb32cb438ee 256 testTimer.reset();
estott 6:f95233916b61 257 if (revCount < maxVel) {
estott 6:f95233916b61 258 maxVel = revCount;
estott 4:1cb32cb438ee 259 } else {
estott 4:1cb32cb438ee 260 findMax = 0;
estott 4:1cb32cb438ee 261 }
estott 6:f95233916b61 262 revCount = 0;
estott 4:1cb32cb438ee 263 }
estott 4:1cb32cb438ee 264 }
estott 4:1cb32cb438ee 265 printf("%d rps\n\r",maxVel);
estott 6:f95233916b61 266 if (maxVel > 20) testDC = dc;
estott 4:1cb32cb438ee 267 if (maxVel == 0) dc = 0.0;
estott 4:1cb32cb438ee 268 }
estott 4:1cb32cb438ee 269
estott 4:1cb32cb438ee 270
estott 4:1cb32cb438ee 271 //Test encoder
estott 4:1cb32cb438ee 272
estott 6:f95233916b61 273 MotorPWM.write(1.0);
estott 6:f95233916b61 274 testTimer.reset();
estott 6:f95233916b61 275 while (testTimer.read_ms() < 1000) {
estott 6:f95233916b61 276 intState = readRotorState();
estott 6:f95233916b61 277 if (intState != intStateOld) {
estott 6:f95233916b61 278 intStateOld = intState;
estott 6:f95233916b61 279 motorOut((intState-orState+lead+6)%6);
estott 6:f95233916b61 280 }
estott 6:f95233916b61 281 }
estott 6:f95233916b61 282 dc=testDC;
estott 4:1cb32cb438ee 283 printf("Testing encoder. Duty cycle = %0.1f\n\r",dc);
estott 4:1cb32cb438ee 284 MotorPWM.write(dc);
estott 4:1cb32cb438ee 285
estott 4:1cb32cb438ee 286 revCount = 0;
estott 4:1cb32cb438ee 287
estott 4:1cb32cb438ee 288 CHA.rise(&encISR0);
estott 4:1cb32cb438ee 289 CHB.rise(&encISR1);
estott 4:1cb32cb438ee 290 CHA.fall(&encISR2);
estott 4:1cb32cb438ee 291 CHB.fall(&encISR3);
estott 4:1cb32cb438ee 292
estott 4:1cb32cb438ee 293 motorOut((readRotorState()-orState+lead+6)%6); //+6 to make sure the remainder is positive
estott 4:1cb32cb438ee 294
estott 6:f95233916b61 295 testTimer.reset();
estott 6:f95233916b61 296 while (testTimer.read_ms() < ENC_TEST_PRE) {
estott 4:1cb32cb438ee 297 intState = readRotorState();
estott 4:1cb32cb438ee 298 if (intState != intStateOld) {
estott 4:1cb32cb438ee 299 intStateOld = intState;
estott 4:1cb32cb438ee 300 motorOut((intState-orState+lead+6)%6);
estott 4:1cb32cb438ee 301 }
estott 4:1cb32cb438ee 302 }
estott 4:1cb32cb438ee 303
estott 4:1cb32cb438ee 304 maxEncCount = 0;
estott 4:1cb32cb438ee 305 minEncCount = -1;
estott 4:1cb32cb438ee 306 maxBadEdges = 0;
estott 4:1cb32cb438ee 307 revCount = 0;
estott 4:1cb32cb438ee 308 totalEncCount = 0;
estott 4:1cb32cb438ee 309
estott 6:f95233916b61 310 testTimer.reset();
estott 6:f95233916b61 311 while (testTimer.read_ms() < ENC_TEST_DUR) {
estott 4:1cb32cb438ee 312 intState = readRotorState();
estott 4:1cb32cb438ee 313 if (intState != intStateOld) {
estott 4:1cb32cb438ee 314 intStateOld = intState;
estott 4:1cb32cb438ee 315 motorOut((intState-orState+lead+6)%6);
estott 4:1cb32cb438ee 316 }
estott 4:1cb32cb438ee 317 }
estott 4:1cb32cb438ee 318
estott 4:1cb32cb438ee 319 I1.rise(NULL);
estott 6:f95233916b61 320 printf("Min, Mean, Max encoder count = %d, %d, %d\n\r",minEncCount,totalEncCount/revCount,maxEncCount);
estott 4:1cb32cb438ee 321 printf("Max bad transitions = %d\n\r",maxBadEdges);
estott 4:1cb32cb438ee 322
estott 1:184cb0870c04 323 while (1) {
estott 2:4e88faab6988 324 intState = readRotorState();
estott 2:4e88faab6988 325 if (intState != intStateOld) {
estott 2:4e88faab6988 326 intStateOld = intState;
estott 4:1cb32cb438ee 327 motorOut((intState-orState+lead+6)%6);
estott 0:de4320f74764 328 }
estott 2:4e88faab6988 329 }
estott 4:1cb32cb438ee 330
estott 0:de4320f74764 331 }
estott 0:de4320f74764 332