Rome robot excercise
Dependencies: PM2_Libary Eigen
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);