Diff: main.cpp

Revision:

18:622e717da184

Parent:

17:10c18ca3368b

--- a/main.cpp	Mon Nov 06 23:36:22 2017 +0000
+++ b/main.cpp	Tue Nov 07 10:39:27 2017 +0000
@@ -1,5 +1,4 @@
 #include "mbed.h"
-#include "QEI.h"
 #include "MODSERIAL.h"
 #include "math.h"
 #include "FastPWM.h"
@@ -77,7 +76,6 @@
 //Start constants PID ------------------------------- 
 const double M1_TS = 0.01;      // (was 0.0001) 0.001 and 0.01 work without biquad filter. // // Sample time (motor - timestep)
 
-
 const float     M1_KP = 10; 
 const float     M1_KI = 0.5;
 const float     M1_KD = 0.5; //was KP=10 KI=0.5 KD=0.5
@@ -86,7 +84,7 @@
 
 const float     M2_KP = 30; 
 const float     M2_KI = 0.5;
-const float     M2_KD = 0.5; //was KP=10 KI=0.5 KD=0.5
+const float     M2_KD = 0.5; //was KP=10 KI=0.5 KD=0.5, KP increased for faster response
 double          m2_err_int = 0;
 double          m2_prev_err = 0;
 
@@ -94,28 +92,14 @@
 BiQuad bq1(6.38946e-01,-1.27789e+00,6.38946e-01,-1.14298e+00,4.12802e-01);
 BiQuad bq2(6.38946e-01,-1.27789e+00,6.38946e-01,-1.14298e+00,4.12802e-01);
 BiQuadChain bqc;
-
-const double    M1_F_A1 = 1.0;
-const double    M1_F_A2  = 2.0; 
-const double    M1_F_B0  = 1.0; 
-const double    M1_F_B1  = 3.0;
-const double    M1_F_B2  = 4.0;
-double          m1_f_v1  = 0;
-double          m1_f_v2  = 0;
-
-
-
 //---------------------------------End of constants PID
 
 //-----------------Start PID part----------------------------START
-double PID1(double e1, const double Kp1, const double Ki1, const double Kd1, double Ts, double &e_int1, double &e_prev1, 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){
+double PID1(double e1, const double Kp1, const double Ki1, const double Kd1, double Ts, double &e_int1, double &e_prev1){
 
     double e_der1 = (e1 - e_prev1)/Ts; // Ts = motor1-timestep                 // Derivative
     
-    e_der1 = bqc.step(e_der1);
-    // biquad part, see slide
-    //e_der = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2);
-    
+    e_der1 = bqc.step(e_der1);   
     e_prev1 = e1;
     e_int1 += Ts*e1;
     
@@ -129,7 +113,7 @@
     return Kp1*e1 + Ki1*e_int1 + Kd1 * e_der1;                                    //PID
 }
 
-double PID2(double e2, const double Kp2, const double Ki2, const double Kd2, double Ts, double &e_int2, double &e_prev2, 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){
+double PID2(double e2, const double Kp2, const double Ki2, const double Kd2, double Ts, double &e_int2, double &e_prev2){
 
     double e_der2 = (e2 - e_prev2)/Ts; // Ts = motor1-timestep                 // Derivative
     // biquad part, see slide
@@ -323,8 +307,8 @@
     pos_M1 = motor1_Position(); // current position for theta   
     pos_M2 = motor2_Position(); // current position for the radius
       
-    double deltaM1 = PID1(reference_motor1 - pos_M1, 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 deltaM2 = PID2(reference_motor2 - pos_M2, M2_KP, M2_KI, M2_KD, M1_TS, m2_err_int, m2_prev_err, m1_f_v1, m1_f_v2, M1_F_A1, M1_F_A2, M1_F_B0, M1_F_B1, M1_F_B2);
+    double deltaM1 = PID1(reference_motor1 - pos_M1, M1_KP, M1_KI, M1_KD, M1_TS, m1_err_int, m1_prev_err);
+    double deltaM2 = PID2(reference_motor2 - pos_M2, M2_KP, M2_KI, M2_KD, M1_TS, m2_err_int, m2_prev_err);
 
     double dTheta   = reference_motor1 - pos_M1;
     double dRadius  = reference_motor2 - pos_M2;
@@ -334,9 +318,6 @@
     pc.printf("\r DesPosition(X,Y):(%f,%f)\n", x,y);
     //pc.printf("\r pos(M1,M2):(%f,%f)\n", pos_M1, pos_M2);
     
-    //motor1PWM = motor1;
-    //motor2PWM = motor2;
-
 // --- Direction control motors --- START
     if(deltaM1 > 0.5){ 
         motor1DC = 0;