Diff: main.cpp

Revision:

9:575b29cbf5e4

Parent:

8:77627657da80

Child:

10:0309d6c49f26

--- a/main.cpp	Thu Mar 02 20:27:10 2017 +0000
+++ b/main.cpp	Thu Mar 09 09:30:44 2017 +0000
@@ -35,37 +35,48 @@
 7       -   -   -
 */
 //Drive state to output table
-const int8_t driveTable[] = {0x12,0x18,0x09,0x21,0x24,0x06,0x00,0x00};
+//const int8_t driveTable[] = {0x12,0x18,0x09,0x21,0x24,0x06,0x00,0x00};
+const int8_t driveTable[6] = {0x38, 0x2C, 0x0E, 0x0B, 0x23, 0x32};
+
+//const int8_t cwState[7] = {0, 4, 0, 5, 2, 3, 1};
+//const int8_t AcwState[7] = {0, 2, 4, 3, 0, 1, 5};
+//const int8_t cwState[7] = {0x00, 0x23, 0x38, 0x32, 0x0E, 0x0B, 0x2C};
+//const int8_t AcwState[7] = {0x00, 0x0E, 0x23, 0x0B, 0x38, 0x2C, 0x32};
+const int8_t AcwState[7] = {0x00, 0x23, 0x38, 0x32, 0x0E, 0x0B, 0x2C};
+const int8_t cwState[7] = {0x00, 0x0E, 0x23, 0x0B, 0x38, 0x2C, 0x32};
 
 //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
+//int8_t lead = -2;  //2 for forwards, -2 for backwards
 
 //Status LED
 DigitalOut led1(LED1);
 
 //Photointerrupter inputs
-//DigitalIn I1(I1pin);
-InterruptIn I1(I1pin);
-DigitalIn I2(I2pin);
+DigitalIn I1(I1pin);
+//InterruptIn I1(I1pin);
+InterruptIn I2(I2pin);
 DigitalIn I3(I3pin);
 
 InterruptIn qA(CHA);
 InterruptIn qB(CHB);
 
 //Motor Drive outputs
-DigitalOut L1L(L1Lpin);
-DigitalOut L1H(L1Hpin);
-DigitalOut L2L(L2Lpin);
-DigitalOut L2H(L2Hpin);
-DigitalOut L3L(L3Lpin);
-DigitalOut L3H(L3Hpin);
+//DigitalOut L1L(L1Lpin);
+//DigitalOut L1H(L1Hpin);
+//DigitalOut L2L(L2Lpin);
+//DigitalOut L2H(L2Hpin);
+//DigitalOut L3L(L3Lpin);
+//DigitalOut L3H(L3Hpin);
 DigitalOut clk(LED1);
 DigitalOut Direction(LED2);
-//DigitalOut testpin(D13);
+DigitalOut testpin(D13);
+
+//NOTE, BusOut declares things in reverse (ie, 0, 1, 2, 3) compared to binary represenation
+BusOut motor(L1Lpin, L1Hpin, L2Lpin, L2Hpin, L3Lpin, L3Hpin);
 
 //Timeout function for rotating at set speed
 Timeout spinTimer;
@@ -82,47 +93,56 @@
 int8_t orState = 0;    //Rotor offset at motor state 0
 int8_t intState = 0;
 int8_t intStateOld = 0;
+int position = 0;
 
 int i=0;
 int pos=0;
