Revision 52:75927464bf9c, committed 2022-05-25

Comitter:

robleiker

Date:

Wed May 25 08:39:12 2022 +0000

Parent:

51:8c9f9b30dad0

Commit message:

Possible Solution

Changed in this revision

--- a/main.cpp	Thu May 19 09:27:18 2022 +0000
+++ b/main.cpp	Wed May 25 08:39:12 2022 +0000
@@ -57,8 +57,12 @@
  * ###                                                                                         */
 
 // discrete states of this state machine
-
-const int INIT = 0;
+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;
 
 
 /* ###                                                                                         *
@@ -167,10 +171,12 @@
      * ###                                                                                         */
 
     // forward kinematics matrix (transforms wheel speeds to robot speeds)
-    Eigen::Matrix2f Cwheel2robot; // still need to define the values                           
+    Eigen::Matrix2f Cwheel2robot;                               
+    Cwheel2robot << -r_wheel / 2.0f   ,  r_wheel / 2.0f   ,
+                    -r_wheel / L_wheel, -r_wheel / L_wheel;
 
     // inverse kinematic matrix (transform robot speeds to wheel speeds)
-    Eigen::Matrix2f Crobot2wheel; // still need to define the values
+    auto Crobot2wheel = Cwheel2robot.inverse();
     
     /* ###                                                                                         *
      * ### END STUDENT CODE: TASK 1
@@ -236,7 +242,7 @@
          * ###                                                                                         */
         
         // transform wheel speed to robot coordinates
-
+        robot_speed_actual =  Cwheel2robot * wheel_speed_actual;
 
         /* ###                                                                                         *
          * ### END STUDENT CODE: TASK 2
@@ -268,10 +274,81 @@
          * ###    
          * ###                                                                                         */
 
-        // example of setting the robot speed
-        robot_speed_desired(0) = TRANSLATIONAL_VELOCITY;
-        robot_speed_desired(1) = 0.0f;
+        // state machine
+        switch (state) {
+
+            case INIT:
+
+                enable_leds = 1;
+                enable_motors = 0;
+
+                state = ROBOT_OFF;  // default transition
+                break;
+            
+            case ROBOT_OFF:
+                
+                if (robot_turned_on) {
+                    enable_motors = 1;
+                    robot_speed_desired(0) = TRANSLATIONAL_VELOCITY;
+                    robot_speed_desired(1) = 0.0f;
+                    state = MOVE_FORWARD;
+                }
+                break;
+                
+            case MOVE_FORWARD:
 
+                if (!robot_turned_on) {
+                    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 (!robot_turned_on) {
+                    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 (!robot_turned_on) {
+                    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
@@ -290,7 +367,7 @@
          * ###                                                                                         */
 
         // transform robot coordinates to wheel speed
-
+        wheel_speed_desired = Crobot2wheel * robot_speed_desired;
 
         /* ###                                                                                         *
          * ### END STUDENT CODE: TASK 3
@@ -324,7 +401,7 @@
 
 static void user_button_released_fcn()
 {
-    // read timer and toggle robot_turned_on if the button was pressed longer than the below specified time
+    // 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();
     user_button_timer.stop();
     if (user_button_elapsed_time_ms > 200) {