Diff: main.cpp

Revision:

48:31ffd88e7f99

Parent:

47:6693bffcdfd0

Child:

49:f80f5d96716e

--- a/main.cpp	Wed May 18 09:50:32 2022 +0000
+++ b/main.cpp	Wed May 18 22:22:31 2022 +0000
@@ -32,6 +32,9 @@
 Timer user_button_timer;            // create Timer object which we use to check if user button was pressed for a certain time (robust against signal bouncing)
 InterruptIn user_button(PC_13);     // create InterruptIn interface object to evaluate user button falling and rising edge (no blocking code in ISR)
 
+EventFlags event_flags;
+const uint32_t main_task_flag = 1;
+
 /* ------------------------------------------------------------------------------------------- *
  * -- Constants and Parameters
  * ------------------------------------------------------------------------------------------- */
@@ -43,18 +46,21 @@
 const float VELOCITY_THRESHOLD = 0.05;        // velocity threshold before switching off, in [m/s] and [rad/s]
 
 // discrete states of this state machine
-const int ROBOT_OFF = 0;      
-const int MOVE_FORWARD = 1;
-const int TURN_LEFT = 2;
-const int TURN_RIGHT = 3;
-const int SLOWING_DOWN = 4;
+const int INIT = 0;
+const int ROBOT_OFF = 1;      
+const int MOVE_FORWARD = 2;
+const int TURN_LEFT = 3;
+const int TURN_RIGHT = 4;
+const int SLOWING_DOWN = 5;
 
 /* ------------------------------------------------------------------------------------------- *
- * -- Static Function Declarations
+ * -- Function Declarations
  * ------------------------------------------------------------------------------------------- */
 static void user_button_pressed_fcn();     // custom functions which gets executed when user button gets pressed and released, definition below
 static void user_button_released_fcn();
 
+void main_task_trigger();           // triggers the main task each period
+
 /* ------------------------------------------------------------------------------------------- *
  * -- Main
  * ------------------------------------------------------------------------------------------- */
@@ -163,25 +169,26 @@
      * ------------------------------------------------------------------------------------------- */
     
     // while loop gets executed every main_task_period_ms milliseconds
-    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
+    const float main_task_period = 10e-3;               // define main task period time in ms e.g. 50 ms -> main task runns 20 times per second
+
     
-    // start timer
-    main_task_timer.start();
+    Ticker ticker;                                      // calls a fuction with a precise period
+    ticker.attach(main_task_trigger, std::chrono::microseconds(static_cast<int>(main_task_period*1e6))); // call the main task trigger every period
 
     // set initial state machine state, enalbe leds, disable motors
-    int state = ROBOT_OFF;
-
-    /* ------------------------------------------------------------------------------------------- *
-     * -- State Machine
-     * ------------------------------------------------------------------------------------------- */
-
-    enable_leds = 1;
-    enable_motors = 0;
+    int state = INIT;
     
     while (true) { // this loop will run forever
+
+        /* ------------------------------------------------------------------------------------------- *
+         * -- Wait for the next Cycle
+         * ------------------------------------------------------------------------------------------- */
+        event_flags.wait_any(main_task_flag);
+
         
-        main_task_timer.reset();
+        /* ------------------------------------------------------------------------------------------- *
+         * -- Read Sensors
+         * ------------------------------------------------------------------------------------------- */
 
         // set leds according to DISTANCE_THRESHOLD
         for (uint8_t i = 0; i < leds.size(); i++) {
@@ -195,8 +202,21 @@
         wheel_speed_actual << speedControllers[0]->getSpeedRPS(), speedControllers[1]->getSpeedRPS();
         robot_coord_actual =  Cwheel2robot * wheel_speed_actual;
 
+
+        /* ------------------------------------------------------------------------------------------- *
+         * -- State Machine
+         * ------------------------------------------------------------------------------------------- */
+
         // state machine
         switch (state) {
+
+            case INIT:
+
+                enable_leds = 1;
+                enable_motors = 0;
+
+                state = ROBOT_OFF;  // default transition
+                break;
             
             case ROBOT_OFF:
                 
@@ -271,8 +291,8 @@
         wheel_speed = Cwheel2robot.inverse() * robot_coord;
 
         // smooth desired wheel_speeds
-        trajectoryPlaners[0]->incrementToVelocity(wheel_speed(0) / (2.0f * M_PI), static_cast<float>(main_task_period_ms) * 1.0e-3f);
-        trajectoryPlaners[1]->incrementToVelocity(wheel_speed(1) / (2.0f * M_PI), static_cast<float>(main_task_period_ms) * 1.0e-3f);
+        trajectoryPlaners[0]->incrementToVelocity(wheel_speed(0) / (2.0f * M_PI), main_task_period);
+        trajectoryPlaners[1]->incrementToVelocity(wheel_speed(1) / (2.0f * M_PI), main_task_period);
         wheel_speed_smooth << trajectoryPlaners[0]->getVelocity(), trajectoryPlaners[1]->getVelocity();
 
         // command speedController objects
@@ -289,14 +309,6 @@
         
         // do only output via serial what's really necessary (this makes your code slow)
         //printf("%f, %f\r\n", wheel_speed_actual(0), wheel_speed_actual(1));
-
-        /* ------------------------------------------------------------------------------------------- *
-         * -- Wait for the next Cycle
-         * ------------------------------------------------------------------------------------------- */
-        
-        // 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);
     }
 }
 
@@ -314,4 +326,9 @@
     if (user_button_elapsed_time_ms > 200) {
         do_execute_main_task = !do_execute_main_task;
     }
+}
+
+void main_task_trigger()
+{
+    event_flags.set(main_task_flag);
 }
\ No newline at end of file