-bool DIR=0;
+bool spinCW=0;
 
 //Set a given drive state
 void motorOut(int8_t driveState)
 {
 
+    //Set to zero
+    motor=0x2A;
+    
+    //Go to next state
+    if(!spinCW) motor = AcwState[driveState];
+    else motor = cwState[driveState];
     //Lookup the output byte from the drive state.
-    int8_t driveOut = driveTable[driveState & 0x07];
+//    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;
+//    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;
+//    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];
+    return (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);
+    motor=cwState[1];
     wait(1.0);
+    
+    position = 0;
 
     //Get the rotor state
     return readRotorState();
@@ -131,58 +151,100 @@
 void fixedSpeed()
 {
     //Read current motor state
+    clk=!clk;
     intState = readRotorState();
     //Increment state machine to next state
-    motorOut((intState-orState+lead+6)%6);
+    motorOut(intState);
     //If spinning is required, attach the necessary wait to the
     //timeout interrupt to call this function again and
     //keep the motor spinning at the right speed
     if(revsec) spinTimer.attach(&fixedSpeed, spinWait);
 }
 
-void rps()
+/*void rps()
 {
-//      pc.printf("Tick\r\n");
-    speedTimer.stop();
-    revtimer = speedTimer.read_ms();
-    revs = 1000/(revtimer);
-//      pc.printf("Revs / sec: %2.2f\r", revs);
-    speedTimer.start();
+    testpin=!testpin;
+//    clk=!clk;
+//    speedTimer.stop();
+//    revtimer = speedTimer.read_ms();
+//    revs = 1000/(revtimer);
+//    pos=0;
+//    testpin=!testpin;
+//    clk=!clk;
+//    speedTimer.reset();
+//    speedTimer.start();
 }
 
 void speedo()
 {
-    pc.printf("Revs / sec: %2.4f\r", revs);
+    pc.printf("Revs / sec: %2.4f\r\n", revs);
     printSpeed.attach(&speedo, 1.0);
 }
 
-void edgeRiseA(){
+void edgeRiseA()
+{
     pos++;
-    if(pos>=468){
+    if(pos>=468) {
 //        Direction=!Direction;
         pos=pos%468;
 //        testpin=!testpin;
     }
     if(qB) DIR = 0;
     else DIR = 1;
-    clk=DIR;
+//    clk=DIR;
 //CLOCKWISE:        A rises before B -> On A edge, B low -> DIR = 1
 //ANTICLOCKWISE:    B rises before A -> On A edge, B high-> DIR = 0
 }
 
-void edgeIncr(){
+void edgeIncr()
+{
     pos++;
-    if(pos>=468){
+    if(pos>=468) {
 //        Direction=!Direction;
         pos=pos%468;
 //        testpin=!testpin;
     }
-}
+}*/
 
+//#define WAIT 2
 //Main function
 int main()
 {
-    pc.printf("Hello\n\r");
+    pc.printf("spin\n\r");
+    
+//    motor = 0x0E;
+/*    while(1){
+        motor=0x38;
+        printf("0x38\r\n");
+        wait(WAIT);
+        position = I1 + 2*I2 + 4*I3;
+        printf("position: %d\r\n", position);
+        motor=0x2C;
+        printf("0x2C\r\n");
+        wait(WAIT);
+        position = I1 + 2*I2 + 4*I3;
+        printf("position: %d\r\n", position);
+        motor=0x0E;
+        printf("0x0E\r\n");
+        wait(WAIT);
+        position = I1 + 2*I2 + 4*I3;
+        printf("position: %d\r\n", position);
+        motor=0x0B;
+        printf("0x0B\r\n");
+        wait(WAIT);
+        position = I1 + 2*I2 + 4*I3;
+        printf("position: %d\r\n", position);
+        motor=0x23;
+        printf("0x23\r\n");
+        wait(WAIT);
+        position = I1 + 2*I2 + 4*I3;
+        printf("position: %d\r\n", position);
+        motor=0x32;
+        printf("0x32\r\n");
+        wait(WAIT);
+        position = I1 + 2*I2 + 4*I3;
+        printf("position: %d\r\n", position);
+    }*/
 
     //Run the motor synchronisation
     orState = motorHome();
@@ -194,17 +256,19 @@
     int index=0;
     int units = 0, tens = 0, decimals = 0;
     char ch;
-//    testpin=0;
+    testpin=0;
 
-    speedTimer.start();
-    I1.rise(&rps);
-    
-    qA.rise(&edgeRiseA);
-    qB.rise(&edgeIncr);
-    qA.fall(&edgeIncr);
-    qB.fall(&edgeIncr);
+//    speedTimer.reset();
+//    speedTimer.start();
+//    I2.rise(&rps);
+//
+//    qA.rise(&edgeRiseA);
+//    qB.rise(&edgeIncr);
+//    qA.fall(&edgeIncr);
+//    qB.fall(&edgeIncr);
 
     while(1) {
+//        clk = I2;
         //Toggle LED so we know something's happening
 //        clk = !clk;
 
@@ -237,22 +301,42 @@
                     units = 0, tens = 0, decimals = 0;
                     //For each character received, subtract ASCII 0 from ASCII
                     //representation to obtain the integer value of the number
+                    if(command[1]=='-') {
+                        spinCW = 0;
+                        //If decimal point is in the second character (eg, V-.1)
+                        if(command[2]=='.') {
+                            //Extract decimal rev/s
+                            decimals = command[3] - '0';
 
-                    //If decimal point is in the second character (eg, V.1)
-                    if(command[1]=='.') {
-                        //Extract decimal rev/s
-                        decimals = command[2] - '0';
+                            //If decimal point is in the third character (eg, V-0.1)
+                        } else if(command[3]=='.') {
+                            units = command[2] - '0';
+                            decimals = command[4] - '0';
 
-                        //If decimal point is in the third character (eg, V0.1)
-                    } else if(command[2]=='.') {
-                        units = command[1] - '0';
-                        decimals = command[3] - '0';
+                            //If decimal point is in the fourth character (eg, V-10.1)
+                        } else if(command[4]=='.') {
+                            tens = command[2] - '0';
+                            units = command[3] - '0';
+                            decimals = command[5] - '0';
+                        }
+                    } else {
+                        spinCW = 1;
+                        //If decimal point is in the second character (eg, V.1)
+                        if(command[1]=='.') {
+                            //Extract decimal rev/s
+                            decimals = command[2] - '0';
 
-                        //If decimal point is in the fourth character (eg, V10.1)
-                    } else if(command[3]=='.') {
-                        tens = command[1] - '0';
-                        units = command[2] - '0';
-                        decimals = command[4] - '0';
+                            //If decimal point is in the third character (eg, V0.1)
+                        } else if(command[2]=='.') {
+                            units = command[1] - '0';
+                            decimals = command[3] - '0';
+
+                            //If decimal point is in the fourth character (eg, V10.1)
+                        } else if(command[3]=='.') {
+                            tens = command[1] - '0';
+                            units = command[2] - '0';
+                            decimals = command[4] - '0';
+                        }
                     }
 
                     //Calculate the number of revolutions per second required
@@ -268,7 +352,8 @@
                     break;
                     //If anything unexpected was received
                 case 's':
-                    pc.printf("Revs / sec: %2.2f\r", revs);
+//                    pc.printf("Revs / sec: %2.2f\r", revs);
+//                    printSpeed.attach(&speedo, 1.0);
                     break;
                 case 't':
                     pc.printf("%d\n\r", pos);
@@ -277,6 +362,7 @@
                     //Set speed variables to zero to stop motor spinning
                     revsec=0;
                     //Print error message
+                    motor = 0x2A;
                     pc.printf("Error in received data\n\r");
                     break;
             }