Diff: main.cpp

Branch:

michi

Revision:

103:edb8912708f3

Parent:

99:799e90067935

Child:

104:766002f2e46f

--- a/main.cpp	Sat May 14 10:18:54 2022 +0200
+++ b/main.cpp	Sat May 14 11:31:49 2022 +0200
@@ -79,8 +79,8 @@
 
 // definition variables for calculations
 const float   pi = 2 * acos(0.0); // definiton of pi
-const float   end_pos_lift_deg = 180 + asin((dist_arm_ground-(dist_grappleratt_grappler_uk - dist_arm_attach_OK_griparea))/arm_length) * 180 / pi;  // calculates the degree which the arm has to have when lift_up has been executed.
 const float   start_deg_arm = -asin((dist_arm_ground - dist_grappleratt_grappler_uk) / arm_length) * 180.0/pi ; //calculates the starting degree of the arm (gripper has to touch ground in frotn of Wall-E)
+const float   end_pos_lift_deg = 180 + (asin((dist_arm_ground-(dist_grappleratt_grappler_uk - dist_arm_attach_OK_griparea))/arm_length) * 180 / pi) - start_deg_arm;  // calculates the degree which the arm has to have when lift_up has been executed.
 
 // definition of rotation speeds for motors 0 = none 1.0 = max.
 const float   max_speed_rps_wheel = 0.5f;  // define maximum speed that the position controller is changig the speed for the wheels, has to be smaller or equal to kn * max_voltage
@@ -139,16 +139,6 @@
     return new_partial_rotation;
 }
 
-//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 - dist_arm_attach_OK_griparea))/arm_length) + start_deg_arm;
-    end_deg = end_deg * 180 / pi;
-    return end_deg;
-}
-
 //***********************************************************************************************************************************************************
 // important calculatet constant for Wall-E
 const double deg_up_from_horizon_to_stair = calc_arm_deg_for_height(height_stairs); //deg which arm motor has to turn to in order to grab stair. starting from horizontal position
@@ -186,7 +176,7 @@
 // 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 deg = deg_up_from_horizon_to_stair - start_deg_arm;
     float diff;
 
     positionController_M_Arm.setDesiredRotation(deg / 360.0, max_speed_rps_arm); // command to turn motor to desired deg.