PIDcontroller_without_biquadfilter

Dependencies:   MODSERIAL QEI biquadFilter mbed

Fork of 20170910_PID_V1_0 by Arnoud Meutstege

Files at this revision

API Documentation at this revision

Comitter:
vd
Date:
Sun Oct 15 21:19:11 2017 +0000
Parent:
1:b66e14435f70
Commit message:
PIDcontroller_without_biquadfilter

Changed in this revision

biquadFilter.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r b66e14435f70 -r 02b385b96543 biquadFilter.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/biquadFilter.lib	Sun Oct 15 21:19:11 2017 +0000
@@ -0,0 +1,1 @@
+http://os.mbed.com/users/tomlankhorst/code/biquadFilter/#26861979d305
diff -r b66e14435f70 -r 02b385b96543 main.cpp
--- a/main.cpp	Wed Oct 11 09:11:20 2017 +0000
+++ b/main.cpp	Sun Oct 15 21:19:11 2017 +0000
@@ -3,8 +3,6 @@
 #include "MODSERIAL.h"
 #include "math.h"
 
-
-
 DigitalOut gpo(D0);
 DigitalOut ledb(LED_BLUE);
 DigitalOut ledr(LED_RED);
@@ -25,24 +23,78 @@
 Ticker controller;
 
 const double pi = 3.1415926535897;
-const float M1_KP = 10, M1_KI = 0.0;
-const double M1_TS = 0.0001;
+const double M1_TS = 0.01; // (was 0.0001) 0.001 and 0.01 work without biquad filter.
+
+//verplaatst
 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.01;     //TIME INTERVAL/ hZ
+
+const float M1_KP = 10, M1_KI = 1.0, M1_KD = 0.5; //was KP=10 KI=0.5 KD=0.5
+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 ;
+
+
+/*/ 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; // v=v1 vervangen
+v2 = v1; v1=v;
+return y;
+}
+*/
+
+/*
+const float RAD_PER_PULSE = (2*pi)/4200;
+const float CONTROLLER_TS = 0.001;     //TIME INTERVAL/ hZ
+
+*/
+
+//double m1_err_int = 0; --> PI
+
+
 
 // the working P controller beneath here
 //float P(double error, const float Kp){
 //  return Kp * error;
 //    }
 
-// New PI controller
+//New PID
+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;
+
+
+// biquad part, see slide
+//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 PI controller
 
 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;
 }
 
+*/
+
+
 /* Working P controller part
 
 void motor1_Controller(){
@@ -78,23 +130,22 @@
 }
 */
 
-// New PI part 
+// New PID 
 
 void m1_Controller(){
     double reference = 10*potMeter1.read();
     double position = RAD_PER_PULSE*Encoder1.getPulses();
-    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);
     motor1PWM = motor1;
     
-    if(motor1 > 0.5){
+    if(motor1 > 0.1){
         motor1DC = 1;
         
         ledr = 1;
         ledg = 1;       //Blau
         ledb = 0;
     }
-    else if (motor1<-0.5) {
+    else if (motor1< -0.1) {
         motor1DC = 0; 
         
         ledb = 1;
@@ -113,8 +164,45 @@
     
 }
 
+
+// New PI part 
+/*
+void m1_Controller(){
+    double reference = 10*potMeter1.read();
+    double position = RAD_PER_PULSE*Encoder1.getPulses();
+    float motor1 = PI(reference - position, M1_KP, M1_KI, M1_TS, m1_err_int);
+    motor1PWM = motor1;
+    
+    if(motor1 > 0.1){
+        motor1DC = 1;
+        
+        ledr = 1;
+        ledg = 1;       //Blau
+        ledb = 0;
+    }
+    else if (motor1< -0.1) {
+        motor1DC = 0; 
+        
+        ledb = 1;
+        ledr = 1;
+        ledg = 0;       //Groen
+        
+    }
+    else{ 
+    motor1PWM = 0;
+        
+        ledb = 1;       //Rood
+        ledr = 0;
+        ledg = 1;
+    }
+    
+    
+}
+
+*/
+
 int main(){
-    pc.baud(115200);
+    pc.baud(9600);
     //controller.attach(&m1_Controller, CONTROLLER_TS);            --> P one
     
     controller.attach(&m1_Controller, M1_TS);
@@ -123,8 +211,11 @@
     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);
+    double 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);           
     }
 }
+