![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Starter code for brushless motor controller
Diff: main.cpp
- Revision:
- 4:1cb32cb438ee
- Parent:
- 3:569b35e2a602
--- 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); } } + }