Diff: main.cpp

Branch:

michi

Revision:

104:766002f2e46f

Parent:

103:edb8912708f3

--- a/main.cpp	Sat May 14 11:31:49 2022 +0200
+++ b/main.cpp	Sat May 14 12:08:53 2022 +0200
@@ -92,7 +92,7 @@
 // RETURN: deg_arm = absolut Position in deg that the arm has to take.
 float calc_arm_deg_for_height(int height_mm)
 {
-    float height_arm = height_mm - (dist_arm_ground - dist_arm_attach_OK_griparea + dist_grappleratt_grappler_uk); // calculates the height which only the arm has to cover (- attachement height (arm to robot) etc.)
+    float height_arm = height_mm - (dist_arm_ground - dist_arm_attach_OK_griparea); // calculates the height which only the arm has to cover (- attachement height (arm to robot) etc.)
     float deg_arm = asin(height_arm / arm_length) * 180.0/pi; // calculates the absolute degrees which the arm has to reach
     return deg_arm;
 }
@@ -176,11 +176,12 @@
 // bring arm in starting position. Height of stairs.
 int set_arm_stair_height()
 {
-    float deg = deg_up_from_horizon_to_stair - start_deg_arm;
+    float initial_full_rotation = floor(positionController_M_Arm.getRotation());
+    float rotations = (deg_up_from_horizon_to_stair - start_deg_arm) / 360 + initial_full_rotation;
     float diff;
 
-    positionController_M_Arm.setDesiredRotation(deg / 360.0, max_speed_rps_arm); // command to turn motor to desired deg.
-    diff = deg-(positionController_M_Arm.getRotation() * 360.0);
+    positionController_M_Arm.setDesiredRotation(rotations, max_speed_rps_arm); // command to turn motor to desired deg.
+    diff = rotations-positionController_M_Arm.getRotation();
     if (diff <= 0.3){
         drive_straight_iteration = 0;
         return 1;