Diff: main.cpp

Revision:

12:69ab81cf5b7d

Parent:

11:0793a78109a2

Child:

13:a6770307a5d2

--- a/main.cpp	Tue Sep 29 14:01:47 2015 +0000
+++ b/main.cpp	Tue Sep 29 16:28:55 2015 +0000
@@ -2,16 +2,15 @@
 #include "MODSERIAL.h"
 #include "HIDScope.h"
 #include "QEI.h"
+#include "biquadFilter.h"
  
 Serial pc(USBTX, USBRX); // tx, rx
 DigitalOut motor2direction(D4); //D4 en D5 zijn motor 2 (op het motorshield)
 PwmOut motor2speed(D5);
-DigitalIn button1(SW3);
-DigitalIn encoderA(D3);
-DigitalIn encoderB(D2);
 AnalogIn potmeter2(A5);
 QEI Encoder(D3, D2, NC, 128);
 HIDScope scope(3);
+
 Ticker ScopeTime;
 Ticker myControllerTicker;
 
@@ -30,24 +29,49 @@
     scope.send();
     
 }
+// Sample time (motor2-step)
+const double m2_Ts = 0.01;
+
 // Controller gain
-const double m2_Kp = 0.07,m2_Ki = 0.005;
-const double m2_Ts = 0.01;
-double m2_err_int = 0;
+const double m2_Kp = 0.07,m2_Ki = 0.005, m2_Kd = 0.005;
+double m2_err_int = 0, m1_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;
+// Filter variables
+double m2_f_v1 = 0, m2_f_v2 = 0;
 
-// Reusable PI controller
-double P( double error, const double Kp, const double Ki, double Ts, double &e_int) 
+// 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 )
 {
-    e_int = e_int + Ts*error;
-    return Kp * error + Ki*e_int;
+    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_b3)
+{
+    // 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
+    // Integral
+    e_int = e_int + Ts*e;
+    // PID
+    return Kp * e + Ki*e_int + Kd*e_der;
 }
 
-// Next task, measure the error and apply the output to the plant
+// Motor control
 void motor2_Controller() 
 {
     reference = potmeter2.read()*360;
     position = Encoder.getPulses()*360/(0.5*128*131); // Aantal Degs
-    double P2 = P( reference - position, m2_Kp, m2_Ki, m2_Ts, m2_err_int);
+    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)
     {