Diff: main.cpp

Revision:

25:ae908de29943

Parent:

24:d0af4b2be295

--- a/main.cpp	Sat Oct 03 15:59:25 2015 +0000
+++ b/main.cpp	Sat Oct 03 18:04:16 2015 +0000
@@ -1,141 +1,178 @@
+ //--------------------------------------------------------------------------------------------------------------------------//
+ // Motorscript voor 2 motoren voor de "SJOEL ROBOT", Groep 7
+ //--------------------------------------------------------------------------------------------------------------------------//
+ // Libraries
+ //--------------------------------------------------------------------------------------------------------------------------//
 #include "mbed.h"
 #include "MODSERIAL.h"
 #include "HIDScope.h"
 #include "QEI.h"
 #include "biquadFilter.h"
- 
-Serial pc(USBTX, USBRX); // tx, rx
-QEI Encoder2(D3, D2, NC, 32);
-QEI Encoder1(D13,D12,NC, 32);
-HIDScope scope(4);
+
+//--------------------------------------------------------------------------------------------------------------------------//
+// Constanten/Inputs/Outputs
+//--------------------------------------------------------------------------------------------------------------------------//
+    MODSERIAL pc(USBTX, USBRX);             // To/From PC
+    QEI Encoder2(D3, D2, NC, 32);           // Encoder Motor 2
+    QEI Encoder1(D13,D12,NC, 32);           // Encoder Motor 1
+    HIDScope scope(4);                      // Scope, 4 channels
 
-//Ledjes
-DigitalOut LedR(LED_RED);
-DigitalOut LedG(LED_GREEN);
-DigitalOut LedB(LED_BLUE);
+// LEDs
+    DigitalOut LedR(LED_RED);
+    DigitalOut LedG(LED_GREEN);
+    DigitalOut LedB(LED_BLUE);
 
-//Motor
-DigitalOut motor1direction(D6);
-PwmOut motor1speed(D7);
-DigitalOut motor2direction(D4);
-PwmOut motor2speed(D5);
-
-//Tickers
-Ticker ScopeTime;
-Ticker myControllerTicker2;
+// Motor
+    DigitalOut motor1direction(D7);         // Motor 1, Direction & Speed
+    PwmOut motor1speed(D6);
+    DigitalOut motor2direction(D4);         // Motor 2, Direction & Speed
+    PwmOut motor2speed(D5);
 
-//Startwaarden
-double reference2, reference1;
-double position2 = 0, position1 = 0;
-double m2_ref = 0, m1_ref = 0;
-int count = 0;
-double Grens2 = 90, Grens1 = 90;
-double Stapgrootte = 5;
+// Tickers
+    Ticker ScopeTime;
+    Ticker myControllerTicker2;
+    Ticker myControllerTicker1;
+
+// Constants
+    double reference2, reference1;
+    double position2 = 0, position1 = 0;
+    double m2_ref = 0, m1_ref = 0;
+    int count = 0;
+    double Grens2 = 90, Grens1 = 90;
+    double Stapgrootte = 5;
 
-//Sample time (motor2-step)
-const double m2_Ts = 0.01;
-const double m1_Ts = 0.01;
+//Sample time (motor-step)
+    const double m2_Ts = 0.01, m1_Ts = 0.01;
 
-//Controller gain Motor 
-const double m2_Kp = 5,m2_Ki = 0.05, m2_Kd = 2;
-const double m1_Kp = 5,m1_Ki = 0.05, m1_Kd = 2;
-double m2_err_int = 0, m2_prev_err = 0;
-double m1_err_int = 0, m1_prev_err = 0;
+//Controller gain Motor 2 & 1
+    const double m2_Kp = 5,m2_Ki = 0.05, m2_Kd = 2;
+    const double m1_Kp = 5,m1_Ki = 0.05, m1_Kd = 2;
+    double m2_err_int = 0, m2_prev_err = 0;
+    double m1_err_int = 0, m1_prev_err = 0;
 
-//Derivative filter coeffs Motor 2
-const double BiGain = 0.016955;
-const double m2_f_a1 = -0.96608908283*BiGain, m2_f_a2 = 0.0*BiGain, m2_f_b0 = 1.0*BiGain, m2_f_b1 = 1.0*BiGain, m2_f_b2 = 0.0*BiGain;
+//Derivative filter coeffs Motor 2 & 1
+    const double BiGain2 = 0.016955, BiGain1 = 0.016955;
+    const double m2_f_a1 = -0.96608908283*BiGain2, m2_f_a2 = 0.0*BiGain2, m2_f_b0 = 1.0*BiGain2, m2_f_b1 = 1.0*BiGain2, m2_f_b2 = 0.0*BiGain2;
+    const double m1_f_a1 = -0.96608908283*BiGain1, m1_f_a2 = 0.0*BiGain1, m1_f_b0 = 1.0*BiGain1, m1_f_b1 = 1.0*BiGain1, m1_f_b2 = 0.0*BiGain1;
 
 // Filter variables
