Internet of Tings / Mbed OS motor-mining

An embedded device

Dependencies:   Crypto

main.cpp@4:1cb32cb438ee, 2019-02-19 (annotated)

Committer:
estott
Date:
Tue Feb 19 09:19:30 2019 +0000
Revision:
4:1cb32cb438ee
Parent:
3:569b35e2a602 
Made a self-test routine for ES CW2

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