Diff: main.cpp

Branch:

michi

Revision:

68:e3fc5ed0bc0e

Parent:

67:3debc9a3cca5

Child:

71:e740ef7c7813

Child:

73:f415e1fa1ad9

Child:

75:3831b90a4ae0

--- a/main.cpp	Wed Apr 27 08:40:58 2022 +0200
+++ b/main.cpp	Wed Apr 27 08:48:35 2022 +0200
@@ -100,6 +100,7 @@
 {
     float end_deg;
     end_deg = asin((dist_arm_ground-(dist_grappleratt_grappler_uk-gripper_area_height))/arm_length) + start_deg_arm;
+    end_deg = end_deg * 180 / pi;
     return end_deg;
 }
 
@@ -134,13 +135,13 @@
 }
 
 // bring arm in starting position. Height of stairs.
-void start_position()
+void set_arm_stair_height()
 {
     double deg_up_from_horizon = calc_arm_deg_for_height(height_stairs); //deg which arm motor has to turn to in order to grab stair. starting from horizontal position
     float deg = deg_up_from_horizon + start_deg_arm;
     if ((0.0 > deg) || (deg > 360.0))
     {
-        printf("Error in start_position: degree is out of bound for Start Position."); // error when desired reaching point is out of reach.
+        printf("Error in set_arm_stair_height: degree is out of bound for Start Position."); // error when desired reaching point is out of reach.
     }
 
     enable_motors = 1;
@@ -199,7 +200,7 @@
         {
 
             case 1: 
-            start_position();
+            set_arm_stair_height();
             printf("Case 1: Position ARM (rot): %3.3f\n",positionController_M_Arm.getRotation());
             break;