Michael Ernst Peter / Mbed OS PM2_Example_ROME2_Robots

Example project

Dependencies:   PM2_Libary Eigen

main.cpp@52:75927464bf9c, 2022-05-25 (annotated)

Committer:
robleiker
Date:
Wed May 25 08:39:12 2022 +0000
Revision:
52:75927464bf9c
Parent:
51:8c9f9b30dad0 
Possible Solution

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pmic 36:23addefb97af 1 #include <mbed.h>
pmic 42:d2d2db5974c9 2 #include <math.h>
pmic 40:924bdbc33391 3 #include <vector>
pmic 40:924bdbc33391 4
pmic 42:d2d2db5974c9 5 #include "PM2_Libary.h"
pmic 42:d2d2db5974c9 6 #include "Eigen/Dense.h"
pmic 42:d2d2db5974c9 7
pmic 36:23addefb97af 8 #include "IRSensor.h"
pmic 40:924bdbc33391 9
robleiker 47:6693bffcdfd0 10
pmic 40:924bdbc33391 11
pmic 42:d2d2db5974c9 12 /**
robleiker 47:6693bffcdfd0 13 * Note: Hardware related differences
pmic 42:d2d2db5974c9 14 * - IRSensor class is not available in PM2_Libary
pmic 42:d2d2db5974c9 15 * - ROME2 Robot uses different PINS than PES board
pmic 42:d2d2db5974c9 16 */
pmic 42:d2d2db5974c9 17
robleiker 47:6693bffcdfd0 18 /* ------------------------------------------------------------------------------------------- *
robleiker 47:6693bffcdfd0 19 * -- Defines
robleiker 47:6693bffcdfd0 20 * ------------------------------------------------------------------------------------------- */
robleiker 47:6693bffcdfd0 21
robleiker 47:6693bffcdfd0 22 # define M_PI 3.14159265358979323846
robleiker 47:6693bffcdfd0 23
robleiker 47:6693bffcdfd0 24 /* ------------------------------------------------------------------------------------------- *
robleiker 47:6693bffcdfd0 25 * -- Global Variables
robleiker 47:6693bffcdfd0 26 * ------------------------------------------------------------------------------------------- */
robleiker 47:6693bffcdfd0 27
pmic 39:f336caef17d9 28 // logical variable main task
robleiker 51:8c9f9b30dad0 29 bool robot_turned_on = false; // this variable will be toggled via the user button (blue button)
pmic 39:f336caef17d9 30
pmic 39:f336caef17d9 31 // user button on nucleo board
pmic 39:f336caef17d9 32 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)
pmic 42:d2d2db5974c9 33 InterruptIn user_button(PC_13); // create InterruptIn interface object to evaluate user button falling and rising edge (no blocking code in ISR)
robleiker 47:6693bffcdfd0 34
robleiker 48:31ffd88e7f99 35 EventFlags event_flags;
robleiker 48:31ffd88e7f99 36 const uint32_t main_task_flag = 1;
robleiker 48:31ffd88e7f99 37
robleiker 47:6693bffcdfd0 38 /* ------------------------------------------------------------------------------------------- *
robleiker 47:6693bffcdfd0 39 * -- Constants and Parameters
robleiker 47:6693bffcdfd0 40 * ------------------------------------------------------------------------------------------- */
robleiker 47:6693bffcdfd0 41
robleiker 49:f80f5d96716e 42 // kinematic parameters
robleiker 49:f80f5d96716e 43 const float r_wheel = 0.0766f / 2.0f; // wheel radius
robleiker 49:f80f5d96716e 44 const float L_wheel = 0.176f; // distance from wheel to wheel
robleiker 49:f80f5d96716e 45
robleiker 47:6693bffcdfd0 46 // default parameters for robots movement
robleiker 47:6693bffcdfd0 47 const float DISTANCE_THRESHOLD = 0.2f; // minimum allowed distance to obstacle in [m]
robleiker 47:6693bffcdfd0 48 const float TRANSLATIONAL_VELOCITY = 0.4f; // translational velocity in [m/s]
robleiker 47:6693bffcdfd0 49 const float ROTATIONAL_VELOCITY = 1.6f; // rotational velocity in [rad/s]
robleiker 47:6693bffcdfd0 50 const float VELOCITY_THRESHOLD = 0.05; // velocity threshold before switching off, in [m/s] and [rad/s]
pmic 39:f336caef17d9 51
robleiker 50:5947a2237bad 52 /* ########################################################################################### *
robleiker 50:5947a2237bad 53 * ### BEGIN STUDENT CODE: TASK 4: States of the state machine
robleiker 50:5947a2237bad 54 * ###
robleiker 50:5947a2237bad 55 * ### Please define the states of the state machine below.
robleiker 50:5947a2237bad 56 * ### The states can either be of type "const int" or can be defined as an enum.
robleiker 50:5947a2237bad 57 * ### */
robleiker 50:5947a2237bad 58
robleiker 47:6693bffcdfd0 59 // discrete states of this state machine
robleiker 52:75927464bf9c 60 const int INIT = 0;
robleiker 52:75927464bf9c 61 const int ROBOT_OFF = 1;
robleiker 52:75927464bf9c 62 const int MOVE_FORWARD = 2;
robleiker 52:75927464bf9c 63 const int TURN_LEFT = 3;
robleiker 52:75927464bf9c 64 const int TURN_RIGHT = 4;
robleiker 52:75927464bf9c 65 const int SLOWING_DOWN = 5;
robleiker 51:8c9f9b30dad0 66
robleiker 50:5947a2237bad 67
robleiker 50:5947a2237bad 68 /* ### *
robleiker 50:5947a2237bad 69 * ### END STUDENT CODE: TASK 4
robleiker 50:5947a2237bad 70 * ########################################################################################### */
robleiker 47:6693bffcdfd0 71
robleiker 47:6693bffcdfd0 72 /* ------------------------------------------------------------------------------------------- *
robleiker 48:31ffd88e7f99 73 * -- Function Declarations
robleiker 47:6693bffcdfd0 74 * ------------------------------------------------------------------------------------------- */
robleiker 47:6693bffcdfd0 75 static void user_button_pressed_fcn(); // custom functions which gets executed when user button gets pressed and released, definition below
robleiker 47:6693bffcdfd0 76 static void user_button_released_fcn();
robleiker 47:6693bffcdfd0 77
robleiker 49:f80f5d96716e 78 void main_task_trigger(); // triggers the main task each ticker period
robleiker 48:31ffd88e7f99 79
robleiker 47:6693bffcdfd0 80 /* ------------------------------------------------------------------------------------------- *
robleiker 49:f80f5d96716e 81 * -- Main Function
robleiker 47:6693bffcdfd0 82 * ------------------------------------------------------------------------------------------- */
pmic 44:dd746bf0e81f 83 int main()
pmic 44:dd746bf0e81f 84 {
robleiker 47:6693bffcdfd0 85 /* ------------------------------------------------------------------------------------------- *
robleiker 47:6693bffcdfd0 86 * -- Setup: I/O
robleiker 47:6693bffcdfd0 87 * ------------------------------------------------------------------------------------------- */
pmic 39:f336caef17d9 88
pmic 42:d2d2db5974c9 89 // led on nucleo board
pmic 42:d2d2db5974c9 90 DigitalOut user_led(LED1); // create DigitalOut object to command user led
pmic 39:f336caef17d9 91
pmic 42:d2d2db5974c9 92 // create DigitalOut objects for leds
robleiker 47:6693bffcdfd0 93 DigitalOut enable_leds(PC_1);
pmic 42:d2d2db5974c9 94 DigitalOut led0(PC_8);
pmic 42:d2d2db5974c9 95 DigitalOut led1(PC_6);
pmic 42:d2d2db5974c9 96 DigitalOut led2(PB_12);
pmic 42:d2d2db5974c9 97 DigitalOut led3(PA_7);
pmic 42:d2d2db5974c9 98 DigitalOut led4(PC_0);
pmic 42:d2d2db5974c9 99 DigitalOut led5(PC_9);
pmic 42:d2d2db5974c9 100 std::vector<DigitalOut> leds = {led0, led1, led2, led3, led4, led5};
pmic 39:f336caef17d9 101
pmic 42:d2d2db5974c9 102 // create IR sensor objects
pmic 42:d2d2db5974c9 103 AnalogIn dist(PB_1);
pmic 42:d2d2db5974c9 104 DigitalOut bit0(PH_1);
pmic 42:d2d2db5974c9 105 DigitalOut bit1(PC_2);
pmic 42:d2d2db5974c9 106 DigitalOut bit2(PC_3);
pmic 42:d2d2db5974c9 107 IRSensor irSensor0(dist, bit0, bit1, bit2, 0);
pmic 42:d2d2db5974c9 108 IRSensor irSensor1(dist, bit0, bit1, bit2, 1);
pmic 42:d2d2db5974c9 109 IRSensor irSensor2(dist, bit0, bit1, bit2, 2);
pmic 42:d2d2db5974c9 110 IRSensor irSensor3(dist, bit0, bit1, bit2, 3);
pmic 42:d2d2db5974c9 111 IRSensor irSensor4(dist, bit0, bit1, bit2, 4);
pmic 42:d2d2db5974c9 112 IRSensor irSensor5(dist, bit0, bit1, bit2, 5);
pmic 42:d2d2db5974c9 113 std::vector<IRSensor> irSensors = {irSensor0, irSensor1, irSensor2, irSensor3, irSensor4, irSensor5};
pmic 39:f336caef17d9 114
robleiker 47:6693bffcdfd0 115 // attach button fall and rise functions to user button object
robleiker 47:6693bffcdfd0 116 user_button.fall(&user_button_pressed_fcn);
robleiker 47:6693bffcdfd0 117 user_button.rise(&user_button_released_fcn);
robleiker 47:6693bffcdfd0 118
pmic 42:d2d2db5974c9 119 // 19:1 Metal Gearmotor 37Dx68L mm 12V with 64 CPR Encoder (Helical Pinion)
pmic 42:d2d2db5974c9 120 DigitalOut enable_motors(PB_2);
pmic 42:d2d2db5974c9 121 DigitalIn motorDriverFault(PB_14);
pmic 42:d2d2db5974c9 122 DigitalIn motorDriverWarning(PB_15);
pmic 39:f336caef17d9 123
robleiker 47:6693bffcdfd0 124 /* ------------------------------------------------------------------------------------------- *
robleiker 47:6693bffcdfd0 125 * -- Setup: Motion Controller
robleiker 47:6693bffcdfd0 126 * ------------------------------------------------------------------------------------------- */
robleiker 47:6693bffcdfd0 127
pmic 42:d2d2db5974c9 128 // create SpeedController objects
pmic 42:d2d2db5974c9 129 FastPWM pwm_M1(PA_9); // motor M1 is closed-loop speed controlled (angle velocity)
pmic 42:d2d2db5974c9 130 FastPWM pwm_M2(PA_8); // motor M2 is closed-loop speed controlled (angle velocity)
pmic 42:d2d2db5974c9 131 EncoderCounter encoder_M1(PA_6, PC_7); // create encoder objects to read in the encoder counter values
pmic 42:d2d2db5974c9 132 EncoderCounter encoder_M2(PB_6, PB_7);
pmic 42:d2d2db5974c9 133 const float max_voltage = 12.0f; // define maximum voltage of battery packs, adjust this to 6.0f V if you only use one batterypack
pmic 42:d2d2db5974c9 134 const float counts_per_turn = 64.0f * 19.0f; // define counts per turn at gearbox end: counts/turn * gearratio
pmic 42:d2d2db5974c9 135 const float kn = 530.0f / 12.0f; // define motor constant in rpm per V
pmic 42:d2d2db5974c9 136
pmic 46:41c9367da539 137 // create Motion objects (trajectory planner)
pmic 46:41c9367da539 138 Motion* trajectoryPlaners[2];
pmic 46:41c9367da539 139 trajectoryPlaners[0] = new Motion;
pmic 46:41c9367da539 140 trajectoryPlaners[1] = new Motion;
pmic 46:41c9367da539 141 trajectoryPlaners[0]->setProfileVelocity(max_voltage * kn / 60.0f);
pmic 46:41c9367da539 142 trajectoryPlaners[1]->setProfileVelocity(max_voltage * kn / 60.0f);
pmic 46:41c9367da539 143 trajectoryPlaners[0]->setProfileAcceleration(10.0f);
pmic 46:41c9367da539 144 trajectoryPlaners[1]->setProfileAcceleration(10.0f);
pmic 46:41c9367da539 145 trajectoryPlaners[0]->setProfileDeceleration(10.0f);
pmic 46:41c9367da539 146 trajectoryPlaners[1]->setProfileDeceleration(10.0f);
pmic 46:41c9367da539 147
pmic 46:41c9367da539 148 // create SpeedController objects
pmic 42:d2d2db5974c9 149 SpeedController* speedControllers[2];
pmic 42:d2d2db5974c9 150 speedControllers[0] = new SpeedController(counts_per_turn, kn, max_voltage, pwm_M1, encoder_M1);
pmic 42:d2d2db5974c9 151 speedControllers[1] = new SpeedController(counts_per_turn, kn, max_voltage, pwm_M2, encoder_M2);
robleiker 50:5947a2237bad 152 speedControllers[0]->setSpeedCntrlGain(0.04f); // adjust speedcontroller gains
pmic 42:d2d2db5974c9 153 speedControllers[1]->setSpeedCntrlGain(0.04f);
robleiker 50:5947a2237bad 154 speedControllers[0]->setMaxAccelerationRPS(999.0f); // adjust max. acceleration for smooth movement
pmic 46:41c9367da539 155 speedControllers[1]->setMaxAccelerationRPS(999.0f);
pmic 39:f336caef17d9 156
robleiker 47:6693bffcdfd0 157 /* ------------------------------------------------------------------------------------------- *
robleiker 47:6693bffcdfd0 158 * -- Setup: Robot Kinematics
robleiker 47:6693bffcdfd0 159 * ------------------------------------------------------------------------------------------- */
robleiker 50:5947a2237bad 160
robleiker 50:5947a2237bad 161 /* ########################################################################################### *
robleiker 50:5947a2237bad 162 * ### BEGIN STUDENT CODE: TASK 1: Forward and inverse kinematic matrix
robleiker 50:5947a2237bad 163 * ###
robleiker 51:8c9f9b30dad0 164 * ### Define the forward and backward kinematic matrix (named Cwheel2robot resp. Cwheel2robot)
robleiker 50:5947a2237bad 165 * ### using the kinematic parameters r_wheel (wheel radius) and L_wheel (wheel distance).
robleiker 50:5947a2237bad 166 * ### The matrices transform between the robot speed vector [x_dt, alpha_dt] in [m/s] and [rad/s]
robleiker 50:5947a2237bad 167 * ### and the wheel speed vector [w_R, w_L] in [rad/s]
robleiker 50:5947a2237bad 168 * ### You can find the docs for the linear algebra library here:
robleiker 50:5947a2237bad 169 * ### https://eigen.tuxfamily.org/dox/group__TutorialMatrixClass.html
robleiker 50:5947a2237bad 170 * ### https://eigen.tuxfamily.org/dox/group__TutorialLinearAlgebra.html#title4
robleiker 50:5947a2237bad 171 * ### */
robleiker 50:5947a2237bad 172
robleiker 50:5947a2237bad 173 // forward kinematics matrix (transforms wheel speeds to robot speeds)
robleiker 52:75927464bf9c 174 Eigen::Matrix2f Cwheel2robot;
robleiker 52:75927464bf9c 175 Cwheel2robot << -r_wheel / 2.0f , r_wheel / 2.0f ,
robleiker 52:75927464bf9c 176 -r_wheel / L_wheel, -r_wheel / L_wheel;
robleiker 49:f80f5d96716e 177
robleiker 50:5947a2237bad 178 // inverse kinematic matrix (transform robot speeds to wheel speeds)
robleiker 52:75927464bf9c 179 auto Crobot2wheel = Cwheel2robot.inverse();
robleiker 50:5947a2237bad 180
robleiker 50:5947a2237bad 181 /* ### *
robleiker 50:5947a2237bad 182 * ### END STUDENT CODE: TASK 1
robleiker 50:5947a2237bad 183 * ########################################################################################### */
robleiker 49:f80f5d96716e 184
robleiker 49:f80f5d96716e 185 // define kinematic variables
robleiker 49:f80f5d96716e 186 Eigen::Vector2f robot_speed_desired; // Robot speed vector [x_dt, alpha_dt] in [m/s] and [rad/s]
robleiker 49:f80f5d96716e 187 Eigen::Vector2f wheel_speed_desired; // Wheel speeds [w_R, w_L] in [rad/s]
robleiker 49:f80f5d96716e 188 Eigen::Vector2f wheel_speed_smooth; // Wheel speeds limited and smoothed
robleiker 49:f80f5d96716e 189 Eigen::Vector2f wheel_speed_actual; // Measured wheel speeds
robleiker 49:f80f5d96716e 190 Eigen::Vector2f robot_speed_actual; // Measured robot speed
robleiker 49:f80f5d96716e 191
robleiker 49:f80f5d96716e 192 robot_speed_desired.setZero();
robleiker 49:f80f5d96716e 193 wheel_speed_desired.setZero();
pmic 46:41c9367da539 194 wheel_speed_smooth.setZero();
robleiker 49:f80f5d96716e 195 robot_speed_actual.setZero();
pmic 42:d2d2db5974c9 196 wheel_speed_actual.setZero();
pmic 39:f336caef17d9 197
robleiker 47:6693bffcdfd0 198 /* ------------------------------------------------------------------------------------------- *
robleiker 47:6693bffcdfd0 199 * -- Setup: State Machine
robleiker 47:6693bffcdfd0 200 * ------------------------------------------------------------------------------------------- */
robleiker 47:6693bffcdfd0 201
robleiker 49:f80f5d96716e 202 // while loop gets executed every main_task_period
robleiker 48:31ffd88e7f99 203 const float main_task_period = 10e-3; // define main task period time in ms e.g. 50 ms -> main task runns 20 times per second
robleiker 48:31ffd88e7f99 204
pmic 36:23addefb97af 205
robleiker 48:31ffd88e7f99 206 Ticker ticker; // calls a fuction with a precise period
robleiker 48:31ffd88e7f99 207 ticker.attach(main_task_trigger, std::chrono::microseconds(static_cast<int>(main_task_period*1e6))); // call the main task trigger every period
pmic 40:924bdbc33391 208
pmic 42:d2d2db5974c9 209 // set initial state machine state, enalbe leds, disable motors
robleiker 48:31ffd88e7f99 210 int state = INIT;
pmic 36:23addefb97af 211
pmic 39:f336caef17d9 212 while (true) { // this loop will run forever
robleiker 48:31ffd88e7f99 213
robleiker 48:31ffd88e7f99 214 /* ------------------------------------------------------------------------------------------- *
robleiker 48:31ffd88e7f99 215 * -- Wait for the next Cycle
robleiker 48:31ffd88e7f99 216 * ------------------------------------------------------------------------------------------- */
robleiker 48:31ffd88e7f99 217 event_flags.wait_any(main_task_flag);
robleiker 48:31ffd88e7f99 218
pmic 36:23addefb97af 219
robleiker 48:31ffd88e7f99 220 /* ------------------------------------------------------------------------------------------- *
robleiker 48:31ffd88e7f99 221 * -- Read Sensors
robleiker 48:31ffd88e7f99 222 * ------------------------------------------------------------------------------------------- */
pmic 39:f336caef17d9 223
pmic 42:d2d2db5974c9 224 // set leds according to DISTANCE_THRESHOLD
pmic 42:d2d2db5974c9 225 for (uint8_t i = 0; i < leds.size(); i++) {
pmic 42:d2d2db5974c9 226 if (irSensors[i].read() > DISTANCE_THRESHOLD)
pmic 42:d2d2db5974c9 227 leds[i] = 0;
pmic 42:d2d2db5974c9 228 else
pmic 42:d2d2db5974c9 229 leds[i] = 1;
pmic 40:924bdbc33391 230 }
pmic 39:f336caef17d9 231
robleiker 50:5947a2237bad 232 // measure the actual wheel speed
pmic 42:d2d2db5974c9 233 wheel_speed_actual << speedControllers[0]->getSpeedRPS(), speedControllers[1]->getSpeedRPS();
robleiker 50:5947a2237bad 234
robleiker 50:5947a2237bad 235 /* ########################################################################################### *
robleiker 50:5947a2237bad 236 * ### BEGIN STUDENT CODE: TASK 2: Compute the speed of the robot from wheel wheel measurements
robleiker 50:5947a2237bad 237 * ###
robleiker 50:5947a2237bad 238 * ### Every loop of the main task starts by reading the actual wheel speeds into the vector
robleiker 50:5947a2237bad 239 * ### wheel_speed_actual with [w_R, w_L] in [rad/s]. Convert these measurements now into a
robleiker 50:5947a2237bad 240 * ### robot speed vector (robot_speed_actual) with [x_dt, alpha_dt] in [m/s] and [rad/s].
robleiker 50:5947a2237bad 241 * ###
robleiker 50:5947a2237bad 242 * ### */
robleiker 50:5947a2237bad 243
robleiker 51:8c9f9b30dad0 244 // transform wheel speed to robot coordinates
robleiker 52:75927464bf9c 245 robot_speed_actual = Cwheel2robot * wheel_speed_actual;
pmic 42:d2d2db5974c9 246
robleiker 50:5947a2237bad 247 /* ### *
robleiker 50:5947a2237bad 248 * ### END STUDENT CODE: TASK 2
robleiker 50:5947a2237bad 249 * ########################################################################################### */
robleiker 50:5947a2237bad 250
robleiker 48:31ffd88e7f99 251
robleiker 48:31ffd88e7f99 252 /* ------------------------------------------------------------------------------------------- *
robleiker 48:31ffd88e7f99 253 * -- State Machine
robleiker 48:31ffd88e7f99 254 * ------------------------------------------------------------------------------------------- */
robleiker 48:31ffd88e7f99 255
robleiker 50:5947a2237bad 256 /* ########################################################################################### *
robleiker 50:5947a2237bad 257 * ### BEGIN STUDENT CODE: TASK 5: Implementing the State Machine
robleiker 50:5947a2237bad 258 * ###
robleiker 50:5947a2237bad 259 * ### The program's logic is implemented here. The robot should now:
robleiker 50:5947a2237bad 260 * ### - Wait for the user button (the blue one) to be pressed
robleiker 50:5947a2237bad 261 * ### - If the button is pressed, then enable the motors and drive forward
robleiker 50:5947a2237bad 262 * ### - If there is an obstacle, turn away from it and continue to drive forward
robleiker 50:5947a2237bad 263 * ### - If the user button is pressed again, set the speed to zero
robleiker 50:5947a2237bad 264 * ### - Wait until the robot is not moving anymore before disabling the motors
robleiker 50:5947a2237bad 265 * ###
robleiker 50:5947a2237bad 266 * ### Most importantly, do not block the program's execution inside the state machine.
robleiker 51:8c9f9b30dad0 267 * ###
robleiker 51:8c9f9b30dad0 268 * ### Following variables are used:
robleiker 51:8c9f9b30dad0 269 * ### robot_turned_on, // boolen that is toggled by the blue button
robleiker 51:8c9f9b30dad0 270 * ### enable_leds, // to enable the led output
robleiker 51:8c9f9b30dad0 271 * ### enable_motors, // to enable the motor driver
robleiker 51:8c9f9b30dad0 272 * ### irSensors, // to measure the distance to obstacles
robleiker 51:8c9f9b30dad0 273 * ### robot_speed_desired // to set the robot speed
robleiker 51:8c9f9b30dad0 274 * ###
robleiker 50:5947a2237bad 275 * ### */
robleiker 50:5947a2237bad 276
robleiker 52:75927464bf9c 277 // state machine
robleiker 52:75927464bf9c 278 switch (state) {
robleiker 52:75927464bf9c 279
robleiker 52:75927464bf9c 280 case INIT:
robleiker 52:75927464bf9c 281
robleiker 52:75927464bf9c 282 enable_leds = 1;
robleiker 52:75927464bf9c 283 enable_motors = 0;
robleiker 52:75927464bf9c 284
robleiker 52:75927464bf9c 285 state = ROBOT_OFF; // default transition
robleiker 52:75927464bf9c 286 break;
robleiker 52:75927464bf9c 287
robleiker 52:75927464bf9c 288 case ROBOT_OFF:
robleiker 52:75927464bf9c 289
robleiker 52:75927464bf9c 290 if (robot_turned_on) {
robleiker 52:75927464bf9c 291 enable_motors = 1;
robleiker 52:75927464bf9c 292 robot_speed_desired(0) = TRANSLATIONAL_VELOCITY;
robleiker 52:75927464bf9c 293 robot_speed_desired(1) = 0.0f;
robleiker 52:75927464bf9c 294 state = MOVE_FORWARD;
robleiker 52:75927464bf9c 295 }
robleiker 52:75927464bf9c 296 break;
robleiker 52:75927464bf9c 297
robleiker 52:75927464bf9c 298 case MOVE_FORWARD:
pmic 42:d2d2db5974c9 299
robleiker 52:75927464bf9c 300 if (!robot_turned_on) {
robleiker 52:75927464bf9c 301 robot_speed_desired(0) = 0.0f;
robleiker 52:75927464bf9c 302 robot_speed_desired(1) = 0.0f;
robleiker 52:75927464bf9c 303 state = SLOWING_DOWN;
robleiker 52:75927464bf9c 304 } else if ((irSensors[0].read() < DISTANCE_THRESHOLD) || (irSensors[1].read() < DISTANCE_THRESHOLD)) {
robleiker 52:75927464bf9c 305 robot_speed_desired(0) = 0.0f;
robleiker 52:75927464bf9c 306 robot_speed_desired(1) = ROTATIONAL_VELOCITY;
robleiker 52:75927464bf9c 307 state = TURN_LEFT;
robleiker 52:75927464bf9c 308 } else if (irSensors[5].read() < DISTANCE_THRESHOLD) {
robleiker 52:75927464bf9c 309 robot_speed_desired(0) = 0.0f;
robleiker 52:75927464bf9c 310 robot_speed_desired(1) = -ROTATIONAL_VELOCITY;
robleiker 52:75927464bf9c 311 state = TURN_RIGHT;
robleiker 52:75927464bf9c 312 } else {
robleiker 52:75927464bf9c 313 // leave it driving
robleiker 52:75927464bf9c 314 }
robleiker 52:75927464bf9c 315 break;
robleiker 52:75927464bf9c 316
robleiker 52:75927464bf9c 317 case TURN_LEFT:
robleiker 52:75927464bf9c 318
robleiker 52:75927464bf9c 319 if (!robot_turned_on) {
robleiker 52:75927464bf9c 320 robot_speed_desired(1) = 0.0f;
robleiker 52:75927464bf9c 321 state = SLOWING_DOWN;
robleiker 52:75927464bf9c 322
robleiker 52:75927464bf9c 323 } else if ((irSensors[0].read() > DISTANCE_THRESHOLD) && (irSensors[1].read() > DISTANCE_THRESHOLD) && (irSensors[5].read() > DISTANCE_THRESHOLD)) {
robleiker 52:75927464bf9c 324 robot_speed_desired(0) = TRANSLATIONAL_VELOCITY;
robleiker 52:75927464bf9c 325 robot_speed_desired(1) = 0.0f;
robleiker 52:75927464bf9c 326 state = MOVE_FORWARD;
robleiker 52:75927464bf9c 327 }
robleiker 52:75927464bf9c 328 break;
robleiker 52:75927464bf9c 329
robleiker 52:75927464bf9c 330 case TURN_RIGHT:
robleiker 52:75927464bf9c 331
robleiker 52:75927464bf9c 332 if (!robot_turned_on) {
robleiker 52:75927464bf9c 333 robot_speed_desired(1) = 0.0f;
robleiker 52:75927464bf9c 334 state = SLOWING_DOWN;
robleiker 52:75927464bf9c 335 } else if ((irSensors[0].read() > DISTANCE_THRESHOLD) && (irSensors[1].read() > DISTANCE_THRESHOLD) && (irSensors[5].read() > DISTANCE_THRESHOLD)) {
robleiker 52:75927464bf9c 336 robot_speed_desired(0) = TRANSLATIONAL_VELOCITY;
robleiker 52:75927464bf9c 337 robot_speed_desired(1) = 0.0f;
robleiker 52:75927464bf9c 338 state = MOVE_FORWARD;
robleiker 52:75927464bf9c 339 }
robleiker 52:75927464bf9c 340 break;
robleiker 52:75927464bf9c 341
robleiker 52:75927464bf9c 342 case SLOWING_DOWN:
robleiker 52:75927464bf9c 343
robleiker 52:75927464bf9c 344 if ((fabs(robot_speed_actual(0)) < VELOCITY_THRESHOLD) && (fabs(robot_speed_actual(1)) < VELOCITY_THRESHOLD)) {
robleiker 52:75927464bf9c 345 enable_motors = 0;
robleiker 52:75927464bf9c 346 state = ROBOT_OFF;
robleiker 52:75927464bf9c 347 }
robleiker 52:75927464bf9c 348
robleiker 52:75927464bf9c 349 break;
robleiker 52:75927464bf9c 350
robleiker 52:75927464bf9c 351 }
pmic 42:d2d2db5974c9 352
robleiker 50:5947a2237bad 353 /* ### *
robleiker 50:5947a2237bad 354 * ### END STUDENT CODE: TASK 5
robleiker 50:5947a2237bad 355 * ########################################################################################### */
pmic 39:f336caef17d9 356
robleiker 47:6693bffcdfd0 357 /* ------------------------------------------------------------------------------------------- *
robleiker 47:6693bffcdfd0 358 * -- Inverse Kinematics
robleiker 47:6693bffcdfd0 359 * ------------------------------------------------------------------------------------------- */
robleiker 50:5947a2237bad 360
robleiker 50:5947a2237bad 361 /* ########################################################################################### *
robleiker 50:5947a2237bad 362 * ### BEGIN STUDENT CODE: TASK 3: Compute the desired wheel speeds
robleiker 50:5947a2237bad 363 * ###
robleiker 50:5947a2237bad 364 * ### In the main task, the robot_speed_desired vector [x_dt, alpha_dt] in [m/s] and [rad/s]
robleiker 50:5947a2237bad 365 * ### is set. From that we need to compute the desired wheel speeds (wheel_speed_desired)
robleiker 50:5947a2237bad 366 * ### that are handed to the speed controller
robleiker 50:5947a2237bad 367 * ### */
robleiker 47:6693bffcdfd0 368
pmic 42:d2d2db5974c9 369 // transform robot coordinates to wheel speed
robleiker 52:75927464bf9c 370 wheel_speed_desired = Crobot2wheel * robot_speed_desired;
robleiker 50:5947a2237bad 371
robleiker 50:5947a2237bad 372 /* ### *
robleiker 50:5947a2237bad 373 * ### END STUDENT CODE: TASK 3
robleiker 50:5947a2237bad 374 * ########################################################################################### */
pmic 42:d2d2db5974c9 375
pmic 46:41c9367da539 376 // smooth desired wheel_speeds
robleiker 49:f80f5d96716e 377 trajectoryPlaners[0]->incrementToVelocity(wheel_speed_desired(0) / (2.0f * M_PI), main_task_period);
robleiker 49:f80f5d96716e 378 trajectoryPlaners[1]->incrementToVelocity(wheel_speed_desired(1) / (2.0f * M_PI), main_task_period);
pmic 46:41c9367da539 379 wheel_speed_smooth << trajectoryPlaners[0]->getVelocity(), trajectoryPlaners[1]->getVelocity();
pmic 46:41c9367da539 380
pmic 42:d2d2db5974c9 381 // command speedController objects
pmic 46:41c9367da539 382 speedControllers[0]->setDesiredSpeedRPS(wheel_speed_smooth(0)); // set a desired speed for speed controlled dc motors M1
pmic 46:41c9367da539 383 speedControllers[1]->setDesiredSpeedRPS(wheel_speed_smooth(1)); // set a desired speed for speed controlled dc motors M2
pmic 42:d2d2db5974c9 384
pmic 39:f336caef17d9 385 user_led = !user_led;
robleiker 47:6693bffcdfd0 386
robleiker 47:6693bffcdfd0 387 /* ------------------------------------------------------------------------------------------- *
robleiker 47:6693bffcdfd0 388 * -- Printing to Console
robleiker 47:6693bffcdfd0 389 * ------------------------------------------------------------------------------------------- */
pmic 36:23addefb97af 390
pmic 42:d2d2db5974c9 391 // do only output via serial what's really necessary (this makes your code slow)
pmic 45:d9e6e89210f9 392 //printf("%f, %f\r\n", wheel_speed_actual(0), wheel_speed_actual(1));
pmic 1:93d997d6b232 393 }
pmic 1:93d997d6b232 394 }
pmic 39:f336caef17d9 395
robleiker 47:6693bffcdfd0 396 static void user_button_pressed_fcn()
pmic 39:f336caef17d9 397 {
pmic 39:f336caef17d9 398 user_button_timer.start();
pmic 39:f336caef17d9 399 user_button_timer.reset();
pmic 39:f336caef17d9 400 }
pmic 39:f336caef17d9 401
robleiker 47:6693bffcdfd0 402 static void user_button_released_fcn()
pmic 39:f336caef17d9 403 {
robleiker 52:75927464bf9c 404 // read timer and toggle do_execute_main_task if the button was pressed longer than the below specified time
pmic 39:f336caef17d9 405 int user_button_elapsed_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(user_button_timer.elapsed_time()).count();
pmic 39:f336caef17d9 406 user_button_timer.stop();
pmic 39:f336caef17d9 407 if (user_button_elapsed_time_ms > 200) {
robleiker 51:8c9f9b30dad0 408 robot_turned_on = !robot_turned_on;
pmic 39:f336caef17d9 409 }
robleiker 48:31ffd88e7f99 410 }
robleiker 48:31ffd88e7f99 411
robleiker 48:31ffd88e7f99 412 void main_task_trigger()
robleiker 48:31ffd88e7f99 413 {
robleiker 49:f80f5d96716e 414 // set the trigger to resume the waiting main task
robleiker 48:31ffd88e7f99 415 event_flags.set(main_task_flag);
pmic 39:f336caef17d9 416 }