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