Diff: main.cpp

Revision:

13:a6770307a5d2

Parent:

12:69ab81cf5b7d

Child:

14:baebaef79aa6

--- a/main.cpp	Tue Sep 29 16:28:55 2015 +0000
+++ b/main.cpp	Tue Sep 29 16:48:08 2015 +0000
@@ -34,18 +34,19 @@
 
 // Controller gain
 const double m2_Kp = 0.07,m2_Ki = 0.005, m2_Kd = 0.005;
-double m2_err_int = 0, m1_prev_err = 0;
+double m2_err_int = 0, m2_prev_err = 0;
 
 //Derivative filter coeffs
-const double m2_f_a1 = 1.0, m2_f_a2 = 1.0, m2_f_b0 = 1.0, m2_f_b1 = 1.0, m2_f_b2 = 1.0;
+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;
 // Filter variables
 double m2_f_v1 = 0, m2_f_v2 = 0;
 
 // 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;
+    double v = u - a1*v1 - a2*v2;
+    double y = b0*v + b1*v1 + b2*v2;
     v2 = v1; v1 = v;
     return y;
 }    
@@ -53,12 +54,12 @@
 
 // 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_b3)
+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
+    e_prev = e;
     // Integral
     e_int = e_int + Ts*e;
     // PID
@@ -70,7 +71,7 @@
 {
     reference = potmeter2.read()*360;
     position = Encoder.getPulses()*360/(0.5*128*131); // Aantal Degs
-    double P2 = PID( reference − position, 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, 
+    double P2 = PID( reference - position, 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);
     motor2speed = abs(P2);
     if(P2 > 0)