Diff: main.cpp

Revision:

39:104a038f7b92

Parent:

38:17a1622a27f7

Child:

40:bbe7922723df

--- a/main.cpp	Wed Oct 14 06:10:48 2015 +0000
+++ b/main.cpp	Wed Oct 14 11:08:52 2015 +0000
@@ -91,8 +91,8 @@
 void keep_in_range(double * in, double min, double max);
 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; 
@@ -100,7 +100,7 @@
     D_gain=0;
 }
 
-//// Activate PID Controller strike
+// 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
@@ -108,6 +108,11 @@
     D_gain=10;
 }
 
+// Change reference position (two functions)
+void Change_Turn_Position_Left (double& reference, double change_one_bottle);
+
+void Change_Turn_Position_Right (double& reference, double change_one_bottle);
+
                                                      ///////////////////////////////
                                                      //                           //
 ///////////////////////////////////////////////////////      [MAIN FUNCTION]      ////////////////////////////////////////////////////////////////////////////
@@ -118,6 +123,7 @@
     debug_led_blue=off;
     debug_led_green=off;
     
+    const double change_one_bottle=45;    
     double position_turn;                   // Set constant to store current position of the Turn motor
     double error;
     double pwm_to_motor_turn;
@@ -126,11 +132,24 @@
     double pwm_motor_turn_D;
     double reference_turn=0;                  // Set constant to store reference value of the Turn motor
     
+    // TEST: Does change PID gains work? => works correctly
     deactivate_PID_Controller_strike(P_gain_turn,I_gain_turn,D_gain_turn);
     pc.printf("P = %d I = %d D = %d \n\r");
     activate_PID_Controller_strike(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
+    pc.printf("initial reference_turn = %d \n\r", reference_turn);
+    Change_Turn_Position_Left (reference_turn, change_one_bottle);
+    pc.printf("After left turn reference_turn  = %d \n\r", reference_turn);
+    Change_Turn_Position_Right (reference_turn, change_one_bottle);
+    pc.printf("After right turn reference_turn  = %d \n\r", reference_turn);
+    Change_Turn_Position_Right (reference_turn, change_one_bottle);
+    Change_Turn_Position_Right (reference_turn, change_one_bottle);
+    Change_Turn_Position_Right (reference_turn, change_one_bottle);
+    Change_Turn_Position_Right (reference_turn, change_one_bottle);
+    pc.printf("After right turn reference_turn  = %d \n\r", reference_turn); // DOES THE LIMITER WORK? (should be -90 degrees)
+    
     //START OF CODE 
     pc.printf("Start of code \n\r");
     
@@ -306,3 +325,17 @@
 const float gain = 4.0;
 return (input-offset)*gain;
 }
+
+// Change reference
+void Change_Turn_Position_Left (double& reference, double change_one_bottle)
+    {
+        reference = reference + change_one_bottle;
+        keep_in_range(&reference, -90, 90); // reference position stays between -90 and 90 degrees (IF bottles at -90, -45, 0, 45, 90 degrees)
+    }
+    
+void Change_Turn_Position_Right (double& reference, double change_one_bottle)
+    {
+        reference = reference - change_one_bottle;
+        keep_in_range(&reference, -90, 90);
+    }
+