Revision 4:1cb32cb438ee, committed 2019-02-19

Comitter:

estott

Date:

Tue Feb 19 09:19:30 2019 +0000

Parent:

3:569b35e2a602

Commit message:

Made a self-test routine for ES CW2

Changed in this revision

--- a/main.cpp	Thu Mar 01 09:41:46 2018 +0000
+++ b/main.cpp	Tue Feb 19 09:19:30 2019 +0000
@@ -1,21 +1,27 @@
 #include "mbed.h"
 
 //Photointerrupter input pins
-#define I1pin D2
-#define I2pin D11
-#define I3pin D12
+#define I1pin D3
+#define I2pin D6
+#define I3pin D5
 
 //Incremental encoder input pins
-#define CHA   D7
-#define CHB   D8  
+#define CHApin   D12
+#define CHBpin   D11
 
 //Motor Drive output pins   //Mask in output byte
-#define L1Lpin D4           //0x01
-#define L1Hpin D5           //0x02
-#define L2Lpin D3           //0x04
-#define L2Hpin D6           //0x08
-#define L3Lpin D9           //0x10
-#define L3Hpin D10          //0x20
+#define L1Lpin D1           //0x01
+#define L1Hpin A3           //0x02
+#define L2Lpin D0           //0x04
+#define L2Hpin A6          //0x08
+#define L3Lpin D10           //0x10
+#define L3Hpin D2          //0x20
+
+#define PWMpin D9
+
+//Motor current sense
+#define MCSPpin   A1
+#define MCSNpin   A0
 
 //Mapping from sequential drive states to motor phase outputs
 /*
@@ -39,23 +45,65 @@
 //Phase lead to make motor spin
 const int8_t lead = 2;  //2 for forwards, -2 for backwards
 
+const int32_t MIN_CURRENT = 100000;
+
+const int32_t PWM_PRD = 2000;
+
+const uint32_t ENC_TEST_REVS = 1000;
+
+const bool EXT_PWM = true;
+
+float dc=0.0;
+
 //Status LED
 DigitalOut led1(LED1);
 
 //Photointerrupter inputs
-DigitalIn I1(I1pin);
+InterruptIn I1(I1pin);
 DigitalIn I2(I2pin);
 DigitalIn I3(I3pin);
 
 //Motor Drive outputs
 DigitalOut L1L(L1Lpin);
+DigitalOut L2L(L2Lpin);
+DigitalOut L3L(L3Lpin);
 DigitalOut L1H(L1Hpin);
-DigitalOut L2L(L2Lpin);
 DigitalOut L2H(L2Hpin);
-DigitalOut L3L(L3Lpin);
 DigitalOut L3H(L3Hpin);
 
-//Set a given drive state
+AnalogIn MCSP(MCSPpin);
+AnalogIn MCSN(MCSNpin);
+
+PwmOut MotorPWM(PWMpin);
+
+//Encoder inputs
+InterruptIn CHA(CHApin);
+InterruptIn CHB(CHBpin);
+
+//Set a given drive state with PWM
+void motorOut(int8_t driveState, float dc){
+    
+    //Lookup the output byte from the drive state.
+    int8_t driveOut = driveTable[driveState & 0x07];
+    int32_t torque = (int)((float)PWM_PRD*dc);
+    //Turn off first
+    if (~driveOut & 0x01) L1L = 0;
+    if (~driveOut & 0x02) L1H = 1;
+    if (~driveOut & 0x04) L2L = 0;
+    if (~driveOut & 0x08) L2H = 1;
+    if (~driveOut & 0x10) L3L = 0;
+    if (~driveOut & 0x20) L3H = 1;
+
+    //Then turn on
+    if (driveOut & 0x01) L1L = 1;
+    if (driveOut & 0x02) L1H = 0;
+    if (driveOut & 0x04) L2L = 1;
+    if (driveOut & 0x08) L2H = 0;
+    if (driveOut & 0x10) L3L = 1;
+    if (driveOut & 0x20) L3H = 0;
+    }
+    
+//Set a given drive state with no PWM
 void motorOut(int8_t driveState){
     
     //Lookup the output byte from the drive state.
@@ -68,7 +116,7 @@
     if (~driveOut & 0x08) L2H = 1;
     if (~driveOut & 0x10) L3L = 0;
     if (~driveOut & 0x20) L3H = 1;
-    
+
     //Then turn on
     if (driveOut & 0x01) L1L = 1;
     if (driveOut & 0x02) L1H = 0;
@@ -83,15 +131,41 @@
     return stateMap[I1 + 2*I2 + 4*I3];
     }
 
-//Basic synchronisation routine    
-int8_t motorHome() {
-    //Put the motor in drive state 0 and wait for it to stabilise
-    motorOut(0);
-    wait(2.0);
+uint32_t revCount,encCount,maxEncCount,minEncCount,badEdges,maxBadEdges;
+uint32_t encState,totalEncCount;
+void photoISR() {
+    
+    if (encCount > maxEncCount) maxEncCount = encCount;
+    if (encCount < minEncCount) minEncCount = encCount;
+    if (badEdges > maxBadEdges) maxBadEdges = badEdges;
+    
+    revCount++;
+    totalEncCount += encCount;
+    encCount = 0;
+    badEdges = 0;
+    }
     
-    //Get the rotor state
-    return readRotorState();
-}
+void encISR0() {
+    if (encState == 3) encCount++;
+    else badEdges++;
+    encState = 0;
+    }
+void encISR1() {
+    if (encState == 0) encCount++;
+    else badEdges++;
+    encState = 1;
+    }
+void encISR2() {
+    if (encState == 1) encCount++;
+    else badEdges++;
+    encState = 2;
+    }
+void encISR3() {
+    if (encState == 2) encCount++;
+    else badEdges++;
+    encState = 3;
+    }
+
     
 //Main
 int main() {
@@ -100,21 +174,170 @@
     int8_t intStateOld = 0;
     
     //Initialise the serial port
-    Serial pc(SERIAL_TX, SERIAL_RX);
+    RawSerial pc(USBTX, USBRX);
     pc.printf("Hello\n\r");
     
-    //Run the motor synchronisation
-    orState = motorHome();
-    pc.printf("Rotor origin: %x\n\r",orState);
-    //orState is subtracted from future rotor state inputs to align rotor and motor states
+    MotorPWM.period_us(PWM_PRD);
+    MotorPWM.pulsewidth_us(PWM_PRD);
+    
+    Timer testTimer;
+    testTimer.start();
+    
+    pc.printf("Testing drive and photointerrupters\n\r");
+    
+    while (testTimer.read_ms() < 1000) {}
+    
+    //Cycle through states to catch rotor
+    for (int i=5;i>=-1;i--) {
+        motorOut(i);
+        testTimer.reset();
+        while (testTimer.read_ms() < 1000) {}
+    }
+    
+    orState = readRotorState();
+    pc.printf("PI origin %d\n\r",orState);
+     
+    //Test each state
+    bool drivePass = true;
+    bool PIPass = true;
+    for (int i=0;i<6;i++) {
+        motorOut(i);    
+        testTimer.reset();    
+        while (testTimer.read_ms() < 1000) {}
+        
+        int32_t motorCurrent = (MCSP.read_u16()-MCSN.read_u16())*56; //Conversion to microamps
+        printf("Drive State %d: PI input %d, Current %dmA\n\r",i,readRotorState(),motorCurrent/1000);  
+        
+        int8_t stateOffset = (readRotorState() - i + 6)%6;
+        if (stateOffset != orState){
+            printf("  Unexpected PI input\n\r");
+            PIPass = false;
+        }
+        if (motorCurrent < MIN_CURRENT){
+          printf("  Drive current too low\n\r"); 
+            drivePass = false;
+        }
+          
+    }
+    if (drivePass == false) {
+        printf("Motor current fail\n\r");
+        while (1) {}}
+
+    if (PIPass == false) {
+        printf("Disc stuck or PI sensor fault\n\r");
+        while (1) {}}
+
+
+    printf("Finding maximum velocity\n\r"); 
+    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;
+    
+    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 (testTimer.read_ms() >= 1000){
+            testTimer.reset();
+            if (velCount > maxVel) {
+                maxVel = velCount;
+            } else {
+                findMax = 0;
+            }
+            velCount = 0;
+        }
+    }
+        
+    printf("Maximum Velocity %d rps\n\r",maxVel);
     
     //Poll the rotor state and set the motor outputs accordingly to spin the motor
+    for (dc=1.0; dc>0; dc-= 0.1){
+        printf("Testing at %0.1f duty cycle...",dc);
+        MotorPWM.write(dc);
+        findMax = 1;
+        testTimer.reset();  
+        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++;
+            }
+            
+            if (testTimer.read_ms() >= 1000){
+                testTimer.reset();
+                if (velCount < maxVel) {
+                    maxVel = velCount;
+                } else {
+                    findMax = 0;
+                }
+                velCount = 0;
+            }
+        }
+        printf("%d rps\n\r",maxVel);
+        if (maxVel == 0) dc = 0.0;
+    }
+    
+    
+    //Test encoder
+    
+    dc=0.5;
+    printf("Testing encoder. Duty cycle = %0.1f\n\r",dc);
+    MotorPWM.write(dc);
+    
+    revCount = 0;
+    
+    I1.rise(&photoISR);
+    CHA.rise(&encISR0);
+    CHB.rise(&encISR1);
+    CHA.fall(&encISR2);
+    CHB.fall(&encISR3);
+    
+    motorOut((readRotorState()-orState+lead+6)%6); //+6 to make sure the remainder is positive
+    
+    while (revCount < 1000) {
+        intState = readRotorState();
+        if (intState != intStateOld) {
+            intStateOld = intState;
+            motorOut((intState-orState+lead+6)%6);
+        }
+    }
+    
+    maxEncCount = 0;
+    minEncCount = -1;
+    maxBadEdges = 0;
+    revCount = 0;
+    totalEncCount = 0;
+    
+    while (revCount < ENC_TEST_REVS) {
+        intState = readRotorState();
+        if (intState != intStateOld) {
+            intStateOld = intState;
+            motorOut((intState-orState+lead+6)%6);
+        }
+    }
+    
+    I1.rise(NULL);
+    printf("Min, Mean, Max encoder count = %d, %d, %d\n\r",minEncCount,totalEncCount/ENC_TEST_REVS,maxEncCount);
+    printf("Max bad transitions = %d\n\r",maxBadEdges);
+    
     while (1) {
         intState = readRotorState();
         if (intState != intStateOld) {
             intStateOld = intState;
-            motorOut((intState-orState+lead+6)%6); //+6 to make sure the remainder is positive
+            motorOut((intState-orState+lead+6)%6);
         }
     }
+    
 }
 

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-os.lib	Tue Feb 19 09:19:30 2019 +0000
@@ -0,0 +1,1 @@
+https://github.com/ARMmbed/mbed-os/#34c1facf42a174f47fdf9002cd8c6bf10ac41744

--- a/mbed.bld	Thu Mar 01 09:41:46 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-https://os.mbed.com/users/mbed_official/code/mbed/builds/7130f322cb7e
\ No newline at end of file