Diff: main.cpp

Branch:

michi

Revision:

65:1ee1f319a199

Parent:

64:72b9efe62ece

Child:

66:b4e55e1eebfc

--- a/main.cpp	Mon Apr 25 14:04:45 2022 +0200
+++ b/main.cpp	Mon Apr 25 14:26:29 2022 +0200
@@ -63,23 +63,23 @@
 // logic functions for basic movement
 
 //placeholder variables for prototype testing
-const int   drive_stright_mm = 100; // placeholder for testing drives amount forward
-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)
+const int   drive_stright_mm = 100;  // placeholder for testing drives amount forward
+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
 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_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)
-float         current_deg_arm = start_deg_arm; // saves the current degree the arm has.
+
 
 // import functions from file mapping
 extern double powerx(double base, double pow2);
 extern double mapping (float adc_value_mV);
 
 // 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)
-double calc_arm_deg_for_height(int height_mm)
+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
@@ -135,7 +135,6 @@
     }
     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 
     enable_motors = 0;
 }
 
@@ -165,10 +164,9 @@
 //***********************************************************************************************************************************************************
 
 //Function which checks if sensors and motors have been wired correctly and the expectet results will happen. otherwise Wall-E will show with armmovement.
-int check_start()
+void check_start()
 {
 
-    return NULL;
 }
 
 // while loop gets executed every main_task_period_ms milliseconds
@@ -209,14 +207,9 @@
             drive_straight(drive_back_mm);
             printf("Case 4: Position Right(rot): %3.3f;    Position Left (rot): %3.3f\n",
             positionController_M_right.getRotation(),positionController_M_left.getRotation());
+            ToNextFunction = 0;
             break;
 
-            case 5: 
-            lift_up();
-            printf("Case 5: Position ARM (rot): %3.3f\n",positionController_M_Arm.getRotation());
-            ToNextFunction = 0;
-            break;  
-
             default:  ;
         } 
     }