Diff: main.cpp

Branch:

michi

Revision:

80:4eae727a13b5

Parent:

79:368cbf09cf6a

Child:

81:909670edc2a2

--- a/main.cpp	Wed Apr 27 11:10:52 2022 +0200
+++ b/main.cpp	Wed Apr 27 16:57:11 2022 +0200
@@ -60,10 +60,10 @@
 // PositionController positionController_M3(counts_per_turn, kn, max_voltage, pwm_M3, encoder_M3); // default 78.125:1 gear with default contoller parameters
 //PositionController positionController_M3(counts_per_turn * k_gear, kn / k_gear, kp * k_gear, max_voltage, pwm_M3, encoder_M3); // parameters adjusted to 100:1 gear, we need a different speed controller gain here
 //***********************************************************************************************************************************************************
-// logic functions for basic movement
+// calculations for basic movment and controll
 
 //placeholder variables for prototype testing
-const int   drive_stright_mm = 100;  // placeholder for testing drives amount forward
+const int   drive_straight_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)
 
@@ -73,11 +73,6 @@
 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)
 
-
-// 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)
 // PARAM: height_mm = height which OK Gripperarea has to reach.
 // RETURN: deg_arm = absolut Position in deg that the arm has to take.
@@ -90,7 +85,7 @@
     }
     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.)
+        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
     }
     return deg_arm;
@@ -101,8 +96,8 @@
 float calc_pos_end_lift()
 {
     float end_deg;
-    end_deg = asin((dist_arm_ground-(dist_grappleratt_grappler_uk-gripper_area_height))/arm_length) + start_deg_arm;
-    end_deg = end_deg * 180 / pi;
+    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;
 }
 
@@ -116,21 +111,48 @@
 
 // increments the Motor for defined degree from the current one
 // PARAM: deg_to_turn = degree to turn the Motor
-// PARAM: current_full_rotation = the current rotation of the Motor (Motor.getRotation())
-// RETURN: new Rotation value in rotations
-float turn_relative_deg(float deg_to_turn, float current_full_rotation)
+// PARAM: current_rotation = the current rotation of the Motor (Motor.getRotation())
+// RETURN: new_turn_rotation = new Rotation value in rotations
+float turn_relative_deg(float deg_to_turn, float current_rotation)
 {
-    float current_rotations = current_full_rotation;
-    float new_turn_rotation = current_rotations - deg_to_turn/360.0;
+    float new_turn_rotation = current_rotation + deg_to_turn/360.0;
     return new_turn_rotation;
 }
 
+// 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_rotation = the current rotation of the Motor (Motor.getRotation())
+// RETURN: new_partial_rotation = new deg value in rotations
+float turn_absolut_deg(float end_deg, float current_rotations)
+{
+    int full_rotations;
+	if(current_rotations > 0)
+	{
+		full_rotations = round(current_rotations - 0.5);	
+	}
+	else if(current_rotations < 0)
+	{
+		full_rotations = round(current_rotations + 0.5);	
+	} 
+	else
+	{
+		full_rotations = 0;
+	}
+    float new_partial_rotation = full_rotations - start_deg_arm/360 + end_deg/360;
+    return new_partial_rotation;
+}
+//***********************************************************************************************************************************************************
+
+// import functions from file mapping
+extern double powerx(double base, double pow2);
+extern double mapping (float adc_value_mV);
 
 //simple check if there is an object in proximity
 //returns 0 if there is NO object present
 //returns 1 if there is an object present
 //returns 2 if the distance isn't in the expected range
-uint8_t StepDetection(double distance){
+uint8_t StepDetection(double distance)
+{
     double d_valueMM = distance;
     if(d_valueMM >= 4) return 0;
     else if(d_valueMM < 4)  return 1;
@@ -138,32 +160,12 @@
     else return 2;
 
 }
-/*Example implementation
- * distanceCM = mapping(analogIn.read()*3.3e3f);
- * stepDetected = StepDetection(distanceCM);
- */
-
-
-// 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_rotations = the current rotation of the Motor (Motor.getRotation())
-// RETURN: new_partial_rotation = new deg value in rotations
-float turn_absolut_deg(float end_deg, float current_rotations)
-{
-    int full_rotations = current_rotations;
-    float new_partial_rotation = current_rotations - start_deg_arm/360;
-    return new_partial_rotation;
-}
 
 // 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;
-    if ((0.0 > deg) || (deg > 360.0))
-    {
-        printf("Error in set_arm_stair_height: 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.
@@ -197,7 +199,7 @@
 }
 //***********************************************************************************************************************************************************
 
-//Function which checks if sensors and motors have been wired correctly and the expectet results will happen. otherwise Wall-E will show with armmovement.
+//Function which checks if sensors and motors have been wired correctly and the expectet results will happen. otherwise Wall-E will ???.
 void check_start()
 {
 
@@ -228,7 +230,7 @@
             break;
 
             case 2: 
-            drive_straight(drive_stright_mm);
+            drive_straight(drive_straight_mm);
             printf("Case 2: Position Right(rot): %3.3f;    Position Left (rot): %3.3f\n",
             positionController_M_right.getRotation(),positionController_M_left.getRotation());
             break;
@@ -248,10 +250,10 @@
             default:  ;
         } 
     }
-       // read timer and make the main thread sleep for the remaining time span (non blocking)
-        int main_task_elapsed_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(main_task_timer.elapsed_time()).count();
-        thread_sleep_for(main_task_period_ms - main_task_elapsed_time_ms);
-        return 0;
+    // read timer and make the main thread sleep for the remaining time span (non blocking)
+    int main_task_elapsed_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(main_task_timer.elapsed_time()).count();
+    thread_sleep_for(main_task_period_ms - main_task_elapsed_time_ms);
+    return 0;
 }