Diff: main.cpp

Revision:

49:f80f5d96716e

Parent:

48:31ffd88e7f99

Child:

50:5947a2237bad

--- a/main.cpp	Wed May 18 22:22:31 2022 +0000
+++ b/main.cpp	Thu May 19 07:21:26 2022 +0000
@@ -39,6 +39,10 @@
  * -- Constants and Parameters
  * ------------------------------------------------------------------------------------------- */
 
+// kinematic parameters
+const float r_wheel = 0.0766f / 2.0f;   // wheel radius
+const float L_wheel = 0.176f;           // distance from wheel to wheel
+
 // 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]
@@ -59,10 +63,10 @@
 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
+void main_task_trigger();                  // triggers the main task each ticker period
 
 /* ------------------------------------------------------------------------------------------- *
- * -- Main
+ * -- Main Function
  * ------------------------------------------------------------------------------------------- */
 int main()
 {
@@ -143,32 +147,32 @@
     /* ------------------------------------------------------------------------------------------- *
      * -- 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
-    Eigen::Matrix2f Cwheel2robot; // transform wheel to robot
-    //Eigen::Matrix2f Crobot2wheel; // transform robot to wheel
+    // forward kinematics matrix
+    Eigen::Matrix2f Cwheel2robot;                               // transform wheel speeds to robot speed
     Cwheel2robot << -r_wheel / 2.0f   ,  r_wheel / 2.0f   ,
                     -r_wheel / L_wheel, -r_wheel / L_wheel;
-    //Crobot2wheel << -1.0f / r_wheel, -L_wheel / (2.0f * r_wheel),
-    //                 1.0f / r_wheel, -L_wheel / (2.0f * r_wheel);
-    Eigen::Vector2f robot_coord;         // contains v and w (robot translational and rotational velocities)
-    Eigen::Vector2f wheel_speed;         // w1 w2 (wheel speed)
-    Eigen::Vector2f wheel_speed_smooth;  // w1 w2 (wheel speed)
-    Eigen::Vector2f robot_coord_actual;
-    Eigen::Vector2f wheel_speed_actual;
-    robot_coord.setZero();
-    wheel_speed.setZero();
+
+    // inverse kinematics matrix
+    auto Crobot2wheel = Cwheel2robot.inverse();                 // transform robot speed to wheel speeds
+
+    // define kinematic variables
+    Eigen::Vector2f robot_speed_desired;    // Robot speed vector [x_dt, alpha_dt] in [m/s] and [rad/s]
+    Eigen::Vector2f wheel_speed_desired;    // Wheel speeds [w_R, w_L] in [rad/s]
+    Eigen::Vector2f wheel_speed_smooth;     // Wheel speeds limited and smoothed
+    Eigen::Vector2f wheel_speed_actual;     // Measured wheel speeds
+    Eigen::Vector2f robot_speed_actual;     // Measured robot speed
+
+    robot_speed_desired.setZero();
+    wheel_speed_desired.setZero();
     wheel_speed_smooth.setZero();
-    robot_coord_actual.setZero();
+    robot_speed_actual.setZero();
     wheel_speed_actual.setZero();
 
     /* ------------------------------------------------------------------------------------------- *
      * -- Setup: State Machine
      * ------------------------------------------------------------------------------------------- */
     
-    // while loop gets executed every main_task_period_ms milliseconds
+    // while loop gets executed every main_task_period
     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
 
     
@@ -200,7 +204,7 @@
 
         // read actual wheel speed and transform it to robot coordinates
         wheel_speed_actual << speedControllers[0]->getSpeedRPS(), speedControllers[1]->getSpeedRPS();
-        robot_coord_actual =  Cwheel2robot * wheel_speed_actual;
+        robot_speed_actual =  Cwheel2robot * wheel_speed_actual;
 
 
         /* ------------------------------------------------------------------------------------------- *
@@ -222,8 +226,8 @@
                 
                 if (do_execute_main_task) {
                     enable_motors = 1;
-                    robot_coord(0) = TRANSLATIONAL_VELOCITY;
-                    robot_coord(1) = 0.0f;
+                    robot_speed_desired(0) = TRANSLATIONAL_VELOCITY;
+                    robot_speed_desired(1) = 0.0f;
                     state = MOVE_FORWARD;
                 }
                 break;
@@ -231,16 +235,16 @@
             case MOVE_FORWARD:
 
                 if (!do_execute_main_task) {
-                    robot_coord(0) = 0.0f;
-                    robot_coord(1) = 0.0f;
+                    robot_speed_desired(0) = 0.0f;
+                    robot_speed_desired(1) = 0.0f;
                     state = SLOWING_DOWN;
                 } else if ((irSensors[0].read() < DISTANCE_THRESHOLD) || (irSensors[1].read() < DISTANCE_THRESHOLD)) {
-                    robot_coord(0) = 0.0f;
-                    robot_coord(1) =  ROTATIONAL_VELOCITY;
+                    robot_speed_desired(0) = 0.0f;
+                    robot_speed_desired(1) =  ROTATIONAL_VELOCITY;
                     state = TURN_LEFT;
                 } else if (irSensors[5].read() < DISTANCE_THRESHOLD) {
-                    robot_coord(0) = 0.0f;
-                    robot_coord(1) = -ROTATIONAL_VELOCITY;
+                    robot_speed_desired(0) = 0.0f;
+                    robot_speed_desired(1) = -ROTATIONAL_VELOCITY;
                     state = TURN_RIGHT;
                 } else {
                     // leave it driving
@@ -250,12 +254,12 @@
             case TURN_LEFT:
 
                 if (!do_execute_main_task) {
-                    robot_coord(1) = 0.0f;
+                    robot_speed_desired(1) = 0.0f;
                     state = SLOWING_DOWN;
 
                 } else if ((irSensors[0].read() > DISTANCE_THRESHOLD) && (irSensors[1].read() > DISTANCE_THRESHOLD) && (irSensors[5].read() > DISTANCE_THRESHOLD)) {
-                    robot_coord(0) = TRANSLATIONAL_VELOCITY;
-                    robot_coord(1) = 0.0f;
+                    robot_speed_desired(0) = TRANSLATIONAL_VELOCITY;
+                    robot_speed_desired(1) = 0.0f;
                     state = MOVE_FORWARD;
                 }
                 break;
@@ -263,18 +267,18 @@
             case TURN_RIGHT:
 
                 if (!do_execute_main_task) {
-                    robot_coord(1) = 0.0f;
+                    robot_speed_desired(1) = 0.0f;
                     state = SLOWING_DOWN;
                 } else if ((irSensors[0].read() > DISTANCE_THRESHOLD) && (irSensors[1].read() > DISTANCE_THRESHOLD) && (irSensors[5].read() > DISTANCE_THRESHOLD)) {
-                    robot_coord(0) = TRANSLATIONAL_VELOCITY;
-                    robot_coord(1) = 0.0f;
+                    robot_speed_desired(0) = TRANSLATIONAL_VELOCITY;
+                    robot_speed_desired(1) = 0.0f;
                     state = MOVE_FORWARD;
                 }
                 break;
                 
             case SLOWING_DOWN:
                 
-                if ((fabs(robot_coord_actual(0)) < VELOCITY_THRESHOLD) && (fabs(robot_coord_actual(1)) < VELOCITY_THRESHOLD)) {
+                if ((fabs(robot_speed_actual(0)) < VELOCITY_THRESHOLD) && (fabs(robot_speed_actual(1)) < VELOCITY_THRESHOLD)) {
                     enable_motors = 0;
                     state = ROBOT_OFF;
                 }
@@ -288,16 +292,16 @@
          * ------------------------------------------------------------------------------------------- */
 
         // transform robot coordinates to wheel speed
-        wheel_speed = Cwheel2robot.inverse() * robot_coord;
+        wheel_speed_desired = Cwheel2robot.inverse() * robot_speed_desired;
 
         // smooth desired wheel_speeds
-        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);
+        trajectoryPlaners[0]->incrementToVelocity(wheel_speed_desired(0) / (2.0f * M_PI), main_task_period);
+        trajectoryPlaners[1]->incrementToVelocity(wheel_speed_desired(1) / (2.0f * M_PI), main_task_period);
         wheel_speed_smooth << trajectoryPlaners[0]->getVelocity(), trajectoryPlaners[1]->getVelocity();
 
         // command speedController objects
-        //speedControllers[0]->setDesiredSpeedRPS(wheel_speed(0) / (2.0f * M_PI)); // use this if you're not using the trajectoryPlaners
-        //speedControllers[1]->setDesiredSpeedRPS(wheel_speed(1) / (2.0f * M_PI)); // use this if you're not using the trajectoryPlaners
+        //speedControllers[0]->setDesiredSpeedRPS(wheel_speed_desired(0) / (2.0f * M_PI)); // use this if you're not using the trajectoryPlaners
+        //speedControllers[1]->setDesiredSpeedRPS(wheel_speed_desired(1) / (2.0f * M_PI)); // use this if you're not using the trajectoryPlaners
         speedControllers[0]->setDesiredSpeedRPS(wheel_speed_smooth(0)); // set a desired speed for speed controlled dc motors M1
         speedControllers[1]->setDesiredSpeedRPS(wheel_speed_smooth(1)); // set a desired speed for speed controlled dc motors M2
         
@@ -330,5 +334,6 @@
 
 void main_task_trigger()
 {
+    // set the trigger to resume the waiting main task
     event_flags.set(main_task_flag);
 }
\ No newline at end of file