Diff: main.cpp

Revision:

2:c2c76a7a7250

Parent:

1:b66e14435f70

--- a/main.cpp	Wed Oct 11 09:11:20 2017 +0000
+++ b/main.cpp	Fri Oct 13 08:00:35 2017 +0000
@@ -25,13 +25,23 @@
 Ticker controller;
 
 const double pi = 3.1415926535897;
-const float M1_KP = 10, M1_KI = 0.0;
-const double M1_TS = 0.0001;
+const float M1_KP = 20 , M1_KI = 1.0, M1_KD = 0.5;
+const double M1_TS = 0.001;
 const float RAD_PER_PULSE = (2*pi)/4200;
-const float CONTROLLER_TS = 0.0001;     //TIME INTERVAL/ hZ
-double m1_err_int = 0;
+const float CONTROLLER_TS = 0.001;     //TIME INTERVAL/ hZ
+double m1_err_int = 0, m1_prev_err = 0;
+const double M1_F_A1 = 1.0, M1_F_A2 = 2.0, M1_F_B0 = 1.0, M1_F_B1 = 3.0, M1_F_B2 = 4.0;
+double m1_f_v1=0, m1_f_v2 = 0;
 
-// the working P controller beneath here
+//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;
+}
+
+/* the working P controller beneath here
 //float P(double error, const float Kp){
 //  return Kp * error;
 //    }
@@ -41,6 +51,19 @@
 double PI(double e, const double Kp, const double Ki, double Ts, double &e_int){
     e_int += Ts*e;
     return Kp*e+Ki*e_int;
+}*/
+
+//new PID part
+
+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;
+// Integral
+    e_int += Ts*e;
+// PID
+    return Kp * e + Ki * e_int + Kd * e_der;
 }
 
 /* Working P controller part
@@ -78,7 +101,7 @@
 }
 */
 
-// New PI part 
+/*/ New PI part 
 
 void m1_Controller(){
     double reference = 10*potMeter1.read();
@@ -112,6 +135,41 @@
     
     
 }
+*/
+
+void m1_Controller() {
+    double reference = potMeter1.read();
+    double position = RAD_PER_PULSE*Encoder1.getPulses();
+    //float motor1 = PID( reference - position, 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 );
+    //pc.printf("\r value motor1: %f. reference Pot: %f. Position: %f \n", motor1, reference, position);
+    pc.printf("\r value motor1: . reference Pot: %f. Position: %f \n", reference, position);
+
+    motor1PWM = motor1;
+
+        if(motor1 > 0.5){
+            motor1DC = 1;
+        
+            ledr = 1;
+            ledg = 1;       //Blau
+            ledb = 0;
+        }
+        else if (motor1<-0.5) {
+            motor1DC = 0; 
+        
+            ledb = 1;
+            ledr = 1;
+            ledg = 0;       //Groen
+        
+        }
+        else{ 
+            motor1PWM = 0;
+        
+            ledb = 1;       //Rood
+            ledr = 0;
+            ledg = 1;
+        }
+
+}
 
 int main(){
     pc.baud(115200);
@@ -123,8 +181,8 @@
     double reference = 10*potMeter1.read();
     double position = RAD_PER_PULSE*Encoder1.getPulses();
     // double motor1 = P(reference-position, M1_KP); --> old one
-    float motor1 = PI(reference - position, M1_KP, M1_KI, M1_TS, m1_err_int);
-    
-      pc.printf("\r value motor1: %f. reference Pot: %f. Position: %f \n", motor1, reference, position);         
+    //float motor1 = PI(reference - position, M1_KP, M1_KI, M1_TS, m1_err_int);
+    float motor1 = PID( reference - position, 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 );
+             
     }
 }