Diff: main.cpp

Revision:

8:b932f8b71d3a

Parent:

7:88d1ccba9200

Child:

9:edf01d06935e

--- a/main.cpp	Wed Nov 01 00:01:58 2017 +0000
+++ b/main.cpp	Wed Nov 01 00:26:33 2017 +0000
@@ -13,8 +13,8 @@
 DigitalOut ledg(LED_GREEN);
 DigitalOut motor1DC(D7);
 DigitalOut motor2DC(D4);
-PwmOut motor1PWM(D6);
-PwmOut motor2PWM(D5);
+FastPWM motor1PWM(D6);
+FastPWM motor2PWM(D5);
 
 AnalogIn   potMeter1(A0);
 AnalogIn   potMeter2(A1);
@@ -38,28 +38,28 @@
 
 //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.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.001;     //TIME INTERVAL/ hZ
-const float     M1_KP = 10;
+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.2; //was KP=10 KI=0.5 KD=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_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;
 
-
-// --- const biquad filter---------------------------
+// Constants Biquad
 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_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;
@@ -69,22 +69,24 @@
 //-----------------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 e_der1 = (e1 - e_prev1)/Ts; // Ts = motor1-timestep             // Derivative
+    double e_der1 = (e1 - e_prev1)/Ts; // Ts = motor1-timestep                 // Derivative
     // biquad part, see slide
     //e_der = 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;                                //PID
+    e_int1 += Ts*e1;                                                          // Integral   
+    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 e_der2 = (e2 - e_prev2)/Ts; // Ts = motor1-timestep             // Derivative
+    double e_der2 = (e2 - e_prev2)/Ts; // Ts = motor1-timestep                 // Derivative
     // biquad part, see slide
     //e_der = 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
-    return Kp2*e2 + Ki2*e_int2 + Kd2 * e_der2;                                //PID
+    e_int2 += Ts*e2;                                                          // Integral   
+    return Kp2*e2 + Ki2*e_int2 + Kd2 * e_der2;                                    //PID
 }
 
 //------------Get reference position-----------------START
@@ -135,14 +137,14 @@
     //motor1PWM = motor1;
     //motor2PWM = motor2;
 
-    if(delta1 > 5.0){
+    if(delta1 > 10.0){
         motor1DC = 0;
         
         ledr = 1;
         ledg = 1;       //Blau
         ledb = 0;
     }
-    else if (delta1< -5.0) {
+    else if (delta1< -10.0) {
         motor1DC = 1; 
         
         ledb = 1;
@@ -166,35 +168,35 @@
 if(delta2 > 10.0){
         motor2DC = 0;
         
-        //ledr = 1;
-        //ledg = 1;       //Blau
-        //ledb = 0;
+        ledr = 1;
+        ledg = 1;       //Blau
+        ledb = 0;
     }
     else if (delta2<-10.0) {
         motor2DC = 1; 
         
-        //ledb = 1;
-        //ledr = 1;
-        //ledg = 0;       //Groen
+        ledb = 1;
+        ledr = 1;
+        ledg = 0;       //Groen
         
     }
     else{ 
     motor2PWM = 0;
         
-        //ledb = 1;       //Rood
-        //ledr = 0;
-        //ledg = 1;
+        ledb = 1;       //Rood
+        ledr = 0;
+        ledg = 1;
     }
     
     motor2 = abs(delta2)/1000.0f;
-    if(motor1 >= 0.50f) {
-        motor1 = 0.50f;
+    if(motor2 >= 0.50f) {
+        motor2 = 0.50f;
     }
     
     motor1PWM = motor1 + 0.50f;
-    motor2PWM = motor1 + 0.50f;
+    motor2PWM = motor2 + 0.50f;
     
-    //pc.printf("\r motorvalues (M1,M2):(%f,%f) \n", motor1PWM, motor2PWM);
+    //pc.printf("\r motorvalues (M1,M2):(%f,%f), error:( \n", , motor1PWM, motor2PWM);
     //pc.printf("\r 
 }