Diff: main.cpp

Revision:

24:4032857546f4

Parent:

23:d48d51e5db97

Child:

25:9e6e870821d8

--- a/main.cpp	Fri Mar 10 19:53:08 2017 +0000
+++ b/main.cpp	Fri Mar 10 21:11:49 2017 +0000
@@ -4,7 +4,7 @@
 #include "PID.h"
 
 //PID controller configuration
-float PIDrate = 0.2;
+float PIDrate = 0.1;
 float Kc = 4.5;
 float Ti = 0.1;
 float Td = 0.5;
@@ -45,7 +45,13 @@
 7       -   -   -
 */
 //Drive state to output table
-const int8_t driveTable[6] = {0x38, 0x2C, 0x0E, 0x0B, 0x23, 0x32};
+//const int8_t driveTable[6] = {0x38, 0x2C, 0x0E, 0x0B, 0x23, 0x32};
+
+//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 cwState[7] = {0x00, 0x23, 0x38, 0x32, 0x0E, 0x0B, 0x2C};
 const int8_t AcwState[7] = {0x00, 0x0E, 0x23, 0x0B, 0x38, 0x2C, 0x32};
@@ -72,7 +78,22 @@
 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);
+BusOut motor(L1Hpin, L2Lpin, L2Hpin, L3Lpin, L3Hpin);//L1Lpin,
+PwmOut singpin(L1Lpin);
+
+//BusOut digitalpins(L1Hpin, L2Lpin, L2Hpin, L3Lpin, L3Hpin);
+//PwmOut motor[6] = {L1Lpin, L1Hpin, L2Lpin, L2Hpin, L3Lpin, L3Hpin};
+//PwmOut singpin(L1Lpin);
+
+//motor[] = (singpin, digitalpins);
+
+//PwmOut L1L(L1Lpin);
+//DigitalOut L1H(L1Hpin);
+//DigitalOut L2L(L2Lpin);
+//DigitalOut L2H(L2Hpin);
+//DigitalOut L3L(L3Lpin);
+//DigitalOut L3H(L3Hpin);
+
 
 //Timeout function for rotating at set speed
 Timeout spinTimer;
@@ -102,21 +123,27 @@
 void motorOut(int8_t driveState)
 {
 
-    //Set to zero
-    motor=0x2A;
+    motor = 0x2A;
 
     if(revtimer<33) {
         clk=1;
-        if(!spinCW) motor = FastStateACW[driveState];
-        else motor = FastStateCW[driveState];
+        if(!spinCW) {
+            motor = (FastStateACW[driveState]>>1);
+            singpin = float(FastStateACW[driveState]&&0x01)/2;
+        } else {
+            motor = (FastStateCW[driveState]>>1);
+            singpin = float(FastStateCW[driveState]&&0x01)/2;
+        }
     } else {
         clk=0;
-        //Go to next state
-        if(!spinCW) motor = AcwState[driveState];
-        else motor = cwState[driveState];
+        if(!spinCW) {
+            motor = (AcwState[driveState]>>1);
+            singpin = float(AcwState[driveState]&&0x01)/2;
+        } else {
+            motor = (cwState[driveState]>>1);
+            singpin = float(cwState[driveState]&&0x01)/2;
+        }
     }
-    //Lookup the output byte from the drive state.
-//    int8_t driveOut = driveTable[driveState & 0x07];
 }
 
 inline void motorStop()
@@ -139,6 +166,7 @@
 {
     //Put the motor in drive state 0 and wait for it to stabilise
     motor=cwState[1];
+//    motorOut(1);
     wait(1.0);
 
     position = 0;
@@ -153,7 +181,7 @@
     //timeout interrupt to call this function again and
     //keep the motor spinning at the right speed
     if(revsec) spinTimer.attach(&fixedSpeed, spinWait);
-    
+
     intState = readRotorState();
     //Increment state machine to next state
     motorOut(intState);
@@ -208,31 +236,31 @@
     }
 }
 
-void sing(float singFreq)
-{
-    motor = driveTable[1];
-    wait(5.0);
-    float singDelayus = 1000000/singFreq;
-    for(int count=0; count<singFreq; count++) {
-        motor = driveTable[2];
-        wait_us(singDelayus);
-        motor = driveTable[1];
-    }
-    singFreq = 15000;
-    singDelayus = 1000000/singFreq;
-    for(int count=0; count<singFreq; count++) {
-        motor = driveTable[2];
-        wait_us(singDelayus);
-        motor = driveTable[1];
-    }
-}
+//void sing(float singFreq)
+//{
+//    motor = driveTable[1];
+//    wait(5.0);
+//    float singDelayus = 1000000/singFreq;
+//    for(int count=0; count<singFreq; count++) {
+//        motor = driveTable[2];
+//        wait_us(singDelayus);
+//        motor = driveTable[1];
+//    }
+//    singFreq = 15000;
+//    singDelayus = 1000000/singFreq;
+//    for(int count=0; count<singFreq; count++) {
+//        motor = driveTable[2];
+//        wait_us(singDelayus);
+//        motor = driveTable[1];
+//    }
+//}
 
 
 //Main function
 int main()
 {
     pc.printf("spin\n\r");
-
+    spinWait = 0.01;
     motorStop();
 
     //Run the motor synchronisation
@@ -255,6 +283,9 @@
     I2.mode(PullNone);
     I2.fall(&rps);
 
+    singpin.period_us(100);
+
+
     VPIDthread.start(VPID);
 
     while(1) {
@@ -396,7 +427,7 @@
                     }
                     //Calculate the number of revolutions required
                     targetRevs = float(hdrds)*100 + float(tens)*10 + float(units) + float(decimals)/10;
-
+                    singpin.period_us(float(hdrds)*100 + float(tens)*10 + float(units) + float(decimals)/10);
                     //Print values for verification
                     pc.printf("Target revs: %2.4f\n\r", targetRevs);
 
@@ -463,7 +494,7 @@
                     controller.setBias(bias);
                     break;
                 case 'l':
-                    sing(261.63);
+//                    sing(261.63);
                     break;
                 default:
                     //Set speed variables to zero to stop motor spinning
@@ -474,8 +505,6 @@
                     break;
             }
         }
-//        printSpeed.attach(&speedo, 1.0);
-//        pc.printf("Revs / sec: %2.2f\r", revs);
     }
 
 }