Revision 53:054f42566910, committed 2022-05-23

Comitter:

johnschray2001

Date:

Mon May 23 11:28:24 2022 +0200

Parent:

52:19de6d1cce3a

Commit message:

Rome Robot Final

Changed in this revision

diff -r 19de6d1cce3a -r 054f42566910 main.cpp
--- a/main.cpp	Thu May 19 11:55:07 2022 +0000
+++ b/main.cpp	Mon May 23 11:28:24 2022 +0200
@@ -59,6 +59,9 @@
 // discrete states of this state machine
 
 const int INIT = 0;
+const int go = 1;
+const int turn = 2;
+const int stop = 3;
 
 
 /* ###                                                                                         *
@@ -167,10 +170,15 @@
      * ###                                                                                         */
 
     // forward kinematics matrix (transforms wheel speeds to robot speeds)
-    Eigen::Matrix2f Cwheel2robot; // still need to define the values                           
+    Eigen::Matrix2f Cwheel2robot; // still need to define the values 
+
+    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
+
+    Crobot2wheel = Cwheel2robot.inverse();
     
     /* ###                                                                                         *
      * ### END STUDENT CODE: TASK 1
@@ -236,6 +244,7 @@
          * ###                                                                                         */
         
         // transform wheel speed to robot coordinates
+        robot_speed_actual = Cwheel2robot * wheel_speed_actual;
 
 
         /* ###                                                                                         *
@@ -269,8 +278,94 @@
          * ###                                                                                         */
 
         // example of setting the robot speed
-        robot_speed_desired(0) = TRANSLATIONAL_VELOCITY;
-        robot_speed_desired(1) = 0.0f;
+        // robot_speed_desired(0) = TRANSLATIONAL_VELOCITY;
+        // robot_speed_desired(1) = 0.0f;
+
+        switch(state) {
+
+            case INIT:
+                // wait for button to be pressed
+                // if button pressed, next state is forward
+
+                // make sure speed is 0
+                robot_speed_desired(0) = 0.0f;
+                robot_speed_desired(1) = 0.0f;
+
+                // button pressed
+                if (robot_turned_on){
+                    enable_motors = 1;
+                    enable_leds = 1;
+                    state = go;
+                }
+
+                break;
+
+            case go:
+                // if an object is seen, turn
+                // if button pressed, stop
+                // continue if object not seen
+
+                // button pressed
+                if (!robot_turned_on){
+                    state = stop;
+                    break;
+                }
+                
+                // go forward
+                robot_speed_desired(0) = TRANSLATIONAL_VELOCITY;
+                robot_speed_desired(1) = 0.0f;
+
+                // if any of the leds are lit up, too close to an object and turn
+                if ((irSensor0.read() < DISTANCE_THRESHOLD) || (irSensor1.read() < DISTANCE_THRESHOLD) || (irSensor5.read() < DISTANCE_THRESHOLD)) {
+                    state = turn;
+                } 
+                
+                break;
+
+            case turn:
+                // keep turning if object seen
+                // if object no longer seen, stop turning
+                // if button pressed, stop
+
+                // button pressed
+                if (!robot_turned_on){
+                    state = stop;
+                    break;
+                }
+                
+                // make robot stop moving forward
+                robot_speed_desired(0) = 0.0f;
+
+                // turn left
+                if ((irSensor0.read() < DISTANCE_THRESHOLD) || (irSensor1.read() < DISTANCE_THRESHOLD)) {
+                    robot_speed_desired(1) = ROTATIONAL_VELOCITY;
+                } 
+                // turn right
+                else if (irSensor5.read() < DISTANCE_THRESHOLD) {
+                    robot_speed_desired(1) = -1.0f * ROTATIONAL_VELOCITY;
+                } 
+                // no sensors close enough, go forward
+                else {
+                    robot_speed_desired(1) = 0.0f;
+                    state = go;
+                }
+                break;
+
+            case stop:
+                // if button pressed, go forward
+                // stay stopped otherwise
+
+                // stop robot
+                robot_speed_desired(0) = 0.0f;
+                robot_speed_desired(1) = 0.0f;
+
+                // button pressed
+                if (robot_turned_on){
+                    state = go;
+                }
+
+                break;
+        }
 
 
         /* ###                                                                                         *
@@ -290,7 +385,7 @@
          * ###                                                                                         */
 
         // transform robot coordinates to wheel speed
-
+        wheel_speed_desired = Cwheel2robot.inverse() * robot_speed_desired;
 
         /* ###                                                                                         *
          * ### END STUDENT CODE: TASK 3