Diff: main.cpp

Revision:

6:d4f6d9400f53

Parent:

5:a1a5b5bebd5c

Child:

7:88d1ccba9200

diff -r a1a5b5bebd5c -r d4f6d9400f53 main.cpp
--- a/main.cpp	Tue Oct 31 23:04:25 2017 +0000
+++ b/main.cpp	Tue Oct 31 23:10:30 2017 +0000
@@ -33,50 +33,39 @@
 float gainM1 = 1/35.17;     // encoder pulses per degree theta
 float gainM2 = 0.01;      // gain for radius r
 
-// new PID constants, will have to be determined trough trial and error.
-
-double kp = 250;
-double ki = 100;
-double kd = 0;
-
-
 volatile float motor1;
 volatile float motor2;
 
 //Start constants PID ------------------------------- 
 const double pi = 3.1415926535897;
-const double M1_TS = 0.001f; // (was 0.0001) 0.001 and 0.01 work without biquad filter. // // Sample time (motor - timestep)
+const double M1_TS = 0.01; // (was 0.0001) 0.001 and 0.01 work without biquad filter. // // Sample time (motor - timestep)
 
 //verplaatst
 const float     RAD_PER_PULSE = (2*pi)/4200;
 const float     CONTROLLER_TS = 0.01;     //TIME INTERVAL/ hZ
-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
-double          m1_err_int = 0; 
-double          m1_prev_err = 0;
-
-
-//---- Biquad constants---------
-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;
+const float     M1_KP = 10, M1_KI = 0.5, 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 ;
 //---------------------------------End of constants PID
 
 //-----------------Start PID part----------------------------START
-double PID1(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){
+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; // Ts = motor1-timestep
+
+// biquad part, see slide
+//e_der = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2);
 
-    double e_der = (e - e_prev)/Ts;         // Derivative, Ts = motor1-timestep
-    // 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;
-    e_int += Ts*e;                          // Integral   
+e_prev = e;
     
-    return Kp*e + Ki*e_int + Kd * e_der;
+// Integral
+e_int += Ts*e;
+    
+    
+//PID
+return Kp*e + Ki*e_int + Kd * e_der;
 
 }
 
@@ -116,13 +105,11 @@
     float pos_M1 = motor1_Position(); // current position for theta   
     float pos_M2 = motor2_Position(); // current position for the radius
     
-    double delta1 = 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 delta2 = PID2(reference_motor2 - pos_M2, 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 delta1 = PID(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 delta2 = PID(reference_motor2 - pos_M2, 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 dTheta   = reference_motor1 - pos_M1;
     double dRadius  = reference_motor2 - pos_M2;
-    
-    
 
     pc.baud(115200);
     pc.printf("\r DesPosition(X,Y):(%f,%f), pos Error(dTheta, dError):(%f,%f)\n",x,y, delta1 ,delta2);