Diff: main.cpp

Revision:

47:6693bffcdfd0

Parent:

46:41c9367da539

Child:

48:31ffd88e7f99

--- a/main.cpp	Mon May 16 11:07:26 2022 +0200
+++ b/main.cpp	Wed May 18 09:50:32 2022 +0000
@@ -7,46 +7,68 @@
 
 #include "IRSensor.h"
 
-# define M_PI 3.14159265358979323846  // number pi
+
 
 /**
- * notes:
+ * Note: Hardware related differences
  * - IRSensor class is not available in PM2_Libary
  * - ROME2 Robot uses different PINS than PES board
  */
 
+/* ------------------------------------------------------------------------------------------- *
+ * -- Defines
+ * ------------------------------------------------------------------------------------------- */
+ 
+# define M_PI 3.14159265358979323846
+
+/* ------------------------------------------------------------------------------------------- *
+ * -- Global Variables
+ * ------------------------------------------------------------------------------------------- */
+
 // logical variable main task
 bool do_execute_main_task = false;  // this variable will be toggled via the user button (blue button) to or not to execute the main task
 
 // user button on nucleo board
 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)
-void user_button_pressed_fcn();     // custom functions which gets executed when user button gets pressed and released, definition below
-void user_button_released_fcn();
+
+/* ------------------------------------------------------------------------------------------- *
+ * -- Constants and Parameters
+ * ------------------------------------------------------------------------------------------- */
+
+// default parameters for robots movement
+const float DISTANCE_THRESHOLD = 0.2f;        // minimum allowed distance to obstacle in [m]
+const float TRANSLATIONAL_VELOCITY = 0.4f;    // translational velocity in [m/s]
+const float ROTATIONAL_VELOCITY = 1.6f;       // rotational velocity in [rad/s]
+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;
+
+/* ------------------------------------------------------------------------------------------- *
+ * -- Static 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();
+
+/* ------------------------------------------------------------------------------------------- *
+ * -- Main
+ * ------------------------------------------------------------------------------------------- */
 int main()
 {
-    // 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
+    /* ------------------------------------------------------------------------------------------- *
+     * -- Setup: I/O
+     * ------------------------------------------------------------------------------------------- */
 
     // led on nucleo board
     DigitalOut user_led(LED1);      // create DigitalOut object to command user led
 
-    // 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;
-
-    // default parameters for robots movement
-    const float DISTANCE_THRESHOLD = 0.2f;        // minimum allowed distance to obstacle in [m]
-    const float TRANSLATIONAL_VELOCITY = 0.4f;    // translational velocity in [m/s]
-    const float ROTATIONAL_VELOCITY = 1.6f;       // rotational velocity in [rad/s]
-    const float VELOCITY_THRESHOLD = 0.05;        // velocity threshold before switching off, in [m/s] and [rad/s]
-
     // create DigitalOut objects for leds
+    DigitalOut enable_leds(PC_1);
     DigitalOut led0(PC_8);
     DigitalOut led1(PC_6);
     DigitalOut led2(PB_12);
@@ -57,7 +79,6 @@
 
     // create IR sensor objects  
     AnalogIn dist(PB_1);
-    DigitalOut enable_leds(PC_1);
     DigitalOut bit0(PH_1);
     DigitalOut bit1(PC_2);
     DigitalOut bit2(PC_3);
@@ -69,11 +90,19 @@
     IRSensor irSensor5(dist, bit0, bit1, bit2, 5);
     std::vector<IRSensor> irSensors = {irSensor0, irSensor1, irSensor2, irSensor3, irSensor4, irSensor5};
 
+    // attach button fall and rise functions to user button object
+    user_button.fall(&user_button_pressed_fcn);
+    user_button.rise(&user_button_released_fcn);
+
     // 19:1 Metal Gearmotor 37Dx68L mm 12V with 64 CPR Encoder (Helical Pinion)
     DigitalOut enable_motors(PB_2);
     DigitalIn motorDriverFault(PB_14);
     DigitalIn motorDriverWarning(PB_15);
 
+    /* ------------------------------------------------------------------------------------------- *
+     * -- Setup: Motion Controller
+     * ------------------------------------------------------------------------------------------- */
+
     // create SpeedController objects
     FastPWM pwm_M1(PA_9);  // motor M1 is closed-loop speed controlled (angle velocity)
     FastPWM pwm_M2(PA_8);  // motor M2 is closed-loop speed controlled (angle velocity)
@@ -105,6 +134,10 @@
     speedControllers[0]->setMaxAccelerationRPS(999.0f);  // adjust max. acceleration for smooth movement
     speedControllers[1]->setMaxAccelerationRPS(999.0f);
 
+    /* ------------------------------------------------------------------------------------------- *
+     * -- Setup: Robot Kinematics
+     * ------------------------------------------------------------------------------------------- */
+
     // robot kinematics
     const float r_wheel = 0.0766f / 2.0f; // wheel radius
     const float L_wheel = 0.176f;         // distance from wheel to wheel
@@ -125,15 +158,24 @@
     robot_coord_actual.setZero();
     wheel_speed_actual.setZero();
 
-    // attach button fall and rise functions to user button object
-    user_button.fall(&user_button_pressed_fcn);
-    user_button.rise(&user_button_released_fcn);
+    /* ------------------------------------------------------------------------------------------- *
+     * -- Setup: State Machine
+     * ------------------------------------------------------------------------------------------- */
+    
+    // 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
     
     // start timer
     main_task_timer.start();
 
     // set initial state machine state, enalbe leds, disable motors
     int state = ROBOT_OFF;
+
+    /* ------------------------------------------------------------------------------------------- *
+     * -- State Machine
+     * ------------------------------------------------------------------------------------------- */
+
     enable_leds = 1;
     enable_motors = 0;
     
@@ -221,6 +263,10 @@
 
         }
 
+        /* ------------------------------------------------------------------------------------------- *
+         * -- Inverse Kinematics
+         * ------------------------------------------------------------------------------------------- */
+
         // transform robot coordinates to wheel speed
         wheel_speed = Cwheel2robot.inverse() * robot_coord;
 
@@ -236,9 +282,17 @@
         speedControllers[1]->setDesiredSpeedRPS(wheel_speed_smooth(1)); // set a desired speed for speed controlled dc motors M2
         
         user_led = !user_led;
+
+        /* ------------------------------------------------------------------------------------------- *
+         * -- Printing to Console
+         * ------------------------------------------------------------------------------------------- */
         
         // 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();
@@ -246,13 +300,13 @@
     }
 }
 
-void user_button_pressed_fcn()
+static void user_button_pressed_fcn()
 {
     user_button_timer.start();
     user_button_timer.reset();
 }
 
-void user_button_released_fcn()
+static void user_button_released_fcn()
 {
     // read timer and toggle do_execute_main_task if the button was pressed longer than the below specified time
     int user_button_elapsed_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(user_button_timer.elapsed_time()).count();