Michael Ernst Peter
/
PM2_Example_ROME2_Robots
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: PM2_Libary Eigen
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
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- 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) {
