Diff: main.cpp

Revision:

5:a1a5b5bebd5c

Parent:

4:5f7d1654108d

Child:

6:d4f6d9400f53

--- a/main.cpp	Tue Oct 31 22:42:21 2017 +0000
+++ b/main.cpp	Tue Oct 31 23:04:25 2017 +0000
@@ -45,24 +45,17 @@
 
 //Start constants PID ------------------------------- 
 const double pi = 3.1415926535897;
-const double M1_TS = 0.001; // (was 0.0001) 0.001 and 0.01 work without biquad filter. // // Sample time (motor - timestep)
+const double M1_TS = 0.001f; // (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
-
-//----PID constants
-const float     M1_KP = 10;
-const float     M1_KI = 0.5;
+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;
 
-const float     M2_KP = 10;
-const float     M2_KI = 0.5;
-const float     M2_KD = 0.5; //was KP=10 KI=0.5 KD=0.5
-double          m2_err_int = 0; 
-double          m2_prev_err = 0;
 
 //---- Biquad constants---------
 const double    M1_F_A1  = 1.0 ;
@@ -72,30 +65,18 @@
 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 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 e_der1 = (e1 - e_prev1)/Ts;         // Derivative, Ts = motor1-timestep
+    double e_der = (e - e_prev)/Ts;         // Derivative, Ts = motor1-timestep
     // biquad part, see slide
-    //e_der1 = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2);
-    e_prev1 = e1;
-    e_int1 += Ts*e1;                          // Integral   
-       
-    return Kp1*e1 + Ki1*e_int1 + Kd1 * e_der1;
-       
-}
-
-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 e_der2 = (e2 - e_prev2)/Ts;         // Derivative, Ts = motor1-timestep
-    // biquad part, see slide
-    //e_der2 = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2);
-    e_prev2 = e2;
-    e_int2 += Ts*e2;                          // Integral   
+    //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   
     
-    return Kp2*e2 + Ki2*e_int2 + Kd2 * e_der2;
+    return Kp*e + Ki*e_int + Kd * e_der;
 
 }
 
@@ -103,7 +84,6 @@
 float Get_X_Position(){
     double X = potMeter1 * potmultiplier;
     return X;
-    
 }
 
 float Get_Y_Position(){
@@ -114,14 +94,11 @@
 
 //-------------Get current Position-------------------START
 double motor1_Position(){ // has as output Theta
-    double pos_m1 = gainM1*Encoder1.getPosition(); // current position for theta      
+    double pos_m1 = gainM1*Encoder1.getPosition(); // current position for theta   
     return pos_m1;       
 }
 double motor2_Position(){ //output R
     double pos_m2 = gainM2 *Encoder2.getPosition(); // current position for the radius;
-    pc.baud(115200);
-    pc.printf("\r x = %f",pos_m2); 
-    
     return pos_m2;
 }
 //-----------------------------------------------------END
@@ -140,7 +117,7 @@
     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, 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 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 dTheta   = reference_motor1 - pos_M1;
     double dRadius  = reference_motor2 - pos_M2;
@@ -148,8 +125,7 @@
     
 
     pc.baud(115200);
-    //pc.printf("\r DesPosition(X,Y):(%f,%f), pos Error(dTheta, dError):(%f,%f)\n",x,y, dTheta ,dRadius);
-    //pc.printf("\r PID1 result: %f\n", delta1);
+    pc.printf("\r DesPosition(X,Y):(%f,%f), pos Error(dTheta, dError):(%f,%f)\n",x,y, delta1 ,delta2);
     
     //motor1PWM = motor1;
     //motor2PWM = motor2;