Diff: main.cpp

Revision:

50:5947a2237bad

Parent:

49:f80f5d96716e

Child:

51:8c9f9b30dad0

--- a/main.cpp	Thu May 19 07:21:26 2022 +0000
+++ b/main.cpp	Thu May 19 09:02:43 2022 +0000
@@ -49,13 +49,24 @@
 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]
 
+/* ########################################################################################### *
+ * ### BEGIN STUDENT CODE: TASK 4: States of the state machine
+ * ###
+ * ### Please define the states of the state machine below.
+ * ### The states can either be of type "const int" or can be defined as an enum.
+ * ###                                                                                         */
+
 // 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;
+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;
+
+/* ###                                                                                         *
+ * ### END STUDENT CODE: TASK 4
+ * ########################################################################################### */
 
 /* ------------------------------------------------------------------------------------------- *
  * -- Function Declarations
@@ -137,23 +148,39 @@
     SpeedController* speedControllers[2];
     speedControllers[0] = new SpeedController(counts_per_turn, kn, max_voltage, pwm_M1, encoder_M1);
     speedControllers[1] = new SpeedController(counts_per_turn, kn, max_voltage, pwm_M2, encoder_M2);
-    speedControllers[0]->setSpeedCntrlGain(0.04f);      // adjust speedcontroller gains
+    speedControllers[0]->setSpeedCntrlGain(0.04f);          // adjust speedcontroller gains
     speedControllers[1]->setSpeedCntrlGain(0.04f);
-    //speedControllers[0]->setMaxAccelerationRPS(10.0f);  // use this if you're not using the trajectoryPlaners
-    //speedControllers[1]->setMaxAccelerationRPS(10.0f);
-    speedControllers[0]->setMaxAccelerationRPS(999.0f);  // adjust max. acceleration for smooth movement
+    speedControllers[0]->setMaxAccelerationRPS(999.0f);     // adjust max. acceleration for smooth movement
     speedControllers[1]->setMaxAccelerationRPS(999.0f);
 
     /* ------------------------------------------------------------------------------------------- *
      * -- Setup: Robot Kinematics
      * ------------------------------------------------------------------------------------------- */
-    // forward kinematics matrix
-    Eigen::Matrix2f Cwheel2robot;                               // transform wheel speeds to robot speed
+    
+    /* ########################################################################################### *
+     * ### BEGIN STUDENT CODE: TASK 1: Forward and inverse kinematic matrix
+     * ###
+     * ### 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]
+     * ### You can find the docs for the linear algebra library here: 
+     * ### https://eigen.tuxfamily.org/dox/group__TutorialMatrixClass.html
+     * ### https://eigen.tuxfamily.org/dox/group__TutorialLinearAlgebra.html#title4
+     * ###                                                                                         */
+
+    // 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;
 
-    // inverse kinematics matrix
-    auto Crobot2wheel = Cwheel2robot.inverse();                 // transform robot speed to wheel speeds
+    // inverse kinematic matrix (transform robot speeds to wheel speeds)
+    auto Crobot2wheel = Cwheel2robot.inverse();
+    
+
+    /* ###                                                                                         *
+     * ### END STUDENT CODE: TASK 1
+     * ########################################################################################### */
 
     // define kinematic variables
     Eigen::Vector2f robot_speed_desired;    // Robot speed vector [x_dt, alpha_dt] in [m/s] and [rad/s]
@@ -202,15 +229,42 @@
                 leds[i] = 1;
         }
 
-        // read actual wheel speed and transform it to robot coordinates
+        // measure the actual wheel speed
         wheel_speed_actual << speedControllers[0]->getSpeedRPS(), speedControllers[1]->getSpeedRPS();
+
+        /* ########################################################################################### *
+         * ### BEGIN STUDENT CODE: TASK 2: Compute the speed of the robot from wheel wheel measurements
+         * ###
+         * ### Every loop of the main task starts by reading the actual wheel speeds into the vector
+         * ### wheel_speed_actual with [w_R, w_L] in [rad/s]. Convert these measurements now into a
+         * ### robot speed vector (robot_speed_actual) with [x_dt, alpha_dt] in [m/s] and [rad/s].
+         * ###
+         * ###                                                                                         */
+        
         robot_speed_actual =  Cwheel2robot * wheel_speed_actual;
 
+        /* ###                                                                                         *
+         * ### END STUDENT CODE: TASK 2
+         * ########################################################################################### */
+
 
         /* ------------------------------------------------------------------------------------------- *
          * -- State Machine
          * ------------------------------------------------------------------------------------------- */
 
+        /* ########################################################################################### *
+         * ### BEGIN STUDENT CODE: TASK 5: Implementing the State Machine
+         * ###
+         * ### The program's logic is implemented here. The robot should now:
+         * ### - Wait for the user button (the blue one) to be pressed
+         * ### - If the button is pressed, then enable the motors and drive forward
+         * ### - If there is an obstacle, turn away from it and continue to drive forward
+         * ### - If the user button is pressed again, set the speed to zero
+         * ### - 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.
+         * ###                                                                                         */
+
         // state machine
         switch (state) {
 
@@ -286,13 +340,28 @@
                 break;
 
         }
+        /* ###                                                                                         *
+         * ### END STUDENT CODE: TASK 5
+         * ########################################################################################### */
 
         /* ------------------------------------------------------------------------------------------- *
          * -- Inverse Kinematics
          * ------------------------------------------------------------------------------------------- */
+    
+        /* ########################################################################################### *
+         * ### BEGIN STUDENT CODE: TASK 3: Compute the desired wheel speeds
+         * ###
+         * ### In the main task, the robot_speed_desired vector [x_dt, alpha_dt] in [m/s] and [rad/s] 
+         * ### is set. From that we need to compute the desired wheel speeds (wheel_speed_desired) 
+         * ### that are handed to the speed controller
+         * ###                                                                                         */
 
         // transform robot coordinates to wheel speed
-        wheel_speed_desired = Cwheel2robot.inverse() * robot_speed_desired;
+        wheel_speed_desired = Crobot2wheel * robot_speed_desired;
+
+        /* ###                                                                                         *
+         * ### END STUDENT CODE: TASK 3
+         * ########################################################################################### */
 
         // smooth desired wheel_speeds
         trajectoryPlaners[0]->incrementToVelocity(wheel_speed_desired(0) / (2.0f * M_PI), main_task_period);