File content as of revision 4:1cb32cb438ee:

#include "mbed.h"

//Photointerrupter input pins
#define I1pin D3
#define I2pin D6
#define I3pin D5

//Incremental encoder input pins
#define CHApin   D12
#define CHBpin   D11

//Motor Drive output pins   //Mask in output byte
#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
/*
State   L1  L2  L3
0       H   -   L
1       -   H   L
2       L   H   -
3       L   -   H
4       -   L   H
5       H   L   -
6       -   -   -
7       -   -   -
*/
//Drive state to output table
const int8_t driveTable[] = {0x12,0x18,0x09,0x21,0x24,0x06,0x00,0x00};

//Mapping from interrupter inputs to sequential rotor states. 0x00 and 0x07 are not valid
const int8_t stateMap[] = {0x07,0x05,0x03,0x04,0x01,0x00,0x02,0x07};  
//const int8_t stateMap[] = {0x07,0x01,0x03,0x02,0x05,0x00,0x04,0x07}; //Alternative if phase order of input or drive is reversed

//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
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 L2H(L2Hpin);
DigitalOut L3H(L3Hpin);

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.
    int8_t driveOut = driveTable[driveState & 0x07];
      
    //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;
    }
    
    //Convert photointerrupter inputs to a rotor state
inline int8_t readRotorState(){
    return stateMap[I1 + 2*I2 + 4*I3];
    }

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;
    }
    
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() {
    int8_t orState = 0;    //Rotot offset at motor state 0
    int8_t intState = 0;
    int8_t intStateOld = 0;
    
    //Initialise the serial port
    RawSerial pc(USBTX, USBRX);
    pc.printf("Hello\n\r");
    
    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);
        }
    }
    
}