Diff: main.cpp

Revision:

101:ee375f42cef4

Parent:

100:1287abe79a0f

Child:

102:c070d711e1ac

--- a/main.cpp	Sat May 14 10:49:25 2022 +0200
+++ b/main.cpp	Sat May 14 10:54:05 2022 +0200
@@ -12,7 +12,7 @@
 const float dist_arm_ground               = 51; // distance between pivotpoint arm and ground (5)
 const float dist_arm_attach_OK_griparea   = 10.5 ; // Height of Grappler cutout to grapple Stair (8) (maybe add 1mm so gripper is a bit over the plate)
 const float dist_grappleratt_grappler_uk  = 36.5; // distance between pivotpoint Grappler and bottom edge (?)
-const float grip_area_depth               =32.5;
+const float grip_area_depth               = 32.5;
 
 const float height_stairs                 = 100; // height to top of next stairstep in mm
 //***********************************************************************************************************************************************************
@@ -162,15 +162,12 @@
     else if(d_valueMM < 4)  return 1;
     else if(d_valueMM <= 0 || d_valueMM > 100 ) return 2;
     else return 2;
-
 }
 
 // bring arm in starting position. Height of stairs.
 void set_arm_stair_height()
 {
     float deg = deg_up_from_horizon_to_stair - start_deg_arm;
-
-    
     positionController_M_Arm.setDesiredRotation(deg / 360.0, max_speed_rps_arm); // command to turn motor to desired deg.
    
 }
@@ -183,7 +180,6 @@
 
     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());
-
   
     positionController_M_right.setDesiredRotation(relativ_turns_rightmotor, max_speed_rps_wheel);
     positionController_M_left.setDesiredRotation(relativ_turns_leftmotor, max_speed_rps_wheel); 
@@ -195,7 +191,6 @@
 {
     float absolut_pos_arm = turn_absolut_deg(end_pos_lift_deg, positionController_M_Arm.getRotation()-1);
     positionController_M_Arm.setDesiredRotation(absolut_pos_arm, max_speed_rps_arm);
-
 }
 //***********************************************************************************************************************************************************
 
@@ -218,9 +213,9 @@
 
     while (true)
     {
-      if (ToNextFunction>=1){
-
-        enable_motors = 1;
+        if (ToNextFunction>=1)
+        {
+            enable_motors = 1;
         }
 
         switch (ToNextFunction)