Diff: main.cpp

Branch:

michi

Revision:

77:508d8fda4aa0

Parent:

76:d1bbdcd54703

Child:

78:44291588a875

--- a/main.cpp	Wed Apr 27 10:44:50 2022 +0200
+++ b/main.cpp	Wed Apr 27 10:57:11 2022 +0200
@@ -132,13 +132,20 @@
 //returns 2 if the distance isn't in the expected range
 int StepDetection(double distance){
     double d_valueMM = distance;
-    if(d_valueMM >= 4) { 
-        return 0;}
-    if(d_valueMM < 4){ 
-         return 1;}
-    if(d_valueMM < 0 || d_valueMM > 100 ){
-         return 2;}
-    else{
+    if(d_valueMM >= 4) 
+    { 
+        return 0;
+    }
+    if(d_valueMM < 4)
+    { 
+         return 1;
+    }
+    if(d_valueMM < 0 || d_valueMM > 100 )
+    {
+         return 2;
+    }
+    else
+    {
         return 2;
     }
 
@@ -193,10 +200,10 @@
 void lift_up()
 {
     float position_lift_end_deg = asin((-dist_arm_ground - (dist_grappleratt_grappler_uk-gripper_area_height)) / arm_length) - 90; // calculates the degree which has to be reached in order to get on top of next step
-    float relativ_turns_arm = turn_absolut_deg(position_lift_end_deg, positionController_M_Arm.getRotation());
+    float absolut_pos_arm = turn_absolut_deg(position_lift_end_deg, positionController_M_Arm.getRotation());
     
     enable_motors = 1;
-    positionController_M_Arm.setDesiredRotation(0, max_speed_rps_arm);
+    positionController_M_Arm.setDesiredRotation(absolut_pos_arm, max_speed_rps_arm);
     enable_motors = 0;
 }
 //***********************************************************************************************************************************************************