Diff: main.cpp

Revision:

42:0a7898cb3e94

Parent:

41:424264a4c39c

Child:

43:fb69ef657f30

--- a/main.cpp	Wed Oct 14 11:28:09 2015 +0000
+++ b/main.cpp	Wed Oct 14 11:39:13 2015 +0000
@@ -105,21 +105,13 @@
 void setlooptimerflag(void);
 double get_reference(double input);
 
-//// Deactivate PID Controller strike
-void deactivate_PID_Controller_strike(double& P_gain, double& I_gain, double& D_gain)
-{
-    P_gain=0; 
-    I_gain=0; 
-    D_gain=0;
-}
+// STRIKE: Deactivate PID Controller
+void deactivate_PID_Controller(double& P_gain, double& I_gain, double& D_gain);
+// TURN: Activate PID Controller 
+void activate_PID_Controller_turn(double& P_gain, double& I_gain, double& D_gain);
+// STRIKE: Activate PID Controller
+void activate_PID_Controller_strike(double& P_gain, double& I_gain, double& D_gain);
 
-// Activate PID Controller strike
-void activate_PID_Controller_strike(double& P_gain, double& I_gain, double& D_gain)
-{
-    P_gain=10;  // hier waardes P,I,D veranderen (waardes bovenaan doen (tijdelijk) niks meer
-    I_gain=0.1; 
-    D_gain=10;
-}
 
 // Change reference position (two functions)
 void Change_Turn_Position_Left (double& reference, double change_one_bottle);
@@ -138,7 +130,7 @@
     
     const double change_one_bottle=45;    
     double position_turn;                   // Set constant to store current position of the Turn motor
-    double error;
+    double error_turn;
     double pwm_to_motor_turn;
     double pwm_motor_turn_P;
     double pwm_motor_turn_I;
@@ -149,9 +141,9 @@
     
     
     // TEST: Does change PID gains work? => works correctly
-    deactivate_PID_Controller_strike(P_gain_turn,I_gain_turn,D_gain_turn);
+    deactivate_PID_Controller(P_gain_turn,I_gain_turn,D_gain_turn); // Deactivate Turn motor
     pc.printf("P = %d I = %d D = %d \n\r");
-    activate_PID_Controller_strike(P_gain_turn,I_gain_turn,D_gain_turn);
+    activate_PID_Controller_turn(P_gain_turn,I_gain_turn,D_gain_turn);
     pc.printf("P = %d I = %d D = %d \n\r");
     
     // TEST: Does change reference function work? => still needs to be tested
@@ -242,21 +234,21 @@
 //                                       \\___________________________//
 //            // Calculate error then multiply it with the (P, I and D) gains, and store in pwm_to_motor \\
 
-        error=(reference_turn - position_turn);                                     // TURN: Current error (input controller)        
-        integrate_error_turn=integrate_error_turn + sample_time*error;              // TURN: integral error output
+        error_turn=(reference_turn - position_turn);                                     // TURN: Current error (input controller)        
+        integrate_error_turn=integrate_error_turn + sample_time*error_turn;              // 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;     // TURN: derivative error output
+        double error_derivative_turn=(error_turn - 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 
+        previous_error_turn=error_turn;                                // current error is saved to memory constant to be used in 
                                                                   // the next loop for calculating the derivative error 
 
-        pwm_to_motor_turn = error*P_gain_turn;                     // output P controller to pwm   
+        pwm_to_motor_turn = error_turn*P_gain_turn;                     // output P controller to pwm   
 
-        pwm_motor_turn_P = error*P_gain_turn;                     // output P controller to pwm        
+        pwm_motor_turn_P = error_turn*P_gain_turn;                     // output P controller to pwm        
         pwm_motor_turn_I = integrate_error_turn*I_gain_turn;      // output I controller to pwm
         pwm_motor_turn_D = error_derivative_turn*D_gain_turn;     // output D controller to pwm
 
@@ -297,7 +289,7 @@
 //                                       \\___________________________//
 //                                      // Check signals inside HIDSCOPE \\
         
-            scope.set(0,error);             // HIDSCOPE channel 0 : Current Error
+            scope.set(0,error_turn);             // HIDSCOPE channel 0 : Current Error
             scope.set(1,position_turn);     // HIDSCOPE channel 1 : Position_turn
             scope.set(2,pwm_to_motor_turn); // HIDSCOPE channel 2 : Pwm_to_motor_turn
             scope.send();                   // Send channel info to HIDSCOPE
@@ -308,10 +300,10 @@
 //                                       \\___________________________//
 //                // Check whether the motor has reached reference position and can be shut down            
             
-            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);}
+            if(fabs(error_turn)<2)             // Shut down if error is smaller than two degrees
+            {deactivate_PID_Controller(P_gain_turn,I_gain_turn,D_gain_turn);}
             else
-            {activate_PID_Controller_strike(P_gain_turn,I_gain_turn,D_gain_turn);}
+            {activate_PID_Controller_turn(P_gain_turn,I_gain_turn,D_gain_turn);}
     }
 }
 }
@@ -355,3 +347,24 @@
         keep_in_range(&reference, -90, 90);
     }
 
+// Deactivate or Activate PID_Controller
+void deactivate_PID_Controller(double& P_gain, double& I_gain, double& D_gain)
+{
+    P_gain=0; 
+    I_gain=0; 
+    D_gain=0;
+}
+
+void activate_PID_Controller_turn(double& P_gain, double& I_gain, double& D_gain)
+{
+    P_gain=0; // hier waardes P,I,D veranderen (waardes bovenaan doen (tijdelijk) niks meer
+    I_gain=0; 
+    D_gain=0;
+}
+
+void activate_PID_Controller_strike(double& P_gain, double& I_gain, double& D_gain)
+{
+    P_gain=0; // hier waardes P,I,D veranderen (waardes bovenaan doen (tijdelijk) niks meer
+    I_gain=0; 
+    D_gain=0;
+}
\ No newline at end of file