Diff: main.cpp

Revision:

11:ecd83b303252

Parent:

10:09ba965045a7

Child:

12:26759959c960

--- a/main.cpp	Wed Oct 07 12:53:13 2015 +0000
+++ b/main.cpp	Wed Oct 07 13:30:20 2015 +0000
@@ -107,30 +107,37 @@
         integrate_error_turn=integrate_error_turn + sample_time*error;        // integral error output
 //                                                                                        // overwrite previous integrate error by adding the current error multiplied by the sample time.
 //        
-        //double error_derivative_turn=(error - previous_error_turn)/sample_time;    // derivative error output
+        double error_derivative_turn=(error - previous_error_turn)/sample_time;    // derivative error output
     
         // FILTER error_derivative_turn (lowpassfilter)
-            // biquadFilter Lowpassfilter(mT_f_a1,mT_f_a2,mT_f_b0,mT_f_b1,mT_f_b2);
-                //const double mT_f_a1=-1.965293372622690e+00;
+        
+            //const double mT_f_a1=-1.965293372622690e+00;
                 //const double mT_f_a2=9.658854605688177e-01;
                 //const double mT_f_b0=1.480219865318266e-04;
                 //const double mT_f_b1=2.960439730636533e-04;
                 //const double mT_f_b2=1.480219865318266e-04; // Motor Turn filter constants
-            // Lowpassfilter.step(e_der)
+                
+            // biquadFilter Lowpassfilter(mT_f_a1,mT_f_a2,mT_f_b0,mT_f_b1,mT_f_b2);
+
+            // error_derivative_turn=Lowpassfilter.step(error_derivative_turn);
         
-        //previous_error_turn=error;                                // current error is saved to memory constant to be used in 
+        previous_error_turn=error;                                // current error is saved to memory constant to be used in 
                                                                   // the next loop for calculating the derivative error 
-                                                                  
-        double Gain_I_turn=0.01;
-        // double Gain_D_turn=1;
+        
+        // pwm_motor_I=(integrate_error_turn + sample_time*error)*gain;  pwm = (4*0.01 + 4)* Gain => 0.1 pwm gewenst (na 1 seconde een verschil van 4 graden)  
+                                                                            // 0.1 / (4.01) = Gain = 0.025
+        //                                                           
+        double Gain_I_turn=0.025;  //(1/2000) //0.00000134
+        
+        double Gain_D_turn=0.001;
 
         pwm_to_motor_turn = error*Gain_P_turn;                     // output P controller to pwm   
 
         pwm_motor_turn_P = error*Gain_P_turn;                     // output P controller to pwm        
         pwm_motor_turn_I = integrate_error_turn*Gain_I_turn;      // output I controller to pwm
-//        double pwm_motor_turn_D = error_derivative_turn*Gain_D_turn;     // output D controller to pwm
+        pwm_motor_turn_D = error_derivative_turn*Gain_D_turn;     // output D controller to pwm
 
-        pwm_to_motor_turn = pwm_motor_turn_P + integrate_error_turn*Gain_I_turn;
+        pwm_to_motor_turn = pwm_motor_turn_P + pwm_motor_turn_I;
 
 //        
 //        double pwm_to_motor_turn = pwm_motor_turn_P + pwm_motor_turn_I + pwm_motor_turn_D; // Total output PID controller to pwm