Diff: main.cpp

Revision:

40:bbe7922723df

Parent:

39:104a038f7b92

Child:

41:424264a4c39c

--- a/main.cpp	Wed Oct 14 11:08:52 2015 +0000
+++ b/main.cpp	Wed Oct 14 11:19:58 2015 +0000
@@ -132,6 +132,9 @@
     double pwm_motor_turn_D;
     double reference_turn=0;                  // Set constant to store reference value of the Turn motor
     
+    // CALIBRATE (STARTING POSITION)
+    
+    
     // 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");
@@ -284,7 +287,7 @@
 //                                       \\___________________________//
 //                                          // Put pwm to the motor \\
 
-        pwm_motor_turn=(abs(pwm_to_motor_turn));
+            pwm_motor_turn=(abs(pwm_to_motor_turn));
         
 //                                         ___________________________
 //                                       //                           \\
@@ -292,10 +295,21 @@
 //                                       \\___________________________//
 //                                      // Check signals inside HIDSCOPE \\
         
-            scope.set(0,error);             // HIDSCOPE channel 0 : Current Reference
+            scope.set(0,error);             // 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
+            
+//                                         ___________________________
+//                                       //                           \\
+//                                      ||        [Deactivate?]        ||
+//                                       \\___________________________//
+//                // 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
+            {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);}
     }
 }
 }