Diff: main.cpp

Revision:

51:8c9f9b30dad0

Parent:

50:5947a2237bad

Child:

52:75927464bf9c

--- a/main.cpp	Thu May 19 09:02:43 2022 +0000
+++ b/main.cpp	Thu May 19 09:27:18 2022 +0000
@@ -26,7 +26,7 @@
  * ------------------------------------------------------------------------------------------- */
 
 // 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
+bool robot_turned_on = false;  // this variable will be toggled via the user button (blue button)
 
 // 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)
@@ -57,12 +57,9 @@
  * ###                                                                                         */
 
 // discrete states of this state machine
-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;
+
+const int INIT = 0;
+
 
 /* ###                                                                                         *
  * ### END STUDENT CODE: TASK 4
@@ -160,7 +157,7 @@
     /* ########################################################################################### *
      * ### BEGIN STUDENT CODE: TASK 1: Forward and inverse kinematic matrix
      * ###
-     * ### Define the forward and backward kinematic matrix  (named Cwheel2robot resp. Cwheel2robot)
+     * ### Define the forward and backward kinematic matrix (named Cwheel2robot resp. Cwheel2robot)
      * ### using the kinematic parameters r_wheel (wheel radius) and L_wheel (wheel distance). 
      * ### The matrices transform between the robot speed vector [x_dt, alpha_dt] in [m/s] and [rad/s]
      * ### and the wheel speed vector [w_R, w_L] in [rad/s]
@@ -170,14 +167,11 @@
      * ###                                                                                         */
 
     // forward kinematics matrix (transforms wheel speeds to robot speeds)
-    Eigen::Matrix2f Cwheel2robot;                               
-    Cwheel2robot << -r_wheel / 2.0f   ,  r_wheel / 2.0f   ,
-                    -r_wheel / L_wheel, -r_wheel / L_wheel;
+    Eigen::Matrix2f Cwheel2robot; // still need to define the values                           
 
     // inverse kinematic matrix (transform robot speeds to wheel speeds)
-    auto Crobot2wheel = Cwheel2robot.inverse();
+    Eigen::Matrix2f Crobot2wheel; // still need to define the values
     
-
     /* ###                                                                                         *
      * ### END STUDENT CODE: TASK 1
      * ########################################################################################### */
@@ -241,7 +235,8 @@
          * ###
          * ###                                                                                         */
         
-        robot_speed_actual =  Cwheel2robot * wheel_speed_actual;
+        // transform wheel speed to robot coordinates
+
 
         /* ###                                                                                         *
          * ### END STUDENT CODE: TASK 2
@@ -263,83 +258,21 @@
          * ### - Wait until the robot is not moving anymore before disabling the motors
          * ###
          * ### Most importantly, do not block the program's execution inside the state machine.
+         * ###
+         * ### Following variables are used: 
+         * ###    robot_turned_on,          // boolen that is toggled by the blue button
+         * ###    enable_leds,              // to enable the led output
+         * ###    enable_motors,            // to enable the motor driver
+         * ###    irSensors,                // to measure the distance to obstacles
+         * ###    robot_speed_desired       // to set the robot speed
+         * ###    
          * ###                                                                                         */
 
-        // state machine
-        switch (state) {
-
-            case INIT:
-
-                enable_leds = 1;
-                enable_motors = 0;
-
-                state = ROBOT_OFF;  // default transition
-                break;
-            
-            case ROBOT_OFF:
-                
-                if (do_execute_main_task) {
-                    enable_motors = 1;
-                    robot_speed_desired(0) = TRANSLATIONAL_VELOCITY;
-                    robot_speed_desired(1) = 0.0f;
-                    state = MOVE_FORWARD;
-                }
-                break;
-                
-            case MOVE_FORWARD:
+        // example of setting the robot speed
+        robot_speed_desired(0) = TRANSLATIONAL_VELOCITY;
+        robot_speed_desired(1) = 0.0f;
 
-                if (!do_execute_main_task) {
-                    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_speed_desired(0) = 0.0f;
-                    robot_speed_desired(1) =  ROTATIONAL_VELOCITY;
-                    state = TURN_LEFT;
-                } else if (irSensors[5].read() < DISTANCE_THRESHOLD) {
-                    robot_speed_desired(0) = 0.0f;
-                    robot_speed_desired(1) = -ROTATIONAL_VELOCITY;
-                    state = TURN_RIGHT;
-                } else {
-                    // leave it driving
-                }
-                break;
-                
-            case TURN_LEFT:
 
-                if (!do_execute_main_task) {
-                    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_speed_desired(0) = TRANSLATIONAL_VELOCITY;
-                    robot_speed_desired(1) = 0.0f;
-                    state = MOVE_FORWARD;
-                }
-                break;
-                
-            case TURN_RIGHT:
-
-                if (!do_execute_main_task) {
-                    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_speed_desired(0) = TRANSLATIONAL_VELOCITY;
-                    robot_speed_desired(1) = 0.0f;
-                    state = MOVE_FORWARD;
-                }
-                break;
-                
-            case SLOWING_DOWN:
-                
-                if ((fabs(robot_speed_actual(0)) < VELOCITY_THRESHOLD) && (fabs(robot_speed_actual(1)) < VELOCITY_THRESHOLD)) {
-                    enable_motors = 0;
-                    state = ROBOT_OFF;
-                }
-                
-                break;
-
-        }
         /* ###                                                                                         *
          * ### END STUDENT CODE: TASK 5
          * ########################################################################################### */
@@ -357,7 +290,7 @@
          * ###                                                                                         */
 
         // transform robot coordinates to wheel speed
-        wheel_speed_desired = Crobot2wheel * robot_speed_desired;
+
 
         /* ###                                                                                         *
          * ### END STUDENT CODE: TASK 3
@@ -369,8 +302,6 @@
         wheel_speed_smooth << trajectoryPlaners[0]->getVelocity(), trajectoryPlaners[1]->getVelocity();
 
         // command speedController objects
-        //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
         
@@ -393,11 +324,11 @@
 
 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
+    // read timer and toggle robot_turned_on 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();
     user_button_timer.stop();
     if (user_button_elapsed_time_ms > 200) {
-        do_execute_main_task = !do_execute_main_task;
+        robot_turned_on = !robot_turned_on;
     }
 }