Diff: main.cpp

Revision:

97:37550ebdee00

Parent:

89:9c13036600ac

Child:

100:1287abe79a0f

--- a/main.cpp	Mon May 02 16:03:31 2022 +0200
+++ b/main.cpp	Wed May 04 10:21:11 2022 +0200
@@ -10,8 +10,9 @@
 const float dist_arm_attach_distsensor    = 20; // distance between pivot point arm  on body to start distancesensor on top in horizontal (6)
 const float dist_distsensors              = 200; // distance between the two distancesensors on top of Wall-E (9)
 const float dist_arm_ground               = 51; // distance between pivotpoint arm and ground (5)
-const float gripper_area_height           = 16 ; // Height of Grappler cutout to grapple Stair (8)
-const float dist_grappleratt_grappler_uk  = 33; // distance between pivotpoint Grappler and bottom edge (?)
+const float dist_arm_attach_OK_griparea   = 10.5 ; // Height of Grappler cutout to grapple Stair (8) (maybe add 1mm so gripper is a bit over the plate)
+const float dist_grappleratt_grappler_uk  = 36.5; // distance between pivotpoint Grappler and bottom edge (?)
+const float grip_area_depth               =32.5;
 
 const float height_stairs                 = 100; // height to top of next stairstep in mm
 //***********************************************************************************************************************************************************
@@ -24,8 +25,15 @@
 void user_button_released_fcn();
 
 // Sharp GP2Y0A41SK0F, 4-40 cm IR Sensor
-float ir_distance_mV = 0.0f;    // define variable to store measurement from infrared distancesensor in mVolt
-AnalogIn ir_analog_in(PC_3);    // create AnalogIn object to read in infrared distance sensor, 0...3.3V are mapped to 0...1
+// define variable to store measurement from infrared distancesensor in mm
+float ir_distance_mm_L;
+float ir_distance_mm_R;  
+
+AnalogIn ir_analog_in_Distance_L(PC_2);  
+AnalogIn ir_analog_in_Distance_R(PC_3);
+AnalogIn ir_analog_in_Lookdown_B(PC_5);
+AnalogIn ir_analog_in_Lookdown_F(PB_1);
+// create AnalogIn object to read in infrared distance sensor, 0...3.3V are mapped to 0...1
 
 // 78:1, 100:1, ... Metal Gearmotor 20Dx44L mm 12V CB
 DigitalOut enable_motors(PB_15);    // create DigitalOut object to enable dc motors
@@ -63,13 +71,13 @@
 // calculations for basic movment and controll
 
 //placeholder variables for prototype testing
-const int   drive_straight_mm = 100;  // placeholder for testing drives amount forward
-const int   drive_back_mm = -100;    // placeholder for testing drives amount backwards
+const int   drive_straight_mm = 200;  // placeholder for testing drives amount forward
+const int   drive_back_mm = -200;    // placeholder for testing drives amount backwards
 int         ToNextFunction = 0;      // current state of the system (which function is beeing executed)
 
 // definition variables for calculations
 const float   pi = 2 * acos(0.0); // definiton of pi
-const float   end_pos_lift_deg = 180 + asin((dist_arm_ground-(dist_grappleratt_grappler_uk-gripper_area_height))/arm_length) * 180 / pi;  // calculates the degree which the arm has to have when lift_up has been executed.
+const float   end_pos_lift_deg = 180 + asin((dist_arm_ground-(dist_grappleratt_grappler_uk-dist_arm_attach_OK_griparea))/arm_length) * 180 / pi;  // calculates the degree which the arm has to have when lift_up has been executed.
 const 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)
 
 // definition of rotation speeds for motors 0 = none 1.0 = max.
@@ -81,7 +89,7 @@
 // RETURN: deg_arm = absolut Position in deg that the arm has to take.
 float calc_arm_deg_for_height(int height_mm)
 {
-    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_arm_attach_OK_griparea + dist_grappleratt_grappler_uk); // calculates the height which only the arm has to cover (- attachement height (arm to robot) etc.)
     float deg_arm = asin(height_arm / arm_length) * 180.0/pi; // calculates the absolute degrees which the arm has to reach
     return deg_arm;
 }
@@ -90,7 +98,7 @@
 //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);
+    float deg_wheel = distance/(wheel_diameter * pi) * 360 ;
     return deg_wheel;
 }
 
@@ -126,6 +134,15 @@
     float new_partial_rotation = full_rotations - start_deg_arm/360 + end_deg/360;
     return new_partial_rotation;
 }
+
+float calc_pos_end_lift()
+{
+    float end_deg;
+    end_deg = asin((dist_arm_ground - (dist_grappleratt_grappler_uk - dist_arm_attach_OK_griparea)) / arm_length) + start_deg_arm;
+    end_deg = end_deg * 180 / pi;
+    return end_deg;
+}
+
 //***********************************************************************************************************************************************************
 // important calculatet constants for Wall-E
 const double deg_up_from_horizon_to_stair = calc_arm_deg_for_height(height_stairs);
@@ -177,9 +194,10 @@
 void lift_up()
 {
     float absolut_pos_arm = turn_absolut_deg(end_pos_lift_deg, positionController_M_Arm.getRotation()-1);
+    float position_lift_end_deg = asin((-dist_arm_ground - (dist_grappleratt_grappler_uk - dist_arm_attach_OK_griparea)) / arm_length) - 90;
     
   
-    positionController_M_Arm.setDesiredRotation(absolut_pos_arm, max_speed_rps_arm);
+    positionController_M_Arm.setDesiredRotation(position_lift_end_deg, max_speed_rps_arm);
 
 }
 //***********************************************************************************************************************************************************