Diff: main.cpp

Branch:

michi

Revision:

64:72b9efe62ece

Parent:

63:43c14a63b337

Child:

65:1ee1f319a199

--- a/main.cpp	Fri Apr 22 14:15:25 2022 +0200
+++ b/main.cpp	Mon Apr 25 14:04:45 2022 +0200
@@ -1,6 +1,5 @@
 #include "mbed.h"
 #include "PM2_Libary.h"
-#include <cmath> //<-- is <cmath> needed? check later.
 #include <cstdint>
 #include <cstdio>
 #include "math.h"
@@ -126,7 +125,7 @@
 }
 
 // bring arm in starting position. Height of stairs.
-int start_position()
+void start_position()
 {
     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;
@@ -134,31 +133,34 @@
     {
         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.
     current_deg_arm = positionController_M_Arm.getRotation() * 360.0; //write new position to variable 
-    return NULL;
+    enable_motors = 0;
 }
 
 //Drives forward into the next step 
 //Prameter:distance in milimeter
-int drive_straight(float distance)
+void drive_straight(float distance)
 {
     float deg_to_turn = wheel_dist_to_deg(distance);
 
     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);
-    return NULL;   
+    positionController_M_left.setDesiredRotation(relativ_turns_leftmotor, max_speed_rps_wheel); 
+    enable_motors = 0;
 }
 
 //only turns the arm until the robot is on the next step
-int lift_up()
+void lift_up()
 {
     float position_lift_end_deg = asin((-dist_arm_ground - (dist_grappleratt_grappler_uk-gripper_area_height)) / arm_length) - 90; // calculates the degree which has to be reached in order to get on top of next step
     
+    enable_motors = 1;
     positionController_M_Arm.setDesiredRotation(0, max_speed_rps_arm);
-    return NULL;
+    enable_motors = 0;
 }
 //***********************************************************************************************************************************************************
 
@@ -182,7 +184,6 @@
 
     while (true)
     {
-        enable_motors = 1; <-- (fix activate just when needed.)
         ir_distance_mV = 1.0e3f * ir_analog_in.read() * 3.3f;
         
         switch (ToNextFunction)