Diff: main.cpp

Revision:

10:0309d6c49f26

Parent:

9:575b29cbf5e4

Child:

11:1f596bf4182b

--- a/main.cpp	Thu Mar 09 09:30:44 2017 +0000
+++ b/main.cpp	Thu Mar 09 12:16:36 2017 +0000
@@ -1,6 +1,16 @@
 #include "mbed.h"
 #include "rtos.h"
 #include <string>
+#include "PID.h"
+
+//PID controller configuration
+float PIDrate = 0.2;
+float Kc = 1.0;
+float Ti = 0.0;
+float Td = 0.0;
+float speedControl = 0;
+PID controller(Kc, Ti, Td, PIDrate);
+Thread VPIDthread;
 
 //Photointerrupter input pins
 #define I1pin D2
@@ -85,7 +95,7 @@
 
 //Timer used for calculating speed
 Timer speedTimer;
-float revs = 0, revtimer = 0;
+float measuredRevs = 0, revtimer = 0;
 Ticker printSpeed;
 
 Serial pc(SERIAL_TX, SERIAL_RX);
@@ -96,8 +106,9 @@
 int position = 0;
 
 int i=0;
-int pos=0;
+int quadraturePosition=0;
 bool spinCW=0;
+float u = 0; //Set point for VPI
 
 //Set a given drive state
 void motorOut(int8_t driveState)
@@ -105,28 +116,20 @@
 
     //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];
+}
 
-    //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;
+inline void motorStop()
+{
+    //revsec set to zero prevents recurring interrupt for constant speed
+    revsec = 0;
+    //0x2A turns all motor transistors off to prevent any power usage
+    motor = 0x2A;
 }
 
 //Convert photointerrupter inputs to a rotor state
@@ -141,7 +144,7 @@
     //Put the motor in drive state 0 and wait for it to stabilise
     motor=cwState[1];
     wait(1.0);
-    
+
     position = 0;
 
     //Get the rotor state
@@ -151,7 +154,51 @@
 void fixedSpeed()
 {
     //Read current motor state
-    clk=!clk;
+//    clk=!clk;
+//    {0x38, 0x2C, 0x0E, 0x0B, 0x23, 0x32};
+    //if(spinCW) {
+//        switch(motor) {
+//            case 0x38:
+//                motor=0x2C;
+//                break;
+//            case 0x2C:
+//                motor=0x0E;
+//                break;
+//            case 0x0E:
+//                motor=0x0B;
+//                break;
+//            case 0x0B:
+//                motor=0x23;
+//                break;
+//            case 0x23:
+//                motor=0x32;
+//                break;
+//            case 0x32:
+//                motor=0x38;
+//                break;
+//        }
+//    } else {
+//        switch(motor) {
+//            case 0x38:
+//                motor=0x32;
+//                break;
+//            case 0x2C:
+//                motor=0x38;
+//                break;
+//            case 0x0E:
+//                motor=0x2C;
+//                break;
+//            case 0x0B:
+//                motor=0x0E;
+//                break;
+//            case 0x23:
+//                motor=0x0B;
+//                break;
+//            case 0x32:
+//                motor=0x23;
+//                break;
+//        }
+//    }
     intState = readRotorState();
     //Increment state machine to next state
     motorOut(intState);
@@ -161,20 +208,32 @@
     if(revsec) spinTimer.attach(&fixedSpeed, spinWait);
 }
 
-/*void rps()
+void rps()
 {
-    testpin=!testpin;
-//    clk=!clk;
-//    speedTimer.stop();
-//    revtimer = speedTimer.read_ms();
-//    revs = 1000/(revtimer);
-//    pos=0;
-//    testpin=!testpin;
-//    clk=!clk;
-//    speedTimer.reset();
-//    speedTimer.start();
+
+    clk=!clk;
+    speedTimer.stop();
+    revtimer = speedTimer.read_ms();
+    speedTimer.reset();
+    speedTimer.start();
+
+    measuredRevs = 1000/(revtimer);
+    quadraturePosition=0;
+
 }
 
+void VPID()
+{
+    while(1) {
+        controller.setSetPoint(revsec);
+//        printf("revsec: %2.3f\r\n", revsec);
+        controller.setProcessValue(measuredRevs);
+        speedControl = controller.compute();
+//        printf("speed setpoint: %2.3f\r\n", speedControl);
+        Thread::wait(PIDrate);
+    }
+}
+/*
 void speedo()
 {
     pc.printf("Revs / sec: %2.4f\r\n", revs);
@@ -211,40 +270,40 @@
 int main()
 {
     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);
-    }*/
+    /*    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();
@@ -258,15 +317,18 @@
     char ch;
     testpin=0;
 
-//    speedTimer.reset();
-//    speedTimer.start();
-//    I2.rise(&rps);
+    speedTimer.reset();
+    speedTimer.start();
+    I2.mode(PullNone);
+    I2.fall(&rps);
 //
 //    qA.rise(&edgeRiseA);
 //    qB.rise(&edgeIncr);
 //    qA.fall(&edgeIncr);
 //    qB.fall(&edgeIncr);
 
+    VPIDthread.start(VPID);
+
     while(1) {
 //        clk = I2;
         //Toggle LED so we know something's happening
@@ -354,16 +416,17 @@
                 case 's':
 //                    pc.printf("Revs / sec: %2.2f\r", revs);
 //                    printSpeed.attach(&speedo, 1.0);
+                    printf("Measured: %2.3f, revsec: %2.3f\r\n", measuredRevs, revsec);
+                    printf("speed setpoint: %2.3f\r\n", speedControl);
                     break;
                 case 't':
-                    pc.printf("%d\n\r", pos);
+//                    pc.printf("%d\n\r", pos);
                     break;
                 default:
                     //Set speed variables to zero to stop motor spinning
-                    revsec=0;
                     //Print error message
-                    motor = 0x2A;
-                    pc.printf("Error in received data\n\r");
+                    motorStop();
+                    pc.printf("Error in received data 0\n\r");
                     break;
             }
         }