Diff: main.cpp

Branch:

michi

Revision:

92:06c871d9a6ad

Parent:

91:e464d78fce0a

Child:

93:4661e91fdd50

--- a/main.cpp	Mon May 02 14:46:19 2022 +0200
+++ b/main.cpp	Mon May 02 15:33:11 2022 +0200
@@ -25,9 +25,16 @@
 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
 float   pwm_period_s = 0.00005f;    // define pwm period time in seconds and create FastPWM objects to command dc motors
@@ -140,14 +147,13 @@
 //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_down(float sensor)
 {
-    double d_valueMM = distance;
+    double d_valueMM = mapping(sensor * 1.0e3f * 3.3f);
     if(d_valueMM >= 4) return 0;
     else if(d_valueMM < 4)  return 1;
     else if(d_valueMM <= 0 || d_valueMM > 100 ) return 2;
     else return 2;
-
 }
 
 // bring arm in starting position. Height of stairs.
@@ -185,9 +191,18 @@
     enable_motors = 0;
 }
 
+//function to climbe one step up and get in to position to drive in to the next step
+//PARAM: None
+//RETURN: None
 void lift_sequenze()
 {
-    
+    lift_up();
+    if ((StepDetection_down(ir_analog_in_Lookdown_B) == 1) && (StepDetection_down(ir_analog_in_Lookdown_F) == 1))
+    {
+        thread_sleep_for(500);
+        drive_straight(-grip_area_depth - 2);
+    }
+    set_arm_stair_height();
 }
 //***********************************************************************************************************************************************************
 
@@ -210,8 +225,6 @@
 
     while (true)
     {
-        ir_distance_mV = 1.0e3f * ir_analog_in.read() * 3.3f;
-
         switch (ToNextFunction) 
         {
 
@@ -224,13 +237,10 @@
             break;
 
             case 3: 
-            lift_up();
+            lift_sequenze();
             break;
 
-            case 4:
-            set_arm_stair_height();
-
-            case 5: 
+            case 4: 
             drive_straight(drive_back_mm);
             ToNextFunction = 0;
             break;