Diff: main.cpp

Revision:

100:1287abe79a0f

Parent:

97:37550ebdee00

Child:

101:ee375f42cef4

--- a/main.cpp	Wed May 04 10:21:11 2022 +0200
+++ b/main.cpp	Sat May 14 10:49:25 2022 +0200
@@ -77,8 +77,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   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
@@ -89,7 +89,7 @@
 // RETURN: deg_arm = absolut Position in deg that the arm has to take.
 float calc_arm_deg_for_height(int height_mm)
 {
-    float height_arm = height_mm - (dist_arm_ground - dist_arm_attach_OK_griparea + dist_grappleratt_grappler_uk); // calculates the height which only the arm has to cover (- attachement height (arm to robot) etc.)
+    float height_arm = height_mm - (dist_arm_ground - (dist_arm_attach_OK_griparea - dist_grappleratt_grappler_uk)); // calculates the height which only the arm has to cover (- attachement height (arm to robot) etc.)
     float deg_arm = asin(height_arm / arm_length) * 180.0/pi; // calculates the absolute degrees which the arm has to reach
     return deg_arm;
 }
@@ -168,7 +168,7 @@
 // bring arm in starting position. Height of stairs.
 void 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;
 
     
     positionController_M_Arm.setDesiredRotation(deg / 360.0, max_speed_rps_arm); // command to turn motor to desired deg.
@@ -194,10 +194,7 @@
 void lift_up()
 {
     float absolut_pos_arm = turn_absolut_deg(end_pos_lift_deg, positionController_M_Arm.getRotation()-1);
-    float position_lift_end_deg = asin((-dist_arm_ground - (dist_grappleratt_grappler_uk - dist_arm_attach_OK_griparea)) / arm_length) - 90;
-    
-  
-    positionController_M_Arm.setDesiredRotation(position_lift_end_deg, max_speed_rps_arm);
+    positionController_M_Arm.setDesiredRotation(absolut_pos_arm, max_speed_rps_arm);
 
 }
 //***********************************************************************************************************************************************************