Diff: main.cpp

Revision:

105:440d61a4d5eb

Parent:

102:c070d711e1ac

Child:

106:9c80285b97aa

diff -r c070d711e1ac -r 440d61a4d5eb main.cpp
--- a/main.cpp	Sat May 14 10:56:41 2022 +0200
+++ b/main.cpp	Sat May 14 12:11:04 2022 +0200
@@ -159,9 +159,11 @@
 // 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.
-   
+    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(rotations, max_speed_rps_arm); // command to turn motor to desired deg.
 }
 
 //Drives forward into the next step