Diff: main.cpp

Revision:

41:8a63b01edd7e

Parent:

40:7e6b7aec3947

Child:

42:883d16a5d59e

--- a/main.cpp	Tue May 17 12:35:58 2022 +0000
+++ b/main.cpp	Tue May 17 15:42:29 2022 +0200
@@ -14,10 +14,12 @@
 void user_button_pressed_fcn();     // custom functions which gets executed when user button gets pressed and released, definition below
 void user_button_released_fcn();
 
+float ir_distance_mV2cm(float ir_distance_cm);
+
 int main()
 {
     // while loop gets executed every main_task_period_ms milliseconds
-    const int main_task_period_ms = 100;   // define main task period time in ms e.g. 50 ms -> main task runns 20 times per second
+    const int main_task_period_ms = 10;   // define main task period time in ms e.g. 50 ms -> main task runns 20 times per second
     Timer main_task_timer;                // create Timer object which we use to run the main task every main task period time in ms
 
     // a coutner
@@ -28,6 +30,7 @@
 
     // Sharp GP2Y0A41SK0F, 4-40 cm IR Sensor
     float ir_distance_mV = 0.0f;    // define variable to store measurement
+    float ir_distance_cm = 0.0f;    // compensated sensor value in cm
     AnalogIn ir_analog_in(PC_2);    // create AnalogIn object to read in infrared distance sensor, 0...3.3V are mapped to 0...1
 
     // create SensorBar object for sparkfun line follower array, only use this if it is connected (blocking your code if not)
@@ -77,6 +80,7 @@
         
         // read analog input
         ir_distance_mV = 1.0e3f * ir_analog_in.read() * 3.3f;
+        ir_distance_cm = ir_distance_mV2cm(ir_distance_mV);
         
         // read SensorBar, only use this if it is connected (blocking your code if not)
         //if (sensor_bar.isAnyLedActive()) {
@@ -95,18 +99,21 @@
             
         }
 
-        // user_led is switching its state every 100 runs
-        if ( (main_task_cntr%(1000 / main_task_cntr) == 0) && (main_task_cntr!=0) ) {
+        // user_led is switching its state every second
+        if ( (main_task_cntr%(1000 / main_task_period_ms) == 0) && (main_task_cntr!=0) ) {
             user_led = !user_led;
         }
         main_task_cntr++;
         
         // do only output via serial what's really necessary (this makes your code slow)
-        printf("IR sensor (mV): %3.3f, SensorBar angle (rad): %3.3f, Speed M1 (rps) %3.3f, Position M2 (rot): %3.3f\r\n",
+        /*
+        printf("IR sensor (mV): %3.3f, IR sensor (cm): %3.3f, SensorBar angle (rad): %3.3f, Speed M1 (rps) %3.3f, Position M2 (rot): %3.3f\r\n",
                ir_distance_mV,
+               ir_distance_cm,
                sensor_bar_avgAngleRad,
                speedController_M1.getSpeedRPS(),
                positionController_M2.getRotation());
+        */
 
         // 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();
@@ -128,4 +135,13 @@
     if (user_button_elapsed_time_ms > 200) {
         do_execute_main_task = !do_execute_main_task;
     }
+}
+
+float ir_distance_mV2cm(float ir_distance_cm)
+{
+    // defining these variables static makes them persistent within the function
+    static float a =    -4.685f; //  (-6.581, -2.79)
+    static float c = 3.017e+04f; //  (2.853e+04, 3.181e+04)
+
+    return c/(ir_distance_cm + 1) + a;
 }
\ No newline at end of file