-double m2_f_v1 = 0, m2_f_v2 = 0;
+    double m2_f_v1 = 0, m2_f_v2 = 0;
+    double m1_f_v1 = 0, m1_f_v2 = 0;
 
+//--------------------------------------------------------------------------------------------------------------------------//
+// General Functions
+//--------------------------------------------------------------------------------------------------------------------------//
 
 //HIDScope
-void ScopeSend()//Functie die de gegevens voor de scope uitleest en doorstuurt
-{
-    scope.set(0, reference2 - position2);
-    scope.set(1, position2);
-    scope.set(2, reference1 - position1);    
-    scope.set(3, position1);
-    scope.send();
+    void ScopeSend()//Functie die de gegevens voor de scope uitleest en doorstuurt
+    {
+        scope.set(0, reference2 - position2);
+        scope.set(1, position2);
+        scope.set(2, reference1 - position1);    
+        scope.set(3, position1);
+        scope.send();
     
-}
+    }
 
 // Biquad filter
-double biquad( double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2 )
-{
-    double v = u - a1*v1 - a2*v2;
-    double y = b0*v + b1*v1 + b2*v2;
-    v2 = v1; v1 = v;
-    return y;
-}    
+    double biquad( double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2 )
+    {
+        double v = u - a1*v1 - a2*v2;
+        double y = b0*v + b1*v1 + b2*v2;
+        v2 = v1; v1 = v;
+        return y;
+    }    
 
 
 // Reusable PID controller
-double PID( double e, const double Kp, const double Ki, const double Kd, double Ts, double &e_int, double &e_prev, double &f_v1, double &f_v2,
-const double f_a1,const double f_a2, const double f_b0, const double f_b1, const double f_b2)
-{
+    double PID( double e, const double Kp, const double Ki, const double Kd, double Ts, double &e_int, double &e_prev, double &f_v1, double &f_v2,
+        const double f_a1,const double f_a2, const double f_b0, const double f_b1, const double f_b2)
+    {
     // Derivative
-    double e_der = (e-e_prev)/Ts;
-    e_der = biquad(e_der,f_v1,f_v2,f_a1,f_a2,f_b0,f_b1,f_b2);
-    e_prev = e;
+        double e_der = (e-e_prev)/Ts;
+        e_der = biquad(e_der,f_v1,f_v2,f_a1,f_a2,f_b0,f_b1,f_b2);
+        e_prev = e;
     // Integral
-    e_int = e_int + Ts*e;
+        e_int = e_int + Ts*e;
     // PID
-    return Kp * e + Ki*e_int + Kd*e_der;
-}
+        return Kp * e + Ki*e_int + Kd*e_der;
+    }
+//--------------------------------------------------------------------------------------------------------------------------//
+// Motor control functions
+//--------------------------------------------------------------------------------------------------------------------------//
 
 // Motor2 control
-void motor2_Controller() 
-{
-    reference2 = m2_ref; // Setpoint motor 2
-    position2 = Encoder2.getPulses()*360/(32*131); // Aantal Degs motor 2
-    double m2_P1 = PID( reference2 - position2, m2_Kp, m2_Ki, m2_Kd, m2_Ts, m2_err_int, m2_prev_err, m2_f_v1, m2_f_v2, m2_f_a1, m2_f_a2, 
-    m2_f_b0, m2_f_b1, m2_f_b2);
-    double m2_P2 = biquad(m2_P1, m2_f_v1, m2_f_v2, m2_f_a1, m2_f_a2,m2_f_b0, m2_f_b1, m2_f_b2);
-    motor2speed = abs(m2_P2); // Speed control
-    if(m2_P2 > 0) // Direction control
-    {  
-        motor2direction = 0;
-    }
-    else
+    void motor2_Controller() 
     {
-        motor2direction = 1;
+        // Setpoint motor 2
+            reference2 = m2_ref;                           // Reference in degrees
+            position2 = Encoder2.getPulses()*360/(32*131); // Position in degrees
+        // Speed control
+            double m2_P1 = PID( reference2 - position2, m2_Kp, m2_Ki, m2_Kd, m2_Ts, m2_err_int, m2_prev_err, m2_f_v1, m2_f_v2, m2_f_a1, m2_f_a2, 
+                m2_f_b0, m2_f_b1, m2_f_b2);
+            double m2_P2 = biquad(m2_P1, m2_f_v1, m2_f_v2, m2_f_a1, m2_f_a2,m2_f_b0, m2_f_b1, m2_f_b2); // Filter of motorspeed input
+            motor2speed = abs(m2_P2); 
+        // Direction control
+            if(m2_P2 > 0) 
+            {    
+                motor2direction = 0;
+            }
+            else
+            {
+                motor2direction = 1;
+            }
+    }   
+
+// Motor1 control
+    void motor1_Controller() 
+    {
+        // Setpoint Motor 1
+            reference1 = m1_ref;                           // Reference in degrees
+            position1 = Encoder1.getPulses()*360/(32*131); // Position in degrees
+        // Speed control
+            double m1_P1 = PID( reference1 - position1, m1_Kp, m1_Ki, m1_Kd, m1_Ts, m1_err_int, m1_prev_err, m1_f_v1, m1_f_v2, m1_f_a1, m1_f_a2, 
+                m1_f_b0, m1_f_b1, m1_f_b2); 
+            double m1_P2 = biquad(m1_P1, m1_f_v1, m1_f_v2, m1_f_a1, m1_f_a2, m1_f_b0, m1_f_b1, m1_f_b2); 
+            motor1speed = abs(m1_P2); 
+        // Direction control    
+            if(m1_P2 > 0)
+            {  
+                motor1direction = 1;
+            }
+            else
+            {
+                motor1direction = 0;
+            }
     }
-    reference1 = m1_ref; // Setpoint
-    position1 = Encoder1.getPulses()*360/(32*131); // Aantal Degs
-    double m1_P1 = PID( reference1 - position1, m1_Kp, m1_Ki, m1_Kd, m1_Ts, m1_err_int, m1_prev_err, m2_f_v1, m2_f_v2, m2_f_a1, m2_f_a2, 
-    m2_f_b0, m2_f_b1, m2_f_b2); //Is gefilerd met dezelfde coeffs als motor 2
-    double m1_P2 = biquad(m1_P1, m2_f_v1, m2_f_v2, m2_f_a1, m2_f_a2,m2_f_b0, m2_f_b1, m2_f_b2); //Is gefilterd met dezelfde coeffs als motor 2
-    motor2speed = abs(m1_P2); // Speed control
-    if(m1_P2 > 0) // Direction control
-    {  
-        motor1direction = 0;
-    }
-    else
-    {
-        motor1direction = 1;
-    }
-}
 
