Diff: main.cpp

Revision:

89:9c13036600ac

Parent:

81:909670edc2a2

Child:

97:37550ebdee00

--- a/main.cpp	Wed Apr 27 17:50:23 2022 +0200
+++ b/main.cpp	Mon May 02 16:03:31 2022 +0200
@@ -153,9 +153,9 @@
 {
     float deg = deg_up_from_horizon_to_stair + start_deg_arm;
 
-    enable_motors = 1;
+    
     positionController_M_Arm.setDesiredRotation(deg / 360.0, max_speed_rps_arm); // command to turn motor to desired deg.
-    enable_motors = 0;
+   
 }
 
 //Drives forward into the next step 
@@ -167,10 +167,10 @@
     float relativ_turns_rightmotor = turn_relative_deg(deg_to_turn, positionController_M_right.getRotation());
     float relativ_turns_leftmotor = turn_relative_deg(deg_to_turn, positionController_M_left.getRotation());
 
-    enable_motors = 1;
+  
     positionController_M_right.setDesiredRotation(relativ_turns_rightmotor, max_speed_rps_wheel);
     positionController_M_left.setDesiredRotation(relativ_turns_leftmotor, max_speed_rps_wheel); 
-    enable_motors = 0;
+
 }
 
 //turns the arm until the robot is on the next step
@@ -178,9 +178,9 @@
 {
     float absolut_pos_arm = turn_absolut_deg(end_pos_lift_deg, positionController_M_Arm.getRotation()-1);
     
-    enable_motors = 1;
+  
     positionController_M_Arm.setDesiredRotation(absolut_pos_arm, max_speed_rps_arm);
-    enable_motors = 0;
+
 }
 //***********************************************************************************************************************************************************
 
@@ -203,8 +203,10 @@
 
     while (true)
     {
-        ir_distance_mV = 1.0e3f * ir_analog_in.read() * 3.3f;
-        printf("detection: %d\n",StepDetection(mapping(ir_distance_mV)));
+      if (ToNextFunction>=1){
+
+        enable_motors = 1;
+        }
 
         switch (ToNextFunction) 
         {