Diff: main.cpp

Revision:

4:a436ddb6e57e

Parent:

3:60009a5ed1dc

Child:

5:07b2e414d174

diff -r 60009a5ed1dc -r a436ddb6e57e main.cpp
--- a/main.cpp	Fri Mar 17 14:05:26 2017 +0000
+++ b/main.cpp	Sat Mar 18 15:10:48 2017 +0000
@@ -1,15 +1,40 @@
 #include "mbed.h"
 #include "PID.h"
 
+
+
+/////////////////////
+// Pin definitions //
+/////////////////////
+
 //Photointerrupter input pins
 #define I1pin D2
 #define I2pin D11
 #define I3pin D12
 
+DigitalIn I1(I1pin);
+DigitalIn I2(I2pin);
+DigitalIn I3(I3pin);
+
+InterruptIn I1intr(I1pin);
+InterruptIn I2intr(I2pin);
+InterruptIn I3intr(I3pin);
+
 //Incremental encoder input pins
 #define CHA   D7
 #define CHB   D8  
 
+DigitalIn CHAR(CHA);
+DigitalIn CHBR(CHB);
+
+InterruptIn CHAintr(CHA);
+InterruptIn CHBintr(CHB);
+
+//Status LED
+DigitalOut led1 (LED1);
+DigitalOut led2 (LED2);
+DigitalOut led3 (LED3);
+
 //Motor Drive output pins   //Mask in output byte
 #define L1Lpin D4           //0x01
 #define L1Hpin D5           //0x02
@@ -18,6 +43,13 @@
 #define L3Lpin D9           //0x10
 #define L3Hpin D10          //0x20
 
+PwmOut L1L(L1Lpin);
+PwmOut L1H(L1Hpin);
+PwmOut L2L(L2Lpin);
+PwmOut L2H(L2Hpin);
+PwmOut L3L(L3Lpin);
+PwmOut L3H(L3Hpin);
+
 //Mapping from sequential drive states to motor phase outputs
 /*
 State   L1  L2  L3
@@ -30,6 +62,7 @@
 6       -   -   -
 7       -   -   -
 */
+
 //Drive state to output table
 const int8_t driveTable[] = {0x12,0x18,0x09,0x21,0x24,0x06,0x00,0x00};
 
@@ -40,55 +73,30 @@
 //Phase lead to make motor spin
 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;
+//////////////////////
+// Global variables //
+//////////////////////
 
-Serial pc(SERIAL_TX,SERIAL_RX);
-//Motor Drive outputs
-PwmOut L1L(L1Lpin);
-PwmOut L1H(L1Hpin);
-PwmOut L2L(L2Lpin);
-PwmOut L2H(L2Hpin);
-PwmOut L3L(L3Lpin);
-PwmOut L3H(L3Hpin);
+volatile float targetSpeed; // User defined instantaneous speed
+volatile float revPerSec; // Instantaneous speed estimate
+volatile float previousTime = 0; // Used to calculate revPerSec
+volatile float previousSpeed = 0; // Previous value of revPerSec
+volatile float dutyCycle; // PWM duty cycle between 0 and 1
+volatile float timeMap[] = {0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 }; // Keep track of last state
+Timer t; // To keep track of time
+Serial pc(SERIAL_TX,SERIAL_RX); // Serial connection
+PID controller(5.0, 0.0, 0.000001, 0.001); // Arguments are Kp, Ki, Kd and interval
 
-//Set a given drive state
+
 
