Diff: main.cpp

Revision:

26:70e5b6908e0a

Parent:

25:ae908de29943

Child:

27:e5d6a79abf32

Child:

28:d0b26cfe5d80

--- a/main.cpp	Sat Oct 03 18:04:16 2015 +0000
+++ b/main.cpp	Wed Oct 21 13:37:34 2015 +0000
@@ -42,23 +42,20 @@
     double Stapgrootte = 5;
 
 //Sample time (motor-step)
-    const double m2_Ts = 0.01, m1_Ts = 0.01;
+    const double m2_Ts = 0.001, m1_Ts = 0.01;
 
 //Controller gain Motor 2 & 1
-    const double m2_Kp = 5,m2_Ki = 0.05, m2_Kd = 2;
+    const double m2_Kp = 2.1/57,m2_Ki = 3.9/57, m2_Kd = 0.1/57;
     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;
+    
+    const double BiGainEMG_L1= 0.231938;
+    const double EMGL1_a1 = -0.25537145181, EMGL1_a2 = 0.18312488356, EMGL1_b0 = 1.0*BiGainEMG_L1, EMGL1_b1 = 2.00000000000*BiGainEMG_L1, EMGL1_b2 = 1.0*BiGainEMG_L1; // coefficients for low-pass filter
+    
+    biquadFilter m2_lowpass (EMGL1_a1, EMGL1_a2, EMGL1_b0, EMGL1_b1, EMGL1_b2);
 
-//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 m1_f_v1 = 0, m1_f_v2 = 0;
-
+    double x;
 //--------------------------------------------------------------------------------------------------------------------------//
 // General Functions
 //--------------------------------------------------------------------------------------------------------------------------//
@@ -70,32 +67,24 @@
         scope.set(1, position2);
         scope.set(2, reference1 - position1);    
         scope.set(3, position1);
+        scope.set(4, x);
         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;
-    }    
+    }  
 
 
 // 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)
     {
     // 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_der = m2_lowpass.step(e_der);
         e_prev = e;
     // Integral
         e_int = e_int + Ts*e;
     // PID
-        return Kp * e + Ki*e_int + Kd*e_der;
+        x = Kp * e + Ki*e_int + Kd*e_der;
+        return x;
     }
 //--------------------------------------------------------------------------------------------------------------------------//
 // Motor control functions
@@ -106,11 +95,10 @@
     {
         // Setpoint motor 2
             reference2 = m2_ref;                           // Reference in degrees
-            position2 = Encoder2.getPulses()*360/(32*131); // Position in degrees
+            position2 = 1.0*Encoder2.getPulses()*360.0/(32.0*131.0); // 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
+            double m2_P1 = PID( reference2 - position2, m2_Kp, m2_Ki, m2_Kd, m2_Ts, m2_err_int, m2_prev_err);
+            double m2_P2 = m2_P1; // Filter of motorspeed input
             motor2speed = abs(m2_P2); 
         // Direction control
             if(m2_P2 > 0) 
@@ -130,9 +118,8 @@
             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); 
+            double m1_P1 = PID( reference1 - position1, m1_Kp, m1_Ki, m1_Kd, m1_Ts, m1_err_int, m1_prev_err); 
+            double m1_P2 = m1_P1; 
             motor1speed = abs(m1_P2); 
         // Direction control    
             if(m1_P2 > 0)
@@ -162,7 +149,7 @@
     
     // Tickers
         ScopeTime.attach(&ScopeSend, 0.01f);                    // 100 Hz, Scope
-        myControllerTicker2.attach(&motor2_Controller, 0.01f ); // 100 Hz, Motor 2
+        myControllerTicker2.attach(&motor2_Controller, m2_Ts ); // 1000 Hz, Motor 2
         myControllerTicker1.attach(&motor1_Controller, 0.01f ); // 100 Hz, Motor 1
     
 //--------------------------------------------------------------------------------------------------------------------------//