Diff: main.cpp

Revision:

1:2f23368e8638

Parent:

0:f6306e179750

Child:

2:2563df4ee829

--- a/main.cpp	Wed Oct 07 17:20:12 2015 +0000
+++ b/main.cpp	Wed Oct 07 18:19:07 2015 +0000
@@ -35,16 +35,40 @@
 int reference2 = 0;
 
 // CONTROLLER SETTINGS
-const double K1 = 1;       // P constant motorcontrolle 1
-const double K2 = 1;        // p constant motorcontroller 2
+    // motor 1
+    const double m1_Kp = 1;         // Proportional constant     
+    const double m1_Ki = 0;         // integration constant
+    const double m1_Kd = 0;         // differentiation constant
+    // motor 2
+    const double m2_Kp = 1;
+    const double m2_Ki = 0;
+    const double m2_Kd = 0;
+// storage variables
+    // motor 1
+    double m1_err_int = 0; 
+    double m1_prev_err = 0;
+    // motor 2
+    double m2_err_int = 0;
+    double m2_prev_err = 0;
 
 
 // SETPOINT SETTINGS
 // define storage variables for setpoint values
-double c_setpoint1 = 0;
-double c_setpoint2 = 0;
+    double c_setpoint1 = 0;
+    double c_setpoint2 = 0;
 // define the maximum rate of change for the setpoint (velocity)
-double Vmax = 5;            // rad/sec
+    double Vmax = 5;            // rad/sec
+
+
+//// FILTER VARIABLES
+// storage variables
+        // differential action filter, same is used for both controllers
+        double m_f_v1 = 0;
+        double m_f_v2 = 0;
+
+// Filter coefficients
+        // differential action filter
+        const double m_f_a1 = 1.0, m_f_a2 = 2.0, m_f_b0 = 1.0, m_f_b1 = 3.0, m_f_b2 = 4.0;      // coefficients from sheets are used as first test.
 
 
 
@@ -102,6 +126,42 @@
 }
 
 
+// BIQUADFILTER CODE GIVEN IN SHEETS 
+// used for d-control
+double biquadfilter(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;
+    }
+
+
+// PID Controller given in sheets
+// aangepast om zelfde filter te gebruiken en om de termen later gesplitst te kunnen limiteren (windup preventie!!)
+double PID( double e, const double Kp, const double Ki, const double Kd, double Ts,double &e_int, double &e_prev)
+{
+// Proportional
+double P = Kp * e;
+
+// Derivative
+double e_derr = (e - e_prev)/Ts;
+e_derr = biquadfilter(e_derr, m_f_v1, m_f_v2, m_f_a1, m_f_a2, m_f_b0, m_f_b1, m_f_b2);
+e_prev = e;
+double D = Kd* e_derr;
+
+// Integral
+e_int = e_int + Ts * e;
+double I = e_int * Ki;
+
+// PID
+double output = P + I + D;
+return output;
+}
+
+
+
 // this function is a simple K control called by the motor function
 double K_control(double error,double K)
 {
@@ -124,7 +184,9 @@
     // scope.set(0,setpoint1);                                                                                                              // disabled because HIDScope is acting strangely
     // scope.set(1,rads1);
     // scope.send();
-    double output1 = K_control(error1, K1);        // bereken de controller output voor motor 1(zie hierboven)
+    
+    // double output1 = K_control(error1, m1_Kp);        // bereken de controller output voor motor 1(zie hierboven)
+    double output1 = PID(error1, m1_Kp, m1_Ki, m1_Kd, controlstep, m1_err_int, m1_prev_err);
     if(output1 > 0) {                    // uses the calculated output to determine the direction  of the motor
         motor1_rich.write(0);
         motor1_aan.write(output1);
@@ -143,7 +205,9 @@
     // scope.set(3,setpoint2);                                                                                                              // disabled because HIDScope is acting strangely
     // scope.set(4,rads2);
     // scope.send();
-    double output2 = K_control(error2, K2);        // bereken de controller output voor motor 1(zie hierboven)
+    
+    // double output2 = K_control(error2, m2_Kp);        // bereken de controller output voor motor 1(zie hierboven)
+    double output2 = PID(error2, m2_Kp, m2_Ki, m2_Kd, controlstep, m2_err_int, m2_prev_err);
     if(output2 > 0) {                    // uses the calculated output to determine the direction  of the motor
         motor2_rich.write(0);
         motor2_aan.write(output2);
@@ -156,7 +220,7 @@
 
 int main()
 {
-    // Ticker calling the primary functions. If neccessary use the ticker to change bools and execute in the main loop.
+    // Ticker calling the primary functions. If neccessary use the ticker to change bools and execute in the main loop as shown in the sheets.
     controller1.attach(&motor1_control, controlstep);
     controller2.attach(&motor2_control, controlstep);
     while(true)