TreppenRobo
Prototyp V2
Dependencies: PM2_Libary
Diff: main.cpp
- Revision:
- 105:440d61a4d5eb
- Parent:
- 102:c070d711e1ac
- Child:
- 106:9c80285b97aa
--- 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