Diff: main.cpp

Branch:

michi

Revision:

81:909670edc2a2

Parent:

80:4eae727a13b5

Child:

83:8cf96ccfac98

Child:

89:9c13036600ac

--- a/main.cpp	Wed Apr 27 16:57:11 2022 +0200
+++ b/main.cpp	Wed Apr 27 17:42:51 2022 +0200
@@ -67,40 +67,25 @@
 const int   drive_back_mm = -100;    // placeholder for testing drives amount backwards
 int         ToNextFunction = 0;      // current state of the system (which function is beeing executed)
 
-// definition important variables
+// 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-gripper_area_height))/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)
+
+// 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
 const float   max_speed_rps_arm = 0.3f; // define maximum speed that the position controller is changig the speed for the arm, has to be smaller or equal to kn * max_voltage
-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)
 
 // calculates the deg which the arm has to take to reach a certain height (the input height has to be the height of OK Gripper area)
 // PARAM: height_mm = height which OK Gripperarea has to reach.
 // RETURN: deg_arm = absolut Position in deg that the arm has to take.
 float calc_arm_deg_for_height(int height_mm)
 {
-    float deg_arm;
-    if ((height_mm - dist_arm_ground - (dist_grappleratt_grappler_uk - gripper_area_height)) > arm_length) //check if height is reachable
-    {
-        printf("Error in calc_arm_deg_for_height: desired height is bigger than Wall-E arm lenght."); // error message when desired height is not reachable. 
-    }
-    else 
-    {
-        float height_arm = height_mm - (dist_arm_ground - dist_grappleratt_grappler_uk + gripper_area_height); // calculates the height which only the arm has to cover (- attachement height (arm to robot) etc.)
-        deg_arm = asin(height_arm / arm_length) * 180.0/pi; // calculates the absolute degrees which the arm has to reach
-    }
+    float height_arm = height_mm - (dist_arm_ground - dist_grappleratt_grappler_uk + gripper_area_height); // 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;
 }
 
-//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 = 180 + (end_deg * 180 / pi);
-    return end_deg;
-}
-
 //calculates the deg which the wheels have to turn in order to cover specified distance in mm
 //RETURN: deg_wheel = degree which the motor has to turn in order to cover distance(mm)
 float wheel_dist_to_deg(int distance) // distance has to be in mm.
@@ -142,6 +127,8 @@
     return new_partial_rotation;
 }
 //***********************************************************************************************************************************************************
+// important calculatet constants for Wall-E
+const double deg_up_from_horizon_to_stair = calc_arm_deg_for_height(height_stairs);
 
 // import functions from file mapping
 extern double powerx(double base, double pow2);
@@ -164,8 +151,7 @@
 // bring arm in starting position. Height of stairs.
 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;
+    float deg = deg_up_from_horizon_to_stair + start_deg_arm;
 
     enable_motors = 1;
     positionController_M_Arm.setDesiredRotation(deg / 360.0, max_speed_rps_arm); // command to turn motor to desired deg.
@@ -190,8 +176,7 @@
 //turns the arm until the robot is on the next step
 void lift_up()
 {
-    float position_lift_end_deg = calc_pos_end_lift(); // calculates the degree which has to be reached in order to get on top of next step
-    float absolut_pos_arm = turn_absolut_deg(position_lift_end_deg, positionController_M_Arm.getRotation());
+    float absolut_pos_arm = turn_absolut_deg(end_pos_lift_deg, positionController_M_Arm.getRotation()-1);
     
     enable_motors = 1;
     positionController_M_Arm.setDesiredRotation(absolut_pos_arm, max_speed_rps_arm);