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?
User | Revision | Line number | New 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 | } |