Diff: main.cpp

Revision:

2:02b385b96543

Parent:

1:b66e14435f70

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);           
     }
 }
+