Diff: main.cpp

Branch:

michi

Revision:

66:b4e55e1eebfc

Parent:

65:1ee1f319a199

Child:

67:3debc9a3cca5

--- a/main.cpp	Mon Apr 25 14:26:29 2022 +0200
+++ b/main.cpp	Wed Apr 27 08:16:22 2022 +0200
@@ -69,7 +69,7 @@
 
 // definition important variables
 const float   pi = 2 * acos(0.0); // definiton of pi
-const float   max_speed_rps_wheel = 0.6f;  // 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_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)
 
@@ -94,6 +94,13 @@
     return deg_arm;
 }
 
+float calc_pos_end_lift()
+{
+    float end_deg;
+    end_deg = asin((dist_arm_ground-(dist_grappleratt_grappler_uk-gripper_area_height))/arm_length);
+    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.
@@ -108,8 +115,8 @@
 // RETURN: new Rotation value in rotations
 float turn_relative_deg(float deg_to_turn, float current_full_rotation)
 {
-    float current_deg = current_full_rotation * 360;
-    float new_turn_rotation = (current_deg - deg_to_turn) / 360.0;
+    float current_rotations = current_full_rotation;
+    float new_turn_rotation = current_rotations - deg_to_turn/360.0;
     return new_turn_rotation;
 }
 
@@ -120,7 +127,7 @@
 float turn_absolut_deg(float end_deg, float current_rotations)
 {
     int full_rotations = current_rotations;
-    int new_partial_rotation = current_rotations + start_deg_arm/360;
+    float new_partial_rotation = current_rotations - start_deg_arm/360;
     return new_partial_rotation;
 }
 
@@ -133,6 +140,7 @@
     {
         printf("Error in start_position: degree is out of bound for Start Position."); // error when desired reaching point is out of reach.
     }
+
     enable_motors = 1;
     positionController_M_Arm.setDesiredRotation(deg / 360.0, max_speed_rps_arm); // command to turn motor to desired deg.
     enable_motors = 0;
@@ -146,6 +154,7 @@
 
     float relativ_turns_rightmotor = turn_relative_deg(deg_to_turn, positionController_M_right.getRotation());
     float relativ_turns_leftmotor = turn_relative_deg(deg_to_turn, positionController_M_left.getRotation());
+
     enable_motors = 1;
     positionController_M_right.setDesiredRotation(relativ_turns_rightmotor, max_speed_rps_wheel);
     positionController_M_left.setDesiredRotation(relativ_turns_leftmotor, max_speed_rps_wheel);