TreppenRobo / Mbed OS PrototypV2

Prototyp V2

Dependencies:   PM2_Libary

Diff: main.cpp

Branch:
michi
Revision:
67:3debc9a3cca5
Parent:
66:b4e55e1eebfc
Child:
68:e3fc5ed0bc0e
--- a/main.cpp	Wed Apr 27 08:16:22 2022 +0200
+++ b/main.cpp	Wed Apr 27 08:40:58 2022 +0200
@@ -94,10 +94,12 @@
     return deg_arm;
 }
 
+//calculates position of arm when lift up has ended.
+//RETURN: end_deg = degree which the motor has to turn in order to reach end lift position.
 float calc_pos_end_lift()
 {
     float end_deg;
-    end_deg = asin((dist_arm_ground-(dist_grappleratt_grappler_uk-gripper_area_height))/arm_length);
+    end_deg = asin((dist_arm_ground-(dist_grappleratt_grappler_uk-gripper_area_height))/arm_length) + start_deg_arm;
     return end_deg;
 }