Example project
Dependencies:
PM2_Libary
Eigen
Revision 51:8c9f9b30dad0, committed 2022-05-19
- Comitter:
- robleiker
- Date:
- Thu May 19 09:27:18 2022 +0000
- Parent:
- 50:5947a2237bad
- Child:
- 52:75927464bf9c
- Commit message:
- Student Template
Changed in this revision
--- 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;
}
}
Michael Ernst Peter
