Diff: main.cpp

Revision:

2:4e88faab6988

Parent:

1:184cb0870c04

--- a/main.cpp	Mon Feb 27 10:25:06 2017 +0000
+++ b/main.cpp	Tue Feb 28 14:44:23 2017 +0000
@@ -3,15 +3,19 @@
 
 //Photointerrupter input pins
 #define I1pin D2
-#define I2pin D3
-#define I3pin D4
+#define I2pin D11
+#define I3pin D12
+
+//Incremental encoder input pins
+#define CHA   D7
+#define CHB   D8  
 
 //Motor Drive output pins   //Mask in output byte
-#define L1Lpin PA_6         //0x01
-#define L1Hpin D6           //0x02
-#define L2Lpin PA_7         //0x04
-#define L2Hpin D8           //0x08
-#define L3Lpin PA_4         //0x10
+#define L1Lpin D4           //0x01
+#define L1Hpin D5           //0x02
+#define L2Lpin D3           //0x04
+#define L2Hpin D6           //0x08
+#define L3Lpin D9           //0x10
 #define L3Hpin D10          //0x20
 
 //Mapping from sequential drive states to motor phase outputs
@@ -28,18 +32,21 @@
 */
 //Drive state to output table
 const int8_t driveTable[] = {0x12,0x18,0x09,0x21,0x24,0x06,0x00,0x00};
-//Mask to invert the outputs for high side transistors
-const int8_t motorHmask = 0x2a;
+
 //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,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
 
 //Status LED
 DigitalOut led1(LED1);
 
 //Photointerrupter inputs
-DigitalIn I1int(I1pin);
-DigitalIn I2int(I2pin);
-DigitalIn I3int(I3pin);
+DigitalIn I1(I1pin);
+DigitalIn I2(I2pin);
+DigitalIn I3(I3pin);
 
 //Motor Drive outputs
 DigitalOut L1L(L1Lpin);
@@ -49,59 +56,66 @@
 DigitalOut L3L(L3Lpin);
 DigitalOut L3H(L3Hpin);
 
-//Global state
-int32_t outState = 0;   //Current drive state
-int32_t inState = 0;    //Current rotor state
-int32_t leadState = 0;  //Offset to calculate drive state from rotor state
-
 //Set a given drive state
 void motorOut(int8_t driveState){
-    //First turn all phases off to prevent shoot-through
-    L1H = 1;
-    L2H = 1;
-    L3H = 1;
-    
-    //Lookup the output byte from the drive state. Apply high side inversion mask
-    int8_t driveOut = driveTable[driveState & 0x07] ^ motorHmask;
     
-    //Apply the output byte to the pins
-    L1L = driveOut & 0x01;
-    L2L = driveOut & 0x04;
-    L3L = driveOut & 0x10;
-    L1H = driveOut & 0x02;
-    L2H = driveOut & 0x08;
-    L3H = driveOut & 0x20;
+    //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[I1int + 2*I2int + 4*I3int];
+    return stateMap[I1 + 2*I2 + 4*I3];
     }
 
 //Basic synchronisation routine    
-void motorHome() {
+int8_t motorHome() {
     //Put the motor in drive state 0 and wait for it to stabilise
     motorOut(0);
     wait(1.0);
     
     //Get the rotor state
-    inState = readRotorState();
-    
-    //Calculate the rotor-to-drive offset by adding two (modulo 6) the current rotor state
-    leadState = (inState+2)%6;
+    return readRotorState();
 }
     
 //Main
 int main() {
+    int8_t orState = 0;    //Rotot offset at motor state 0
+    
     //Initialise the serial port
     Serial pc(SERIAL_TX, SERIAL_RX);
+    int8_t intState = 0;
+    int8_t intStateOld = 0;
     pc.printf("Hello\n\r");
     
     //Run the motor synchronisation
-    motorHome();
+    orState = motorHome();
+    pc.printf("Rotor origin: %x\n\r",orState);
+    //orState is subtracted from future rotor state inputs to align rotor and motor states
     
     //Poll the rotor state and set the motor outputs accordingly to spin the motor
     while (1) {
-        motorOut(readRotorState()+leadState);
+        intState = readRotorState();
+        if (intState != intStateOld) {
+            intStateOld = intState;
+            motorOut((intState-orState+lead+6)%6); //+6 to make sure the remainder is positive
         }
+    }
 }