Diff: main.cpp

Revision:

3:60009a5ed1dc

Parent:

2:4e88faab6988

Child:

4:a436ddb6e57e

--- a/main.cpp	Tue Feb 28 14:44:23 2017 +0000
+++ b/main.cpp	Fri Mar 17 14:05:26 2017 +0000
@@ -1,5 +1,5 @@
 #include "mbed.h"
-#include "rtos.h"
+#include "PID.h"
 
 //Photointerrupter input pins
 #define I1pin D2
@@ -38,45 +38,134 @@
 //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
+const int8_t lead = 2;  //2 for forwards, -2 for backwards
 
+volatile float revTime;
+volatile float revPerSec;
+volatile float errorPrev = 0;
+volatile float dutyCycle;
+volatile float targetSpeed;
+volatile float acc;
+
+volatile float timeMap[] = {0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 };
+
+volatile float prevTime = 0;
+volatile float prevSpeed = 0;
 //Status LED
 DigitalOut led1(LED1);
+DigitalOut led2 (LED2);
+DigitalOut led3 (LED3);
+
+InterruptIn CHAintr(CHA);
+InterruptIn CHBintr(CHB);
+
+DigitalIn CHAR(CHA);
+DigitalIn CHBR(CHB);
 
 //Photointerrupter inputs
+InterruptIn I1intr(I1pin);
+InterruptIn I2intr(I2pin);
+InterruptIn I3intr(I3pin);
+
 DigitalIn I1(I1pin);
 DigitalIn I2(I2pin);
 DigitalIn I3(I3pin);
 
+
+PID controller(5.0, 0.0, 0.000001, 0.001);
+Ticker tick;
+
+Timer t;
+
+Serial pc(SERIAL_TX,SERIAL_RX);
 //Motor Drive outputs
-DigitalOut L1L(L1Lpin);
-DigitalOut L1H(L1Hpin);
-DigitalOut L2L(L2Lpin);
-DigitalOut L2H(L2Hpin);
-DigitalOut L3L(L3Lpin);
-DigitalOut L3H(L3Hpin);
+PwmOut L1L(L1Lpin);
+PwmOut L1H(L1Hpin);
+PwmOut L2L(L2Lpin);
+PwmOut L2H(L2Hpin);
+PwmOut L3L(L3Lpin);
+PwmOut L3H(L3Hpin);
 
 //Set a given drive state
