Michael Ernst Peter / Mbed OS PM2_Example_ROME2_Robots

Example project

Dependencies:   PM2_Libary Eigen

main.cpp@46:41c9367da539, 2022-05-16 (annotated)

Committer:
pmic
Date:
Mon May 16 11:07:26 2022 +0200
Revision:
46:41c9367da539
Parent:
45:d9e6e89210f9
Child:
47:6693bffcdfd0
Introduced Motion in main ->this might be usefull to show the students

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
pmic 40:924bdbc33391 10 # define M_PI 3.14159265358979323846 // number pi
pmic 40:924bdbc33391 11
pmic 42:d2d2db5974c9 12 /**
pmic 42:d2d2db5974c9 13 * notes:
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
pmic 39:f336caef17d9 18 // logical variable main task
pmic 39:f336caef17d9 19 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
pmic 39:f336caef17d9 20
pmic 39:f336caef17d9 21 // user button on nucleo board
pmic 39:f336caef17d9 22 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 23 InterruptIn user_button(PC_13); // create InterruptIn interface object to evaluate user button falling and rising edge (no blocking code in ISR)
pmic 39:f336caef17d9 24 void user_button_pressed_fcn(); // custom functions which gets executed when user button gets pressed and released, definition below
pmic 39:f336caef17d9 25 void user_button_released_fcn();
pmic 39:f336caef17d9 26
pmic 44:dd746bf0e81f 27 int main()
pmic 44:dd746bf0e81f 28 {
pmic 42:d2d2db5974c9 29 // while loop gets executed every main_task_period_ms milliseconds
pmic 42:d2d2db5974c9 30 const int main_task_period_ms = 10; // define main task period time in ms e.g. 50 ms -> main task runns 20 times per second
pmic 42:d2d2db5974c9 31 Timer main_task_timer; // create Timer object which we use to run the main task every main task period time in ms
pmic 39:f336caef17d9 32
pmic 42:d2d2db5974c9 33 // led on nucleo board
pmic 42:d2d2db5974c9 34 DigitalOut user_led(LED1); // create DigitalOut object to command user led
pmic 39:f336caef17d9 35
pmic 42:d2d2db5974c9 36 // discrete states of this state machine
pmic 42:d2d2db5974c9 37 const int ROBOT_OFF = 0;
pmic 42:d2d2db5974c9 38 const int MOVE_FORWARD = 1;
pmic 42:d2d2db5974c9 39 const int TURN_LEFT = 2;
pmic 42:d2d2db5974c9 40 const int TURN_RIGHT = 3;
pmic 42:d2d2db5974c9 41 const int SLOWING_DOWN = 4;
pmic 39:f336caef17d9 42
pmic 42:d2d2db5974c9 43 // default parameters for robots movement
pmic 42:d2d2db5974c9 44 const float DISTANCE_THRESHOLD = 0.2f; // minimum allowed distance to obstacle in [m]
pmic 42:d2d2db5974c9 45 const float TRANSLATIONAL_VELOCITY = 0.4f; // translational velocity in [m/s]
pmic 42:d2d2db5974c9 46 const float ROTATIONAL_VELOCITY = 1.6f; // rotational velocity in [rad/s]
pmic 42:d2d2db5974c9 47 const float VELOCITY_THRESHOLD = 0.05; // velocity threshold before switching off, in [m/s] and [rad/s]
pmic 42:d2d2db5974c9 48
pmic 42:d2d2db5974c9 49 // create DigitalOut objects for leds
pmic 42:d2d2db5974c9 50 DigitalOut led0(PC_8);
pmic 42:d2d2db5974c9 51 DigitalOut led1(PC_6);
pmic 42:d2d2db5974c9 52 DigitalOut led2(PB_12);
pmic 42:d2d2db5974c9 53 DigitalOut led3(PA_7);
pmic 42:d2d2db5974c9 54 DigitalOut led4(PC_0);
pmic 42:d2d2db5974c9 55 DigitalOut led5(PC_9);
pmic 42:d2d2db5974c9 56 std::vector<DigitalOut> leds = {led0, led1, led2, led3, led4, led5};
pmic 39:f336caef17d9 57
pmic 42:d2d2db5974c9 58 // create IR sensor objects
pmic 42:d2d2db5974c9 59 AnalogIn dist(PB_1);
pmic 42:d2d2db5974c9 60 DigitalOut enable_leds(PC_1);
pmic 42:d2d2db5974c9 61 DigitalOut bit0(PH_1);
pmic 42:d2d2db5974c9 62 DigitalOut bit1(PC_2);
pmic 42:d2d2db5974c9 63 DigitalOut bit2(PC_3);
pmic 42:d2d2db5974c9 64 IRSensor irSensor0(dist, bit0, bit1, bit2, 0);
pmic 42:d2d2db5974c9 65 IRSensor irSensor1(dist, bit0, bit1, bit2, 1);
pmic 42:d2d2db5974c9 66 IRSensor irSensor2(dist, bit0, bit1, bit2, 2);
pmic 42:d2d2db5974c9 67 IRSensor irSensor3(dist, bit0, bit1, bit2, 3);
pmic 42:d2d2db5974c9 68 IRSensor irSensor4(dist, bit0, bit1, bit2, 4);
pmic 42:d2d2db5974c9 69 IRSensor irSensor5(dist, bit0, bit1, bit2, 5);
pmic 42:d2d2db5974c9 70 std::vector<IRSensor> irSensors = {irSensor0, irSensor1, irSensor2, irSensor3, irSensor4, irSensor5};
pmic 39:f336caef17d9 71
pmic 42:d2d2db5974c9 72 // 19:1 Metal Gearmotor 37Dx68L mm 12V with 64 CPR Encoder (Helical Pinion)
pmic 42:d2d2db5974c9 73 DigitalOut enable_motors(PB_2);
pmic 42:d2d2db5974c9 74 DigitalIn motorDriverFault(PB_14);
pmic 42:d2d2db5974c9 75 DigitalIn motorDriverWarning(PB_15);
pmic 39:f336caef17d9 76
pmic 42:d2d2db5974c9 77 // create SpeedController objects
pmic 42:d2d2db5974c9 78 FastPWM pwm_M1(PA_9); // motor M1 is closed-loop speed controlled (angle velocity)
pmic 42:d2d2db5974c9 79 FastPWM pwm_M2(PA_8); // motor M2 is closed-loop speed controlled (angle velocity)
pmic 42:d2d2db5974c9 80 EncoderCounter encoder_M1(PA_6, PC_7); // create encoder objects to read in the encoder counter values
pmic 42:d2d2db5974c9 81 EncoderCounter encoder_M2(PB_6, PB_7);
pmic 42:d2d2db5974c9 82 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 83 const float counts_per_turn = 64.0f * 19.0f; // define counts per turn at gearbox end: counts/turn * gearratio
pmic 42:d2d2db5974c9 84 const float kn = 530.0f / 12.0f; // define motor constant in rpm per V
pmic 42:d2d2db5974c9 85
pmic 46:41c9367da539 86 // create Motion objects (trajectory planner)
pmic 46:41c9367da539 87 Motion* trajectoryPlaners[2];
pmic 46:41c9367da539 88 trajectoryPlaners[0] = new Motion;
pmic 46:41c9367da539 89 trajectoryPlaners[1] = new Motion;
pmic 46:41c9367da539 90 trajectoryPlaners[0]->setProfileVelocity(max_voltage * kn / 60.0f);
pmic 46:41c9367da539 91 trajectoryPlaners[1]->setProfileVelocity(max_voltage * kn / 60.0f);
pmic 46:41c9367da539 92 trajectoryPlaners[0]->setProfileAcceleration(10.0f);
pmic 46:41c9367da539 93 trajectoryPlaners[1]->setProfileAcceleration(10.0f);
pmic 46:41c9367da539 94 trajectoryPlaners[0]->setProfileDeceleration(10.0f);
pmic 46:41c9367da539 95 trajectoryPlaners[1]->setProfileDeceleration(10.0f);
pmic 46:41c9367da539 96
pmic 46:41c9367da539 97 // create SpeedController objects
pmic 42:d2d2db5974c9 98 SpeedController* speedControllers[2];
pmic 42:d2d2db5974c9 99 speedControllers[0] = new SpeedController(counts_per_turn, kn, max_voltage, pwm_M1, encoder_M1);
pmic 42:d2d2db5974c9 100 speedControllers[1] = new SpeedController(counts_per_turn, kn, max_voltage, pwm_M2, encoder_M2);
pmic 42:d2d2db5974c9 101 speedControllers[0]->setSpeedCntrlGain(0.04f); // adjust speedcontroller gains
pmic 42:d2d2db5974c9 102 speedControllers[1]->setSpeedCntrlGain(0.04f);
pmic 46:41c9367da539 103 //speedControllers[0]->setMaxAccelerationRPS(10.0f); // use this if you're not using the trajectoryPlaners
pmic 46:41c9367da539 104 //speedControllers[1]->setMaxAccelerationRPS(10.0f);
pmic 46:41c9367da539 105 speedControllers[0]->setMaxAccelerationRPS(999.0f); // adjust max. acceleration for smooth movement
pmic 46:41c9367da539 106 speedControllers[1]->setMaxAccelerationRPS(999.0f);
pmic 39:f336caef17d9 107
pmic 42:d2d2db5974c9 108 // robot kinematics
pmic 42:d2d2db5974c9 109 const float r_wheel = 0.0766f / 2.0f; // wheel radius
pmic 42:d2d2db5974c9 110 const float L_wheel = 0.176f; // distance from wheel to wheel
pmic 42:d2d2db5974c9 111 Eigen::Matrix2f Cwheel2robot; // transform wheel to robot
pmic 42:d2d2db5974c9 112 //Eigen::Matrix2f Crobot2wheel; // transform robot to wheel
pmic 42:d2d2db5974c9 113 Cwheel2robot << -r_wheel / 2.0f , r_wheel / 2.0f ,
pmic 42:d2d2db5974c9 114 -r_wheel / L_wheel, -r_wheel / L_wheel;
pmic 42:d2d2db5974c9 115 //Crobot2wheel << -1.0f / r_wheel, -L_wheel / (2.0f * r_wheel),
pmic 42:d2d2db5974c9 116 // 1.0f / r_wheel, -L_wheel / (2.0f * r_wheel);
pmic 46:41c9367da539 117 Eigen::Vector2f robot_coord; // contains v and w (robot translational and rotational velocities)
pmic 46:41c9367da539 118 Eigen::Vector2f wheel_speed; // w1 w2 (wheel speed)
pmic 46:41c9367da539 119 Eigen::Vector2f wheel_speed_smooth; // w1 w2 (wheel speed)
pmic 46:41c9367da539 120 Eigen::Vector2f robot_coord_actual;
pmic 42:d2d2db5974c9 121 Eigen::Vector2f wheel_speed_actual;
pmic 42:d2d2db5974c9 122 robot_coord.setZero();
pmic 42:d2d2db5974c9 123 wheel_speed.setZero();
pmic 46:41c9367da539 124 wheel_speed_smooth.setZero();
pmic 46:41c9367da539 125 robot_coord_actual.setZero();
pmic 42:d2d2db5974c9 126 wheel_speed_actual.setZero();
pmic 39:f336caef17d9 127
pmic 39:f336caef17d9 128 // attach button fall and rise functions to user button object
pmic 39:f336caef17d9 129 user_button.fall(&user_button_pressed_fcn);
pmic 39:f336caef17d9 130 user_button.rise(&user_button_released_fcn);
pmic 36:23addefb97af 131
pmic 39:f336caef17d9 132 // start timer
pmic 39:f336caef17d9 133 main_task_timer.start();
pmic 40:924bdbc33391 134
pmic 42:d2d2db5974c9 135 // set initial state machine state, enalbe leds, disable motors
pmic 42:d2d2db5974c9 136 int state = ROBOT_OFF;
pmic 42:d2d2db5974c9 137 enable_leds = 1;
pmic 42:d2d2db5974c9 138 enable_motors = 0;
pmic 36:23addefb97af 139
pmic 39:f336caef17d9 140 while (true) { // this loop will run forever
pmic 36:23addefb97af 141
pmic 39:f336caef17d9 142 main_task_timer.reset();
pmic 39:f336caef17d9 143
pmic 42:d2d2db5974c9 144 // set leds according to DISTANCE_THRESHOLD
pmic 42:d2d2db5974c9 145 for (uint8_t i = 0; i < leds.size(); i++) {
pmic 42:d2d2db5974c9 146 if (irSensors[i].read() > DISTANCE_THRESHOLD)
pmic 42:d2d2db5974c9 147 leds[i] = 0;
pmic 42:d2d2db5974c9 148 else
pmic 42:d2d2db5974c9 149 leds[i] = 1;
pmic 40:924bdbc33391 150 }
pmic 39:f336caef17d9 151
pmic 42:d2d2db5974c9 152 // read actual wheel speed and transform it to robot coordinates
pmic 42:d2d2db5974c9 153 wheel_speed_actual << speedControllers[0]->getSpeedRPS(), speedControllers[1]->getSpeedRPS();
pmic 42:d2d2db5974c9 154 robot_coord_actual = Cwheel2robot * wheel_speed_actual;
pmic 42:d2d2db5974c9 155
pmic 42:d2d2db5974c9 156 // state machine
pmic 39:f336caef17d9 157 switch (state) {
pmic 39:f336caef17d9 158
pmic 42:d2d2db5974c9 159 case ROBOT_OFF:
pmic 42:d2d2db5974c9 160
pmic 39:f336caef17d9 161 if (do_execute_main_task) {
pmic 42:d2d2db5974c9 162 enable_motors = 1;
pmic 42:d2d2db5974c9 163 robot_coord(0) = TRANSLATIONAL_VELOCITY;
pmic 42:d2d2db5974c9 164 robot_coord(1) = 0.0f;
pmic 39:f336caef17d9 165 state = MOVE_FORWARD;
pmic 39:f336caef17d9 166 }
pmic 39:f336caef17d9 167 break;
pmic 42:d2d2db5974c9 168
pmic 39:f336caef17d9 169 case MOVE_FORWARD:
pmic 42:d2d2db5974c9 170
pmic 39:f336caef17d9 171 if (!do_execute_main_task) {
pmic 42:d2d2db5974c9 172 robot_coord(0) = 0.0f;
pmic 42:d2d2db5974c9 173 robot_coord(1) = 0.0f;
pmic 39:f336caef17d9 174 state = SLOWING_DOWN;
pmic 42:d2d2db5974c9 175 } else if ((irSensors[0].read() < DISTANCE_THRESHOLD) || (irSensors[1].read() < DISTANCE_THRESHOLD)) {
pmic 42:d2d2db5974c9 176 robot_coord(0) = 0.0f;
pmic 42:d2d2db5974c9 177 robot_coord(1) = ROTATIONAL_VELOCITY;
pmic 42:d2d2db5974c9 178 state = TURN_LEFT;
pmic 42:d2d2db5974c9 179 } else if (irSensors[5].read() < DISTANCE_THRESHOLD) {
pmic 42:d2d2db5974c9 180 robot_coord(0) = 0.0f;
pmic 42:d2d2db5974c9 181 robot_coord(1) = -ROTATIONAL_VELOCITY;
pmic 39:f336caef17d9 182 state = TURN_RIGHT;
pmic 42:d2d2db5974c9 183 } else {
pmic 42:d2d2db5974c9 184 // leave it driving
pmic 39:f336caef17d9 185 }
pmic 39:f336caef17d9 186 break;
pmic 39:f336caef17d9 187
pmic 39:f336caef17d9 188 case TURN_LEFT:
pmic 42:d2d2db5974c9 189
pmic 39:f336caef17d9 190 if (!do_execute_main_task) {
pmic 42:d2d2db5974c9 191 robot_coord(1) = 0.0f;
pmic 39:f336caef17d9 192 state = SLOWING_DOWN;
pmic 39:f336caef17d9 193
pmic 42:d2d2db5974c9 194 } else if ((irSensors[0].read() > DISTANCE_THRESHOLD) && (irSensors[1].read() > DISTANCE_THRESHOLD) && (irSensors[5].read() > DISTANCE_THRESHOLD)) {
pmic 42:d2d2db5974c9 195 robot_coord(0) = TRANSLATIONAL_VELOCITY;
pmic 42:d2d2db5974c9 196 robot_coord(1) = 0.0f;
pmic 39:f336caef17d9 197 state = MOVE_FORWARD;
pmic 39:f336caef17d9 198 }
pmic 39:f336caef17d9 199 break;
pmic 39:f336caef17d9 200
pmic 42:d2d2db5974c9 201 case TURN_RIGHT:
pmic 42:d2d2db5974c9 202
pmic 39:f336caef17d9 203 if (!do_execute_main_task) {
pmic 42:d2d2db5974c9 204 robot_coord(1) = 0.0f;
pmic 39:f336caef17d9 205 state = SLOWING_DOWN;
pmic 42:d2d2db5974c9 206 } else if ((irSensors[0].read() > DISTANCE_THRESHOLD) && (irSensors[1].read() > DISTANCE_THRESHOLD) && (irSensors[5].read() > DISTANCE_THRESHOLD)) {
pmic 42:d2d2db5974c9 207 robot_coord(0) = TRANSLATIONAL_VELOCITY;
pmic 42:d2d2db5974c9 208 robot_coord(1) = 0.0f;
pmic 39:f336caef17d9 209 state = MOVE_FORWARD;
pmic 39:f336caef17d9 210 }
pmic 39:f336caef17d9 211 break;
pmic 39:f336caef17d9 212
pmic 39:f336caef17d9 213 case SLOWING_DOWN:
pmic 42:d2d2db5974c9 214
pmic 42:d2d2db5974c9 215 if ((fabs(robot_coord_actual(0)) < VELOCITY_THRESHOLD) && (fabs(robot_coord_actual(1)) < VELOCITY_THRESHOLD)) {
pmic 42:d2d2db5974c9 216 enable_motors = 0;
pmic 39:f336caef17d9 217 state = ROBOT_OFF;
pmic 39:f336caef17d9 218 }
pmic 39:f336caef17d9 219
pmic 39:f336caef17d9 220 break;
pmic 42:d2d2db5974c9 221
pmic 39:f336caef17d9 222 }
pmic 39:f336caef17d9 223
pmic 42:d2d2db5974c9 224 // transform robot coordinates to wheel speed
pmic 42:d2d2db5974c9 225 wheel_speed = Cwheel2robot.inverse() * robot_coord;
pmic 42:d2d2db5974c9 226
pmic 46:41c9367da539 227 // smooth desired wheel_speeds
pmic 46:41c9367da539 228 trajectoryPlaners[0]->incrementToVelocity(wheel_speed(0) / (2.0f * M_PI), static_cast<float>(main_task_period_ms) * 1.0e-3f);
pmic 46:41c9367da539 229 trajectoryPlaners[1]->incrementToVelocity(wheel_speed(1) / (2.0f * M_PI), static_cast<float>(main_task_period_ms) * 1.0e-3f);
pmic 46:41c9367da539 230 wheel_speed_smooth << trajectoryPlaners[0]->getVelocity(), trajectoryPlaners[1]->getVelocity();
pmic 46:41c9367da539 231
pmic 42:d2d2db5974c9 232 // command speedController objects
pmic 46:41c9367da539 233 //speedControllers[0]->setDesiredSpeedRPS(wheel_speed(0) / (2.0f * M_PI)); // use this if you're not using the trajectoryPlaners
pmic 46:41c9367da539 234 //speedControllers[1]->setDesiredSpeedRPS(wheel_speed(1) / (2.0f * M_PI)); // use this if you're not using the trajectoryPlaners
pmic 46:41c9367da539 235 speedControllers[0]->setDesiredSpeedRPS(wheel_speed_smooth(0)); // set a desired speed for speed controlled dc motors M1
pmic 46:41c9367da539 236 speedControllers[1]->setDesiredSpeedRPS(wheel_speed_smooth(1)); // set a desired speed for speed controlled dc motors M2
pmic 42:d2d2db5974c9 237
pmic 39:f336caef17d9 238 user_led = !user_led;
pmic 36:23addefb97af 239
pmic 42:d2d2db5974c9 240 // do only output via serial what's really necessary (this makes your code slow)
pmic 45:d9e6e89210f9 241 //printf("%f, %f\r\n", wheel_speed_actual(0), wheel_speed_actual(1));
pmic 36:23addefb97af 242
pmic 39:f336caef17d9 243 // read timer and make the main thread sleep for the remaining time span (non blocking)
pmic 42:d2d2db5974c9 244 int main_task_elapsed_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(main_task_timer.elapsed_time()).count();
pmic 39:f336caef17d9 245 thread_sleep_for(main_task_period_ms - main_task_elapsed_time_ms);
pmic 1:93d997d6b232 246 }
pmic 1:93d997d6b232 247 }
pmic 39:f336caef17d9 248
pmic 39:f336caef17d9 249 void user_button_pressed_fcn()
pmic 39:f336caef17d9 250 {
pmic 39:f336caef17d9 251 user_button_timer.start();
pmic 39:f336caef17d9 252 user_button_timer.reset();
pmic 39:f336caef17d9 253 }
pmic 39:f336caef17d9 254
pmic 39:f336caef17d9 255 void user_button_released_fcn()
pmic 39:f336caef17d9 256 {
pmic 39:f336caef17d9 257 // read timer and toggle do_execute_main_task if the button was pressed longer than the below specified time
pmic 39:f336caef17d9 258 int user_button_elapsed_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(user_button_timer.elapsed_time()).count();
pmic 39:f336caef17d9 259 user_button_timer.stop();
pmic 39:f336caef17d9 260 if (user_button_elapsed_time_ms > 200) {
pmic 39:f336caef17d9 261 do_execute_main_task = !do_execute_main_task;
pmic 39:f336caef17d9 262 }
pmic 39:f336caef17d9 263 }