Diff: main.cpp

Revision:

45:359df0594588

Parent:

44:5dd0a3d24662

Child:

46:9279c7a725bf

--- a/main.cpp	Wed Oct 14 15:06:18 2015 +0000
+++ b/main.cpp	Wed Oct 14 18:57:27 2015 +0000
@@ -143,19 +143,36 @@
     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_turn;
-    double pwm_to_motor_turn;
-    double pwm_motor_turn_P;
-    double pwm_motor_turn_I;
-    double pwm_motor_turn_D;
+    const double change_one_bottle=45;     
+    double position_turn;                   // TURN: Set variable to store current position of the motor
+    double error_turn;                      // TURN: Set variable to store the current error of the motor
+    
+    double pwm_motor_turn_P;                // TURN: variable that stores the P action part of the error
+    double pwm_motor_turn_I;                // TURN: variable that stores the I action part of the error
+    double pwm_motor_turn_D;                // TURN: variable that stores the D action part of the error
+    double pwm_to_motor_turn;               // TURN: variable that is constructed by summing the values of the P, I and D action 
+    
+    double P_gain_turn;                     // TURN: this gain gets multiplied with the error inside the P action of the PID controller
+    double I_gain_turn;                     // TURN: this gain gets multiplied with the error inside the I action of the PID controller
+    double D_gain_turn;                     // TURN: this gain gets multiplied with the error inside the D action of the PID controller
     
-    double P_gain_turn;
-    double I_gain_turn;
-    double D_gain_turn;
+    double reference_turn;                  // TURN: reference position of the motor (position the motor 'likes' to get to)
+
+    //double position_strike;                 // TURN: Set variable to store current position of the motor
+    //double error_strike;                    // TURN: Set variable to store the current error of the motor
+
+    //double pwm_motor_strike_P;              // STRIKE: variable that stores the P action part of the error
+    //double pwm_motor_strike_I;              // STRIKE: variable that stores the I action part of the error
+    //double pwm_motor_strike_D;              // STRIKE: variable that stores the D action part of the error
+    //double pwm_to_motor_strike;             // STRIKE: variable that is constructed by summing the values of the P, I and D action 
+    //double reference_strike;
     
-    double reference_turn;
+    //double P_gain_turn;                     // STRIKE: this gain gets multiplied with the error inside the P action of the PID controller
+    //double I_gain_turn;                     // STRIKE: this gain gets multiplied with the error inside the I action of the PID controller
+    //double D_gain_turn;                     // STRIKE: this gain gets multiplied with the error inside the D action of the PID controller
+     
+    //double reference_strike;                // STRIKE: reference position of the motor (position the motor 'likes' to get to)
+    
 //                                         ___________________________
 //                                       //                           \\
 //                                      ||       [START OF CODE]       ||
@@ -193,7 +210,7 @@
 //                        motordirection_strike = ccw;                       
 //                        pwm_motor_turn = 0.2f;}  
 //                 
-            if ((buttonH2.read() < 0.5) && (buttonH2.read() < 0.5)) // Save current TURN and STRIKE positions as starting position
+            if ((buttonL2.read() < 0.5) && (buttonL1.read() < 0.5)) // Save current TURN and STRIKE positions as starting position
                         {  
                         motor_turn.reset();                         // TURN: Starting Position
                         reference_turn=0;                           // TURN: Set reference position to zero
@@ -208,10 +225,16 @@
     debug_led_red=off;                                              // Calibration end => RED LED off
     
     
-    // Tickers 
-    looptimer.attach(setlooptimerflag,(float)1/Fs);            // calls the looptimer flag every 0.01s
+//                                         ___________________________
+//                                       //                           \\
+//                                      ||       [ATTACH TICKER]       ||
+//                                       \\___________________________//
+//                                        Attach Ticker to looptimerflag
+
+    looptimer.attach(setlooptimerflag,(float)1/Fs);            // calls the looptimer flag every sample
     pc.printf("Start infinite loop \n\r");
-    wait (5);                                                  // Wait before starting system
+    
+    wait (5);                                                  // Wait 5 seconds before starting system
     
     activate_PID_Controller_strike(P_gain_turn, I_gain_turn, D_gain_turn);
     
@@ -374,14 +397,28 @@
 // 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)
+        if(reference==90)
+            {
+                 reference=-90;   
+            }
+        else
+            {
+                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);
+        if(reference==-90)
+            {
+                reference=90;   
+            }
+        else
+            {
+                reference = reference - change_one_bottle;
+                keep_in_range(&reference, -90, 90);
+            }
     }
 
 // Deactivate or Activate PID_Controller
@@ -394,9 +431,9 @@
 
 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;
+    P_gain=0.01; // hier waardes P,I,D veranderen (waardes bovenaan doen (tijdelijk) niks meer
+    I_gain=0.5; 
+    D_gain=5;
 }
 
 void activate_PID_Controller_strike(double& P_gain, double& I_gain, double& D_gain)