-void blinkLED2(){
+///////////////
+// Functions //
+///////////////
+
+// THREAD: Blink LED1 every 0.5 seconds
+void blinkLED1(){ 
     while(1){
         led1 = 0;   
         wait(0.5);
@@ -97,172 +105,152 @@
     }
 }
 
-void blinkLED3(){
+// THREAD: Print instantaneous speed
+void printStatus() {
     while(1){
-        led3 = 0;   
-        wait(0.1);
-        led3 = 1;
-        wait(0.1);
+        led3 = !led3;
+        //pc.printf("%f\n\r",revPerSec);
+        wait(2);
     }
 }
 
-void mesSpeedSlits(){
-    float currTime = t.read();
-    revPerSec = 0.4*prevSpeed + 0.6*((0.0021367521)/(currTime-prevTime));
-    prevTime = currTime;
-    prevSpeed = revPerSec;
-    
+// THREAD: Control loop
+void controlLoop(void){
+    while(true){
+        controller.setProcessValue(revPerSec);
+        dutyCycle = controller.compute();
+        wait(0.001);
+    }
 }
 
-void mesRot(){
+// Measure speed using slits
+void measureSpeedSlits(){
+    float currentTime = t.read();
+    revPerSec = 0.4*previousSpeed + 0.6*((0.0021367521)/(currentTime-previousTime));
+    previousTime = currentTime;
+    previousSpeed = revPerSec;
+}
+
+// Measure speed using photointerrupters
+void measureSpeedPhoto(){
     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;
-        }    
-}
-        
+}          
     
+// Set motor states
 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.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;
+    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.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;
-    }
+    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
+//Convert photointerrupter inputs to a rotor state
 inline int8_t readRotorState(){
     return stateMap[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, 1);
+    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
+
+//////////
+// Main //
+//////////     
+
 int main() {
     
-    Thread th1;
-Thread controlLoopThread;
-    int8_t orState = 0;    //Rotot offset at motor state 0
+    // Initialize threads
+    Thread status;
+    Thread controlLoopThread;
+    status.start(printStatus);
+    controlLoopThread.start(controlLoop);
+    t.start();
     
-      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
+    // Set control loop parameters
+    controller.setInputLimits(0, 200);
+    controller.setOutputLimits(0.0, 1.0);
+    controller.setBias(0.0);
+    controller.setMode(0);
+    controller.setSetPoint(50.0);
+    
+    //Initialize the serial port
     Serial pc(SERIAL_TX, SERIAL_RX);
+    pc.printf("Device on \n\r");
+    
+    int8_t orState = 0;    //Rotot offset at motor state 0
     int8_t intState = 0;
     int8_t intStateOld = 0;
-    float per = 0.001f; 
-     controller.setSetPoint(50.0);
+    float PWM_period = 0.001f; 
+    dutyCycle = 1;
+    
+    L1L.period(PWM_period);
+    L1H.period(PWM_period);
+    L2L.period(PWM_period);
+    L2H.period(PWM_period);
+    L3L.period(PWM_period);
+    L3H.period(PWM_period);
     
-    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");
+    // Slits interrupts
+    CHAintr.rise(&measureSpeedSlits);
+    CHBintr.rise(&measureSpeedSlits);
+    CHAintr.fall(&meeasureSpeedSlits);
+    CHBintr.fall(&measureSpeedSlits);
+    
+    // USE FOR PHOTOINTERRUPTERS
+    // Photointerrupters interrupts
+    //I1intr.rise(&measureSpeedPhoto);
+    //I2intr.rise(&measureSpeedPhoto);
+    //I3intr.rise(&measureSpeedPhoto);
+    //I1intr.fall(&measureSpeedPhoto);
+    //I2intr.fall(&measureSpeedPhoto);
+    //I3intr.fall(&measureSpeedPhoto);
     
     //Run the motor synchronisation
     orState = motorHome();
-    //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();
-            }
+        // Read serial input
+        if (pc.readable()){
+            char buffer[128];
+            pc.gets(buffer, 6);
+            targetSpeed = atof(buffer); 
+            pc.printf("Target speed: '%f'\n\r", targetSpeed); 
+            controller.setSetPoint(targetSpeed);
+            orState = motorHome();
+        }
         
+        intState = readRotorState();
         if (intState != intStateOld) {
             intStateOld = intState;
             motorOut((intState-orState+lead+6)%6, dutyCycle); //+6 to make sure the remainder is positive
         }
     }
-}
-
-
-// hi
\ No newline at end of file
+}
\ No newline at end of file