Diff: main.cpp

Branch:

michi

Revision:

60:b2e9958f2298

Parent:

59:f6c3e42f16c7

Child:

61:2ff627973f2c

--- a/main.cpp	Wed Apr 20 11:02:17 2022 +0200
+++ b/main.cpp	Wed Apr 20 11:30:16 2022 +0200
@@ -97,6 +97,7 @@
 }
 
 //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.
 {
     float deg_wheel = distance * 360 /(wheel_diameter * pi);
@@ -116,12 +117,13 @@
 
 // sets the Motor to a specified degree in one rotation 
 // PARAM: end_deg = new position of the arm in degree 0 <= value >=360
-// PARAM: current_full_rotation = the current rotation of the Motor (Motor.getRotation())
-// RETURN: new Rotation value in rotations
-float turn_to_abs_in_deg(float end_deg, float current_full_rotation)
+// PARAM: current_rotations = the current rotation of the Motor (Motor.getRotation())
+// RETURN: new_partial_rotation = new deg value in rotations
+float turn_to_abs_in_deg(float end_deg, float current_rotations)
 {
-    
-    return NULL;
+    int full_rotations = current_rotations;
+    int new_partial_rotation = current_rotations + start_deg_arm/360;
+    return new_partial_rotation;
 }
 
 // bring arm in starting position. Height of stairs.
@@ -143,10 +145,11 @@
 int drive_straight(float distance)
 {
     float deg_to_turn = wheel_dist_to_deg(distance);
-    float relativ_deg_to_turn_right = turn_relative_deg(deg_to_turn, positionController_M_right.getRotation());
-    float relativ_deg_to_turn_left = turn_relative_deg(deg_to_turn, positionController_M_left.getRotation());
-    positionController_M_right.setDesiredRotation(relativ_deg_to_turn_right / 360.0, max_speed_rps_wheel);
-    positionController_M_left.setDesiredRotation(relativ_deg_to_turn_left / 360.0, max_speed_rps_wheel);
+
+    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());
+    positionController_M_right.setDesiredRotation(relativ_turns_rightmotor, max_speed_rps_wheel);
+    positionController_M_left.setDesiredRotation(relativ_turns_leftmotor, max_speed_rps_wheel);
     return NULL;   
 }