-void motorOut(int8_t driveState){
+
+void blinkLED2(){
+    while(1){
+        led1 = 0;   
+        wait(0.5);
+        led1 = 1;
+        wait(0.5);
+    }
+}
+
+void blinkLED3(){
+    while(1){
+        led3 = 0;   
+        wait(0.1);
+        led3 = 1;
+        wait(0.1);
+    }
+}
+
+void mesSpeedSlits(){
+    float currTime = t.read();
+    revPerSec = 0.4*prevSpeed + 0.6*((0.0021367521)/(currTime-prevTime));
+    prevTime = currTime;
+    prevSpeed = revPerSec;
     
+}
+
+void mesRot(){
+    led3 = !led3;
+    float speedTime;
+    speedTime = t.read();
+    revPerSec = 1.0/(speedTime - timeMap[I1 + 2*I2 + 4*I3]);
+    timeMap[I1 + 2*I2 + 4*I3] = speedTime;
+}   
+
+void controlLoop(void){
+    while(true){
+        //float error = targetSpeed - revPerSec;
+        //float errorDer = (error - errorPrev);
+        //float k = 5.0;
+        //float kd = 1;
+        //dutyCycle = k*error + kd*errorDer;
+        controller.setProcessValue(revPerSec);
+    //Set the new output.
+    dutyCycle = controller.compute();
+        //errorPrev = error; //remeber error
+        wait(0.001);
+    }
+}
+
+void decrease(){
+    dutyCycle -= 0.05;
+    pc.printf("current value: %f", dutyCycle);
+    if (dutyCycle < 0.2){
+        dutyCycle = 1;
+        }    
+}
+        
+    
+void motorOut(int8_t driveState, float dutyCycle){
+    //float dutyCycle = 1.0f;
     //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;
+    if (~driveOut & 0x01) L1L.write(0); // = 0
+    if (~driveOut & 0x02) L1H.write(dutyCycle);// = 1;
+    if (~driveOut & 0x04) L2L.write(0); // = 0;
+    if (~driveOut & 0x08) L2H.write(dutyCycle);// = 1;
+    if (~driveOut & 0x10) L3L.write(0);// = 0;
+    if (~driveOut & 0x20) L3H.write(dutyCycle);// = 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.write(dutyCycle);// = 1;
+    if (driveOut & 0x02) L1H.write(0); // = 0;
+    if (driveOut & 0x04) L2L.write(dutyCycle);// = 1;
+    if (driveOut & 0x08) L2H.write(0);// = 0;
+    if (driveOut & 0x10) L3L.write(dutyCycle);// = 1;
+    if (driveOut & 0x20) L3H.write(0);// = 0;
     }
     
     //Convert photointerrupter inputs to a rotor state
@@ -87,35 +176,93 @@
 //Basic synchronisation routine    
 int8_t motorHome() {
     //Put the motor in drive state 0 and wait for it to stabilise
-    motorOut(0);
-    wait(1.0);
+    motorOut(0, 1);
+    wait(3.0);
     
     //Get the rotor state
     return readRotorState();
 }
     
+void printStatus() {
+    while(1){
+        led3 = !led3;
+    //pc.printf("%f\n\r",revPerSec);
+    wait(2);
+    }
+    } 
 //Main
 int main() {
+    
+    Thread th1;
+Thread controlLoopThread;
     int8_t orState = 0;    //Rotot offset at motor state 0
     
+      controller.setInputLimits(0, 200);
+  //Pwm output from 0.0 to 1.0
+  controller.setOutputLimits(0.0, 1.0);
+  //If there's a bias.
+  controller.setBias(0.0);
+  controller.setMode(0);
     //Initialise the serial port
     Serial pc(SERIAL_TX, SERIAL_RX);
     int8_t intState = 0;
     int8_t intStateOld = 0;
-    pc.printf("Hello\n\r");
+    float per = 0.001f; 
+     controller.setSetPoint(50.0);
+    
+    L1L.period(per);
+    L1H.period(per);
+    L2L.period(per);
+    L2H.period(per);
+    L3L.period(per);
+    L3H.period(per);
+    dutyCycle = 1;
+    pc.printf("Device on \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
+    //th2.set_priority(osPriorityRealtime);
+    //th1.start(blinkLED);
+    //th2.start(blinkLED3);
     
+    //define interrupts
+    //I1intr.rise(&mesRot); //start photoInterrupt
+    //I2intr.rise(&mesRot);
+    //I3intr.rise(&mesRot);
+    //I1intr.fall(&mesRot);
+    //I2intr.fall(&mesRot);
+    //I3intr.fall(&mesRot);
+    
+    CHAintr.rise(&mesSpeedSlits);
+    CHBintr.rise(&mesSpeedSlits);
+    CHAintr.fall(&mesSpeedSlits);
+    CHBintr.fall(&mesSpeedSlits);
+    
+    th1.start(printStatus);
+    controlLoopThread.start(controlLoop);  //start conrol unit thread
+    t.start();
+    //tick.attach(&decrease, 10);
     //Poll the rotor state and set the motor outputs accordingly to spin the motor
     while (1) {
         intState = readRotorState();
+        if (pc.readable()){
+             char buffer[128];
+        
+                pc.gets(buffer, 6);
+                targetSpeed = atof(buffer);
+                //pc.printf("I got '%s'\n\r", buffer); 
+                pc.printf("Also in float '%f'\n\r", targetSpeed); 
+                 controller.setSetPoint(targetSpeed);
+                orState = motorHome();
+            }
+        
         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, dutyCycle); //+6 to make sure the remainder is positive
         }
     }
 }
 
+
+// hi
\ No newline at end of file