Michael Ernst Peter / Mbed OS PM2_Example_ROME2_Robots

Example project

Dependencies:   PM2_Libary Eigen

main.cpp@48:31ffd88e7f99, 2022-05-18 (annotated)

Committer:
robleiker
Date:
Wed May 18 22:22:31 2022 +0000
Revision:
48:31ffd88e7f99
Parent:
47:6693bffcdfd0
Child:
49:f80f5d96716e
ticker instead of wait

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
pmic 39:f336caef17d9 29 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 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 47:6693bffcdfd0 42 // default parameters for robots movement
robleiker 47:6693bffcdfd0 43 const float DISTANCE_THRESHOLD = 0.2f; // minimum allowed distance to obstacle in [m]
robleiker 47:6693bffcdfd0 44 const float TRANSLATIONAL_VELOCITY = 0.4f; // translational velocity in [m/s]
robleiker 47:6693bffcdfd0 45 const float ROTATIONAL_VELOCITY = 1.6f; // rotational velocity in [rad/s]
robleiker 47:6693bffcdfd0 46 const float VELOCITY_THRESHOLD = 0.05; // velocity threshold before switching off, in [m/s] and [rad/s]
pmic 39:f336caef17d9 47
robleiker 47:6693bffcdfd0 48 // discrete states of this state machine
robleiker 48:31ffd88e7f99 49 const int INIT = 0;
robleiker 48:31ffd88e7f99 50 const int ROBOT_OFF = 1;
robleiker 48:31ffd88e7f99 51 const int MOVE_FORWARD = 2;
robleiker 48:31ffd88e7f99 52 const int TURN_LEFT = 3;
robleiker 48:31ffd88e7f99 53 const int TURN_RIGHT = 4;
robleiker 48:31ffd88e7f99 54 const int SLOWING_DOWN = 5;
robleiker 47:6693bffcdfd0 55
robleiker 47:6693bffcdfd0 56 /* ------------------------------------------------------------------------------------------- *
robleiker 48:31ffd88e7f99 57 * -- Function Declarations
robleiker 47:6693bffcdfd0 58 * ------------------------------------------------------------------------------------------- */
robleiker 47:6693bffcdfd0 59 static void user_button_pressed_fcn(); // custom functions which gets executed when user button gets pressed and released, definition below
robleiker 47:6693bffcdfd0 60 static void user_button_released_fcn();
robleiker 47:6693bffcdfd0 61
robleiker 48:31ffd88e7f99 62 void main_task_trigger(); // triggers the main task each period
robleiker 48:31ffd88e7f99 63
robleiker 47:6693bffcdfd0 64 /* ------------------------------------------------------------------------------------------- *
robleiker 47:6693bffcdfd0 65 * -- Main
robleiker 47:6693bffcdfd0 66 * ------------------------------------------------------------------------------------------- */
pmic 44:dd746bf0e81f 67 int main()
pmic 44:dd746bf0e81f 68 {
robleiker 47:6693bffcdfd0 69 /* ------------------------------------------------------------------------------------------- *
robleiker 47:6693bffcdfd0 70 * -- Setup: I/O
robleiker 47:6693bffcdfd0 71 * ------------------------------------------------------------------------------------------- */
pmic 39:f336caef17d9 72
pmic 42:d2d2db5974c9 73 // led on nucleo board
pmic 42:d2d2db5974c9 74 DigitalOut user_led(LED1); // create DigitalOut object to command user led
pmic 39:f336caef17d9 75
pmic 42:d2d2db5974c9 76 // create DigitalOut objects for leds
robleiker 47:6693bffcdfd0 77 DigitalOut enable_leds(PC_1);
pmic 42:d2d2db5974c9 78 DigitalOut led0(PC_8);
pmic 42:d2d2db5974c9 79 DigitalOut led1(PC_6);
pmic 42:d2d2db5974c9 80 DigitalOut led2(PB_12);
pmic 42:d2d2db5974c9 81 DigitalOut led3(PA_7);
pmic 42:d2d2db5974c9 82 DigitalOut led4(PC_0);
pmic 42:d2d2db5974c9 83 DigitalOut led5(PC_9);
pmic 42:d2d2db5974c9 84 std::vector<DigitalOut> leds = {led0, led1, led2, led3, led4, led5};
pmic 39:f336caef17d9 85
pmic 42:d2d2db5974c9 86 // create IR sensor objects
pmic 42:d2d2db5974c9 87 AnalogIn dist(PB_1);
pmic 42:d2d2db5974c9 88 DigitalOut bit0(PH_1);
pmic 42:d2d2db5974c9 89 DigitalOut bit1(PC_2);
pmic 42:d2d2db5974c9 90 DigitalOut bit2(PC_3);
pmic 42:d2d2db5974c9 91 IRSensor irSensor0(dist, bit0, bit1, bit2, 0);
pmic 42:d2d2db5974c9 92 IRSensor irSensor1(dist, bit0, bit1, bit2, 1);
pmic 42:d2d2db5974c9 93 IRSensor irSensor2(dist, bit0, bit1, bit2, 2);
pmic 42:d2d2db5974c9 94 IRSensor irSensor3(dist, bit0, bit1, bit2, 3);
pmic 42:d2d2db5974c9 95 IRSensor irSensor4(dist, bit0, bit1, bit2, 4);
pmic 42:d2d2db5974c9 96 IRSensor irSensor5(dist, bit0, bit1, bit2, 5);
pmic 42:d2d2db5974c9 97 std::vector<IRSensor> irSensors = {irSensor0, irSensor1, irSensor2, irSensor3, irSensor4, irSensor5};
pmic 39:f336caef17d9 98
robleiker 47:6693bffcdfd0 99 // attach button fall and rise functions to user button object
robleiker 47:6693bffcdfd0 100 user_button.fall(&user_button_pressed_fcn);
robleiker 47:6693bffcdfd0 101 user_button.rise(&user_button_released_fcn);
robleiker 47:6693bffcdfd0 102
pmic 42:d2d2db5974c9 103 // 19:1 Metal Gearmotor 37Dx68L mm 12V with 64 CPR Encoder (Helical Pinion)
pmic 42:d2d2db5974c9 104 DigitalOut enable_motors(PB_2);
pmic 42:d2d2db5974c9 105 DigitalIn motorDriverFault(PB_14);
pmic 42:d2d2db5974c9 106 DigitalIn motorDriverWarning(PB_15);
pmic 39:f336caef17d9 107
robleiker 47:6693bffcdfd0 108 /* ------------------------------------------------------------------------------------------- *
robleiker 47:6693bffcdfd0 109 * -- Setup: Motion Controller
robleiker 47:6693bffcdfd0 110 * ------------------------------------------------------------------------------------------- */
robleiker 47:6693bffcdfd0 111
pmic 42:d2d2db5974c9 112 // create SpeedController objects
pmic 42:d2d2db5974c9 113 FastPWM pwm_M1(PA_9); // motor M1 is closed-loop speed controlled (angle velocity)
pmic 42:d2d2db5974c9 114 FastPWM pwm_M2(PA_8); // motor M2 is closed-loop speed controlled (angle velocity)
pmic 42:d2d2db5974c9 115 EncoderCounter encoder_M1(PA_6, PC_7); // create encoder objects to read in the encoder counter values
pmic 42:d2d2db5974c9 116 EncoderCounter encoder_M2(PB_6, PB_7);
pmic 42:d2d2db5974c9 117 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 118 const float counts_per_turn = 64.0f * 19.0f; // define counts per turn at gearbox end: counts/turn * gearratio
pmic 42:d2d2db5974c9 119 const float kn = 530.0f / 12.0f; // define motor constant in rpm per V
pmic 42:d2d2db5974c9 120
pmic 46:41c9367da539 121 // create Motion objects (trajectory planner)
pmic 46:41c9367da539 122 Motion* trajectoryPlaners[2];
pmic 46:41c9367da539 123 trajectoryPlaners[0] = new Motion;
pmic 46:41c9367da539 124 trajectoryPlaners[1] = new Motion;
pmic 46:41c9367da539 125 trajectoryPlaners[0]->setProfileVelocity(max_voltage * kn / 60.0f);
pmic 46:41c9367da539 126 trajectoryPlaners[1]->setProfileVelocity(max_voltage * kn / 60.0f);
pmic 46:41c9367da539 127 trajectoryPlaners[0]->setProfileAcceleration(10.0f);
pmic 46:41c9367da539 128 trajectoryPlaners[1]->setProfileAcceleration(10.0f);
pmic 46:41c9367da539 129 trajectoryPlaners[0]->setProfileDeceleration(10.0f);
pmic 46:41c9367da539 130 trajectoryPlaners[1]->setProfileDeceleration(10.0f);
pmic 46:41c9367da539 131
pmic 46:41c9367da539 132 // create SpeedController objects
pmic 42:d2d2db5974c9 133 SpeedController* speedControllers[2];
pmic 42:d2d2db5974c9 134 speedControllers[0] = new SpeedController(counts_per_turn, kn, max_voltage, pwm_M1, encoder_M1);
pmic 42:d2d2db5974c9 135 speedControllers[1] = new SpeedController(counts_per_turn, kn, max_voltage, pwm_M2, encoder_M2);
pmic 42:d2d2db5974c9 136 speedControllers[0]->setSpeedCntrlGain(0.04f); // adjust speedcontroller gains
pmic 42:d2d2db5974c9 137 speedControllers[1]->setSpeedCntrlGain(0.04f);
pmic 46:41c9367da539 138 //speedControllers[0]->setMaxAccelerationRPS(10.0f); // use this if you're not using the trajectoryPlaners
pmic 46:41c9367da539 139 //speedControllers[1]->setMaxAccelerationRPS(10.0f);
pmic 46:41c9367da539 140 speedControllers[0]->setMaxAccelerationRPS(999.0f); // adjust max. acceleration for smooth movement
pmic 46:41c9367da539 141 speedControllers[1]->setMaxAccelerationRPS(999.0f);
pmic 39:f336caef17d9 142
robleiker 47:6693bffcdfd0 143 /* ------------------------------------------------------------------------------------------- *
robleiker 47:6693bffcdfd0 144 * -- Setup: Robot Kinematics
robleiker 47:6693bffcdfd0 145 * ------------------------------------------------------------------------------------------- */
robleiker 47:6693bffcdfd0 146
pmic 42:d2d2db5974c9 147 // robot kinematics
pmic 42:d2d2db5974c9 148 const float r_wheel = 0.0766f / 2.0f; // wheel radius
pmic 42:d2d2db5974c9 149 const float L_wheel = 0.176f; // distance from wheel to wheel
pmic 42:d2d2db5974c9 150 Eigen::Matrix2f Cwheel2robot; // transform wheel to robot
pmic 42:d2d2db5974c9 151 //Eigen::Matrix2f Crobot2wheel; // transform robot to wheel
pmic 42:d2d2db5974c9 152 Cwheel2robot << -r_wheel / 2.0f , r_wheel / 2.0f ,
pmic 42:d2d2db5974c9 153 -r_wheel / L_wheel, -r_wheel / L_wheel;
pmic 42:d2d2db5974c9 154 //Crobot2wheel << -1.0f / r_wheel, -L_wheel / (2.0f * r_wheel),
pmic 42:d2d2db5974c9 155 // 1.0f / r_wheel, -L_wheel / (2.0f * r_wheel);
pmic 46:41c9367da539 156 Eigen::Vector2f robot_coord; // contains v and w (robot translational and rotational velocities)
pmic 46:41c9367da539 157 Eigen::Vector2f wheel_speed; // w1 w2 (wheel speed)
pmic 46:41c9367da539 158 Eigen::Vector2f wheel_speed_smooth; // w1 w2 (wheel speed)
pmic 46:41c9367da539 159 Eigen::Vector2f robot_coord_actual;
pmic 42:d2d2db5974c9 160 Eigen::Vector2f wheel_speed_actual;
pmic 42:d2d2db5974c9 161 robot_coord.setZero();
pmic 42:d2d2db5974c9 162 wheel_speed.setZero();
pmic 46:41c9367da539 163 wheel_speed_smooth.setZero();
pmic 46:41c9367da539 164 robot_coord_actual.setZero();
pmic 42:d2d2db5974c9 165 wheel_speed_actual.setZero();
pmic 39:f336caef17d9 166
robleiker 47:6693bffcdfd0 167 /* ------------------------------------------------------------------------------------------- *
robleiker 47:6693bffcdfd0 168 * -- Setup: State Machine
robleiker 47:6693bffcdfd0 169 * ------------------------------------------------------------------------------------------- */
robleiker 47:6693bffcdfd0 170
robleiker 47:6693bffcdfd0 171 // while loop gets executed every main_task_period_ms milliseconds
robleiker 48:31ffd88e7f99 172 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 173
pmic 36:23addefb97af 174
robleiker 48:31ffd88e7f99 175 Ticker ticker; // calls a fuction with a precise period
robleiker 48:31ffd88e7f99 176 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 177
pmic 42:d2d2db5974c9 178 // set initial state machine state, enalbe leds, disable motors
robleiker 48:31ffd88e7f99 179 int state = INIT;
pmic 36:23addefb97af 180
pmic 39:f336caef17d9 181 while (true) { // this loop will run forever
robleiker 48:31ffd88e7f99 182
robleiker 48:31ffd88e7f99 183 /* ------------------------------------------------------------------------------------------- *
robleiker 48:31ffd88e7f99 184 * -- Wait for the next Cycle
robleiker 48:31ffd88e7f99 185 * ------------------------------------------------------------------------------------------- */
robleiker 48:31ffd88e7f99 186 event_flags.wait_any(main_task_flag);
robleiker 48:31ffd88e7f99 187
pmic 36:23addefb97af 188
robleiker 48:31ffd88e7f99 189 /* ------------------------------------------------------------------------------------------- *
robleiker 48:31ffd88e7f99 190 * -- Read Sensors
robleiker 48:31ffd88e7f99 191 * ------------------------------------------------------------------------------------------- */
pmic 39:f336caef17d9 192
pmic 42:d2d2db5974c9 193 // set leds according to DISTANCE_THRESHOLD
pmic 42:d2d2db5974c9 194 for (uint8_t i = 0; i < leds.size(); i++) {
pmic 42:d2d2db5974c9 195 if (irSensors[i].read() > DISTANCE_THRESHOLD)
pmic 42:d2d2db5974c9 196 leds[i] = 0;
pmic 42:d2d2db5974c9 197 else
pmic 42:d2d2db5974c9 198 leds[i] = 1;
pmic 40:924bdbc33391 199 }
pmic 39:f336caef17d9 200
pmic 42:d2d2db5974c9 201 // read actual wheel speed and transform it to robot coordinates
pmic 42:d2d2db5974c9 202 wheel_speed_actual << speedControllers[0]->getSpeedRPS(), speedControllers[1]->getSpeedRPS();
pmic 42:d2d2db5974c9 203 robot_coord_actual = Cwheel2robot * wheel_speed_actual;
pmic 42:d2d2db5974c9 204
robleiker 48:31ffd88e7f99 205
robleiker 48:31ffd88e7f99 206 /* ------------------------------------------------------------------------------------------- *
robleiker 48:31ffd88e7f99 207 * -- State Machine
robleiker 48:31ffd88e7f99 208 * ------------------------------------------------------------------------------------------- */
robleiker 48:31ffd88e7f99 209
pmic 42:d2d2db5974c9 210 // state machine
pmic 39:f336caef17d9 211 switch (state) {
robleiker 48:31ffd88e7f99 212
robleiker 48:31ffd88e7f99 213 case INIT:
robleiker 48:31ffd88e7f99 214
robleiker 48:31ffd88e7f99 215 enable_leds = 1;
robleiker 48:31ffd88e7f99 216 enable_motors = 0;
robleiker 48:31ffd88e7f99 217
robleiker 48:31ffd88e7f99 218 state = ROBOT_OFF; // default transition
robleiker 48:31ffd88e7f99 219 break;
pmic 39:f336caef17d9 220
pmic 42:d2d2db5974c9 221 case ROBOT_OFF:
pmic 42:d2d2db5974c9 222
pmic 39:f336caef17d9 223 if (do_execute_main_task) {
pmic 42:d2d2db5974c9 224 enable_motors = 1;
pmic 42:d2d2db5974c9 225 robot_coord(0) = TRANSLATIONAL_VELOCITY;
pmic 42:d2d2db5974c9 226 robot_coord(1) = 0.0f;
pmic 39:f336caef17d9 227 state = MOVE_FORWARD;
pmic 39:f336caef17d9 228 }
pmic 39:f336caef17d9 229 break;
pmic 42:d2d2db5974c9 230
pmic 39:f336caef17d9 231 case MOVE_FORWARD:
pmic 42:d2d2db5974c9 232
pmic 39:f336caef17d9 233 if (!do_execute_main_task) {
pmic 42:d2d2db5974c9 234 robot_coord(0) = 0.0f;
pmic 42:d2d2db5974c9 235 robot_coord(1) = 0.0f;
pmic 39:f336caef17d9 236 state = SLOWING_DOWN;
pmic 42:d2d2db5974c9 237 } else if ((irSensors[0].read() < DISTANCE_THRESHOLD) || (irSensors[1].read() < DISTANCE_THRESHOLD)) {
pmic 42:d2d2db5974c9 238 robot_coord(0) = 0.0f;
pmic 42:d2d2db5974c9 239 robot_coord(1) = ROTATIONAL_VELOCITY;
pmic 42:d2d2db5974c9 240 state = TURN_LEFT;
pmic 42:d2d2db5974c9 241 } else if (irSensors[5].read() < DISTANCE_THRESHOLD) {
pmic 42:d2d2db5974c9 242 robot_coord(0) = 0.0f;
pmic 42:d2d2db5974c9 243 robot_coord(1) = -ROTATIONAL_VELOCITY;
pmic 39:f336caef17d9 244 state = TURN_RIGHT;
pmic 42:d2d2db5974c9 245 } else {
pmic 42:d2d2db5974c9 246 // leave it driving
pmic 39:f336caef17d9 247 }
pmic 39:f336caef17d9 248 break;
pmic 39:f336caef17d9 249
pmic 39:f336caef17d9 250 case TURN_LEFT:
pmic 42:d2d2db5974c9 251
pmic 39:f336caef17d9 252 if (!do_execute_main_task) {
pmic 42:d2d2db5974c9 253 robot_coord(1) = 0.0f;
pmic 39:f336caef17d9 254 state = SLOWING_DOWN;
pmic 39:f336caef17d9 255
pmic 42:d2d2db5974c9 256 } else if ((irSensors[0].read() > DISTANCE_THRESHOLD) && (irSensors[1].read() > DISTANCE_THRESHOLD) && (irSensors[5].read() > DISTANCE_THRESHOLD)) {
pmic 42:d2d2db5974c9 257 robot_coord(0) = TRANSLATIONAL_VELOCITY;
pmic 42:d2d2db5974c9 258 robot_coord(1) = 0.0f;
pmic 39:f336caef17d9 259 state = MOVE_FORWARD;
pmic 39:f336caef17d9 260 }
pmic 39:f336caef17d9 261 break;
pmic 39:f336caef17d9 262
pmic 42:d2d2db5974c9 263 case TURN_RIGHT:
pmic 42:d2d2db5974c9 264
pmic 39:f336caef17d9 265 if (!do_execute_main_task) {
pmic 42:d2d2db5974c9 266 robot_coord(1) = 0.0f;
pmic 39:f336caef17d9 267 state = SLOWING_DOWN;
pmic 42:d2d2db5974c9 268 } else if ((irSensors[0].read() > DISTANCE_THRESHOLD) && (irSensors[1].read() > DISTANCE_THRESHOLD) && (irSensors[5].read() > DISTANCE_THRESHOLD)) {
pmic 42:d2d2db5974c9 269 robot_coord(0) = TRANSLATIONAL_VELOCITY;
pmic 42:d2d2db5974c9 270 robot_coord(1) = 0.0f;
pmic 39:f336caef17d9 271 state = MOVE_FORWARD;
pmic 39:f336caef17d9 272 }
pmic 39:f336caef17d9 273 break;
pmic 39:f336caef17d9 274
pmic 39:f336caef17d9 275 case SLOWING_DOWN:
pmic 42:d2d2db5974c9 276
pmic 42:d2d2db5974c9 277 if ((fabs(robot_coord_actual(0)) < VELOCITY_THRESHOLD) && (fabs(robot_coord_actual(1)) < VELOCITY_THRESHOLD)) {
pmic 42:d2d2db5974c9 278 enable_motors = 0;
pmic 39:f336caef17d9 279 state = ROBOT_OFF;
pmic 39:f336caef17d9 280 }
pmic 39:f336caef17d9 281
pmic 39:f336caef17d9 282 break;
pmic 42:d2d2db5974c9 283
pmic 39:f336caef17d9 284 }
pmic 39:f336caef17d9 285
robleiker 47:6693bffcdfd0 286 /* ------------------------------------------------------------------------------------------- *
robleiker 47:6693bffcdfd0 287 * -- Inverse Kinematics
robleiker 47:6693bffcdfd0 288 * ------------------------------------------------------------------------------------------- */
robleiker 47:6693bffcdfd0 289
pmic 42:d2d2db5974c9 290 // transform robot coordinates to wheel speed
pmic 42:d2d2db5974c9 291 wheel_speed = Cwheel2robot.inverse() * robot_coord;
pmic 42:d2d2db5974c9 292
pmic 46:41c9367da539 293 // smooth desired wheel_speeds
robleiker 48:31ffd88e7f99 294 trajectoryPlaners[0]->incrementToVelocity(wheel_speed(0) / (2.0f * M_PI), main_task_period);
robleiker 48:31ffd88e7f99 295 trajectoryPlaners[1]->incrementToVelocity(wheel_speed(1) / (2.0f * M_PI), main_task_period);
pmic 46:41c9367da539 296 wheel_speed_smooth << trajectoryPlaners[0]->getVelocity(), trajectoryPlaners[1]->getVelocity();
pmic 46:41c9367da539 297
pmic 42:d2d2db5974c9 298 // command speedController objects
pmic 46:41c9367da539 299 //speedControllers[0]->setDesiredSpeedRPS(wheel_speed(0) / (2.0f * M_PI)); // use this if you're not using the trajectoryPlaners
pmic 46:41c9367da539 300 //speedControllers[1]->setDesiredSpeedRPS(wheel_speed(1) / (2.0f * M_PI)); // use this if you're not using the trajectoryPlaners
pmic 46:41c9367da539 301 speedControllers[0]->setDesiredSpeedRPS(wheel_speed_smooth(0)); // set a desired speed for speed controlled dc motors M1
pmic 46:41c9367da539 302 speedControllers[1]->setDesiredSpeedRPS(wheel_speed_smooth(1)); // set a desired speed for speed controlled dc motors M2
pmic 42:d2d2db5974c9 303
pmic 39:f336caef17d9 304 user_led = !user_led;
robleiker 47:6693bffcdfd0 305
robleiker 47:6693bffcdfd0 306 /* ------------------------------------------------------------------------------------------- *
robleiker 47:6693bffcdfd0 307 * -- Printing to Console
robleiker 47:6693bffcdfd0 308 * ------------------------------------------------------------------------------------------- */
pmic 36:23addefb97af 309
pmic 42:d2d2db5974c9 310 // do only output via serial what's really necessary (this makes your code slow)
pmic 45:d9e6e89210f9 311 //printf("%f, %f\r\n", wheel_speed_actual(0), wheel_speed_actual(1));
pmic 1:93d997d6b232 312 }
pmic 1:93d997d6b232 313 }
pmic 39:f336caef17d9 314
robleiker 47:6693bffcdfd0 315 static void user_button_pressed_fcn()
pmic 39:f336caef17d9 316 {
pmic 39:f336caef17d9 317 user_button_timer.start();
pmic 39:f336caef17d9 318 user_button_timer.reset();
pmic 39:f336caef17d9 319 }
pmic 39:f336caef17d9 320
robleiker 47:6693bffcdfd0 321 static void user_button_released_fcn()
pmic 39:f336caef17d9 322 {
pmic 39:f336caef17d9 323 // read timer and toggle do_execute_main_task if the button was pressed longer than the below specified time
pmic 39:f336caef17d9 324 int user_button_elapsed_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(user_button_timer.elapsed_time()).count();
pmic 39:f336caef17d9 325 user_button_timer.stop();
pmic 39:f336caef17d9 326 if (user_button_elapsed_time_ms > 200) {
pmic 39:f336caef17d9 327 do_execute_main_task = !do_execute_main_task;
pmic 39:f336caef17d9 328 }
robleiker 48:31ffd88e7f99 329 }
robleiker 48:31ffd88e7f99 330
robleiker 48:31ffd88e7f99 331 void main_task_trigger()
robleiker 48:31ffd88e7f99 332 {
robleiker 48:31ffd88e7f99 333 event_flags.set(main_task_flag);
pmic 39:f336caef17d9 334 }