Omar Muttawa
SMARTASSES 2019
Diff: main.cpp
- Revision:
- 4:1cb32cb438ee
- Parent:
- 3:569b35e2a602
diff -r 569b35e2a602 -r 1cb32cb438ee main.cpp
--- 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);
}
}
+
}