-
-
-
+//--------------------------------------------------------------------------------------------------------------------------//
+// Main function
+//--------------------------------------------------------------------------------------------------------------------------//
 int main()
-{   
-    LedR.write(1);
-    LedB.write(1);
-    LedG.write(1);
-    pc.baud(115200);
-    pc.printf("Tot aan loop werkt\n");
+{  
+//--------------------------------------------------------------------------------------------------------------------------//
+// Initalizing
+//--------------------------------------------------------------------------------------------------------------------------// 
+    //LEDs OFF
+        LedR = LedB = LedG = 1;
     
-    ScopeTime.attach_us(&ScopeSend, 10e4);
-    myControllerTicker2.attach(&motor2_Controller, 0.01f ); // 100 Hz
-
+    //PC connection & check
+        pc.baud(115200);
+        pc.printf("Tot aan loop werkt\n");
+    
+    // Tickers
+        ScopeTime.attach(&ScopeSend, 0.01f);                    // 100 Hz, Scope
+        myControllerTicker2.attach(&motor2_Controller, 0.01f ); // 100 Hz, Motor 2
+        myControllerTicker1.attach(&motor1_Controller, 0.01f ); // 100 Hz, Motor 1
+    
+//--------------------------------------------------------------------------------------------------------------------------//
+// Control Program
+//--------------------------------------------------------------------------------------------------------------------------//
     while(true)
     {
         char c = pc.getc();
-        if(c == 'e') //Ga 1 programma omhoog
+    // 1 Program UP
+        if(c == 'e') 
         {
             count = count + 1;
             if(count > 2)
@@ -144,15 +181,17 @@
                 }
 
         }
-        if(c == 'd') //Ga 1 programma omlaag
+     // 1 Program DOWN
+        if(c == 'd')
         {
             count = count - 1;
             if(count < 0)
                 {
                     count = 0;
                 }
-        }      
-        if(count == 0) //Motor 2 control, Groene LED
+        }
+    // PROGRAM 0: Motor 2 control and indirect control of motor 1, Green LED      
+        if(count == 0)
         {
                 
                 LedR = LedB = 1;
@@ -160,21 +199,26 @@
                 if(c == 'r')
                 {
                     m2_ref = m2_ref + Stapgrootte;
+                    m1_ref = m1_ref - Stapgrootte;
                     if (m2_ref > Grens2)
                     {
                         m2_ref = Grens2;
+                        m1_ref = -1*Grens1;
                     }
                 }
                 if(c == 'f')
                 {
                     m2_ref = m2_ref - Stapgrootte;
+                    m1_ref = m1_ref + Stapgrootte;
                     if (m2_ref < -1*Grens2)
                     {
                         m2_ref = -1*Grens2;
+                        m1_ref = Grens1;
                     }
                 }
         }
-        if(count == 1) //Motor 1 control, Rode LED
+    // PROGRAM 1: Motor 1 control, Red LED
+        if(count == 1) 
         {
                 LedG = LedB = 1;
                 LedR = 0;
@@ -195,12 +239,13 @@
                     }
                 }
         }
-        if(count == 2) //Vuur mechanisme & Reset, Blauwe LED
+    // PROGRAM 2: Firing mechanism & Reset, Blue LED
+        if(count == 2) 
         {
 
                 LedR = LedG = 1;
                 LedB = 0;
-                //VUUUR!
+                //VUUUUR!! (To Do)
                 wait(1);
                 m2_ref = 0;
                 m1_ref = 0;