Edward Stott
/
ES_CW2_selftest
Self-test routine for Embedded Systems Coursework 2 Hardware
Revision 6:f95233916b61, committed 2019-02-28
- Comitter:
- estott
- Date:
- Thu Feb 28 09:30:42 2019 +0000
- Parent:
- 5:a01230d95321
- Commit message:
- Removed old PWM mode
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Tue Feb 19 10:12:57 2019 +0000 +++ b/main.cpp Thu Feb 28 09:30:42 2019 +0000 @@ -49,7 +49,8 @@ const int32_t PWM_PRD = 2000; -const uint32_t ENC_TEST_REVS = 1000; +const uint32_t ENC_TEST_PRE = 10000; +const uint32_t ENC_TEST_DUR = 20000; const bool EXT_PWM = true; @@ -108,36 +109,23 @@ return stateMap[I1 + 2*I2 + 4*I3]; } - //ISRs for counting optical inputs uint32_t revCount,encCount,maxEncCount,minEncCount,badEdges,maxBadEdges; uint32_t encState,totalEncCount; - - //Rising edge of I1 - occurs once per revolution void photoISR() { - //Update min and max statistics if (encCount > maxEncCount) maxEncCount = encCount; if (encCount < minEncCount) minEncCount = encCount; if (badEdges > maxBadEdges) maxBadEdges = badEdges; - //Increment revolutions count revCount++; - - //Accumulate encoder counts totalEncCount += encCount; - - //Reset encoder counts encCount = 0; badEdges = 0; } - //ISR for each edge of encoder inputs void encISR0() { - //Increment encoder count if this edge was expected if (encState == 3) encCount++; else badEdges++; - - //Record the current state encState = 0; } void encISR1() { @@ -167,11 +155,9 @@ RawSerial pc(USBTX, USBRX); pc.printf("Hello\n\r"); - //Initialise the PWM MotorPWM.period_us(PWM_PRD); MotorPWM.pulsewidth_us(PWM_PRD); - //Start the timer Timer testTimer; testTimer.start(); @@ -186,11 +172,10 @@ while (testTimer.read_ms() < 1000) {} } - //Read the rotor position in state 0 orState = readRotorState(); pc.printf("PI origin %d\n\r",orState); - //Test motor current and photointerrupter input in each state + //Test each state bool drivePass = true; bool PIPass = true; for (int i=0;i<6;i++) { @@ -220,80 +205,86 @@ printf("Disc stuck or PI sensor fault\n\r"); while (1) {}} - //Find maximum velocity - printf("Finding maximum velocity\n\r"); + + printf("Finding maximum velocity\n\r"); + I1.rise(&photoISR); testTimer.reset(); intStateOld = readRotorState(); motorOut((readRotorState()-orState+lead+6)%6); //+6 to make sure the remainder is positive int32_t velCount = 0; int32_t maxVel = 0; uint8_t findMax = 1; + revCount = 0; while (findMax){ intState = readRotorState(); if (intState != intStateOld) { intStateOld = intState; motorOut((intState-orState+lead+6)%6); //+6 to make sure the remainder is positive - if (intState == 0) velCount++; + //if (intState == 0) velCount++; } - //Check the number of revolutions every second. Stop when there is no increase if (testTimer.read_ms() >= 1000){ testTimer.reset(); - if (velCount > maxVel) { - maxVel = velCount; + if (revCount > maxVel) { + maxVel = revCount; } else { findMax = 0; } - velCount = 0; + revCount = 0; } } printf("Maximum Velocity %d rps\n\r",maxVel); - //Reduce the PWM in steps. Check speed at each step + //Poll the rotor state and set the motor outputs accordingly to spin the motor + float testDC = 1.0; for (dc=1.0; dc>0; dc-= 0.1){ printf("Testing at %0.1f duty cycle...",dc); MotorPWM.write(dc); findMax = 1; testTimer.reset(); + revCount = 0; while (findMax) { intState = readRotorState(); if (intState != intStateOld) { intStateOld = intState; - if (EXT_PWM == false) - motorOut((intState-orState+lead+6)%6,dc); //+6 to make sure the remainder is positive - else - motorOut((intState-orState+lead+6)%6); - if (intState == 0) velCount++; + motorOut((intState-orState+lead+6)%6); } - //Check the number of revolutions every second. Stop when there is no decrease if (testTimer.read_ms() >= 1000){ testTimer.reset(); - if (velCount < maxVel) { - maxVel = velCount; + if (revCount < maxVel) { + maxVel = revCount; } else { findMax = 0; } - velCount = 0; + revCount = 0; } } printf("%d rps\n\r",maxVel); + if (maxVel > 20) testDC = dc; if (maxVel == 0) dc = 0.0; } //Test encoder - dc=0.5; + MotorPWM.write(1.0); + testTimer.reset(); + while (testTimer.read_ms() < 1000) { + intState = readRotorState(); + if (intState != intStateOld) { + intStateOld = intState; + motorOut((intState-orState+lead+6)%6); + } + } + dc=testDC; printf("Testing encoder. Duty cycle = %0.1f\n\r",dc); MotorPWM.write(dc); revCount = 0; - //Enable ISRs - I1.rise(&photoISR); CHA.rise(&encISR0); CHB.rise(&encISR1); CHA.fall(&encISR2); @@ -301,8 +292,8 @@ motorOut((readRotorState()-orState+lead+6)%6); //+6 to make sure the remainder is positive - //Spin for a while to allow speed to stabilise - while (revCount < 200) { + testTimer.reset(); + while (testTimer.read_ms() < ENC_TEST_PRE) { intState = readRotorState(); if (intState != intStateOld) { intStateOld = intState; @@ -310,15 +301,14 @@ } } - //Reset counters maxEncCount = 0; minEncCount = -1; maxBadEdges = 0; revCount = 0; totalEncCount = 0; - //Run the motor for the required number of revolutions - while (revCount < ENC_TEST_REVS) { + testTimer.reset(); + while (testTimer.read_ms() < ENC_TEST_DUR) { intState = readRotorState(); if (intState != intStateOld) { intStateOld = intState; @@ -326,12 +316,10 @@ } } - //Stop the photointerrupter ISR so no more updates to counters I1.rise(NULL); - printf("Min, Mean, Max encoder count = %d, %d, %d\n\r",minEncCount,totalEncCount/ENC_TEST_REVS,maxEncCount); + printf("Min, Mean, Max encoder count = %d, %d, %d\n\r",minEncCount,totalEncCount/revCount,maxEncCount); printf("Max bad transitions = %d\n\r",maxBadEdges); - //Spin forever while (1) { intState = readRotorState(); if (intState != intStateOld) {