Diff: main.cpp

Revision:

41:424264a4c39c

Parent:

40:bbe7922723df

Child:

42:0a7898cb3e94

--- a/main.cpp	Wed Oct 14 11:19:58 2015 +0000
+++ b/main.cpp	Wed Oct 14 11:28:09 2015 +0000
@@ -81,6 +81,19 @@
                     // One revolution = 360 degrees
                     // degrees_per_encoder_tick = 360/(gear_ratio*ticks_per_magnet_rotation)=360/131*32=0.085877862594198 
 
+//                    ___________________________
+//                  //                           \\
+//                 ||     [FILTER CONSTANTS]      ||
+//                  \\___________________________//
+//
+
+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
+                
+biquadFilter Lowpassfilter_derivative(mT_f_a1,mT_f_a2,mT_f_b0,mT_f_b1,mT_f_b2);
     
 //                    ___________________________
 //                  //                           \\
@@ -229,25 +242,14 @@
 //                                       \\___________________________//
 //            // Calculate error then multiply it with the (P, I and D) gains, and store in pwm_to_motor \\
 
-        error=(reference_turn - position_turn);                                     // Current error (input controller)
-        
-        integrate_error_turn=integrate_error_turn + sample_time*error;              // integral error output
+        error=(reference_turn - position_turn);                                     // TURN: Current error (input controller)        
+        integrate_error_turn=integrate_error_turn + sample_time*error;              // TURN: 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
-    
-            // FILTER error_derivative_turn (lowpassfilter)
-        
-            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
-                
-            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);
+        double error_derivative_turn=(error - previous_error_turn)/sample_time;     // TURN: derivative error output
+        error_derivative_turn=Lowpassfilter_derivative.step(error_derivative_turn); // TURN: Filter
+        error_derivative_turn=Lowpassfilter_derivative.step(error_derivative_turn);
         
         previous_error_turn=error;                                // current error is saved to memory constant to be used in 
                                                                   // the next loop for calculating the derivative error 
@@ -304,9 +306,9 @@
 //                                       //                           \\
 //                                      ||        [Deactivate?]        ||
 //                                       \\___________________________//
-//                // Check whether the motor has reached reference position and can be shut down \\            
+//                // Check whether the motor has reached reference position and can be shut down            
             
-            if(fabs(error)<2)             // Shut down if error smaller than two degrees
+            if(fabs(error)<2)             // Shut down if error is smaller than two degrees
             {deactivate_PID_Controller_strike(P_gain_turn,I_gain_turn,D_gain_turn);}
             else
             {activate_PID_Controller_strike(P_gain_turn,I_gain_turn,D_gain_turn);}