Example project
Dependencies: PM2_Libary Eigen
main.cpp@39:f336caef17d9, 2022-05-10 (annotated)
- Committer:
- pmic
- Date:
- Tue May 10 14:11:06 2022 +0200
- Revision:
- 39:f336caef17d9
- Parent:
- 36:23addefb97af
- Child:
- 40:924bdbc33391
Version without StateMachine
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
pmic | 36:23addefb97af | 1 | #include <stdio.h> |
pmic | 36:23addefb97af | 2 | #include <mbed.h> |
pmic | 36:23addefb97af | 3 | #include "IRSensor.h" |
pmic | 36:23addefb97af | 4 | #include "EncoderCounterROME2.h" |
pmic | 36:23addefb97af | 5 | #include "Controller.h" |
pmic | 39:f336caef17d9 | 6 | |
pmic | 39:f336caef17d9 | 7 | // logical variable main task |
pmic | 39:f336caef17d9 | 8 | 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 | 9 | |
pmic | 39:f336caef17d9 | 10 | // user button on nucleo board |
pmic | 39:f336caef17d9 | 11 | 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 | 39:f336caef17d9 | 12 | InterruptIn user_button(BUTTON1); // create InterruptIn interface object to evaluate user button falling and rising edge (no blocking code in ISR) |
pmic | 39:f336caef17d9 | 13 | void user_button_pressed_fcn(); // custom functions which gets executed when user button gets pressed and released, definition below |
pmic | 39:f336caef17d9 | 14 | void user_button_released_fcn(); |
pmic | 39:f336caef17d9 | 15 | |
pmic | 39:f336caef17d9 | 16 | // while loop gets executed every main_task_period_ms milliseconds |
pmic | 39:f336caef17d9 | 17 | int main_task_period_ms = 50; // define main task period time in ms e.g. 50 ms -> main task runns 20 times per second |
pmic | 39:f336caef17d9 | 18 | 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 | 19 | |
pmic | 39:f336caef17d9 | 20 | // led on nucleo board |
pmic | 39:f336caef17d9 | 21 | DigitalOut user_led(LED1); // create DigitalOut object to command user led |
pmic | 39:f336caef17d9 | 22 | |
pmic | 39:f336caef17d9 | 23 | static const int ROBOT_OFF = 0; // discrete states of this state machine |
pmic | 39:f336caef17d9 | 24 | static const int MOVE_FORWARD = 1; |
pmic | 39:f336caef17d9 | 25 | static const int TURN_LEFT = 2; |
pmic | 39:f336caef17d9 | 26 | static const int TURN_RIGHT = 3; |
pmic | 39:f336caef17d9 | 27 | static const int SLOWING_DOWN = 4; |
pmic | 39:f336caef17d9 | 28 | |
pmic | 39:f336caef17d9 | 29 | const float DISTANCE_THRESHOLD = 0.4f; // minimum allowed distance to obstacle in [m] |
pmic | 39:f336caef17d9 | 30 | const float TRANSLATIONAL_VELOCITY = 1.0f; // translational velocity in [m/s] |
pmic | 39:f336caef17d9 | 31 | const float ROTATIONAL_VELOCITY = 1.0f; // rotational velocity in [rad/s] |
pmic | 39:f336caef17d9 | 32 | const float VELOCITY_THRESHOLD = 0.01; // velocity threshold before switching off, in [m/s] and [rad/s] |
pmic | 39:f336caef17d9 | 33 | |
pmic | 39:f336caef17d9 | 34 | DigitalOut led0(PD_4); |
pmic | 39:f336caef17d9 | 35 | DigitalOut led1(PD_3); |
pmic | 39:f336caef17d9 | 36 | DigitalOut led2(PD_6); |
pmic | 39:f336caef17d9 | 37 | DigitalOut led3(PD_2); |
pmic | 39:f336caef17d9 | 38 | DigitalOut led4(PD_7); |
pmic | 39:f336caef17d9 | 39 | DigitalOut led5(PD_5); |
pmic | 39:f336caef17d9 | 40 | |
pmic | 39:f336caef17d9 | 41 | // create IR sensor objects |
pmic | 39:f336caef17d9 | 42 | AnalogIn dist(PA_0); |
pmic | 39:f336caef17d9 | 43 | DigitalOut enable(PG_1); |
pmic | 39:f336caef17d9 | 44 | DigitalOut bit0(PF_0); |
pmic | 39:f336caef17d9 | 45 | DigitalOut bit1(PF_1); |
pmic | 39:f336caef17d9 | 46 | DigitalOut bit2(PF_2); |
pmic | 39:f336caef17d9 | 47 | |
pmic | 39:f336caef17d9 | 48 | IRSensor irSensor0(dist, bit0, bit1, bit2, 0); |
pmic | 39:f336caef17d9 | 49 | IRSensor irSensor1(dist, bit0, bit1, bit2, 1); |
pmic | 39:f336caef17d9 | 50 | IRSensor irSensor2(dist, bit0, bit1, bit2, 2); |
pmic | 39:f336caef17d9 | 51 | IRSensor irSensor3(dist, bit0, bit1, bit2, 3); |
pmic | 39:f336caef17d9 | 52 | IRSensor irSensor4(dist, bit0, bit1, bit2, 4); |
pmic | 39:f336caef17d9 | 53 | IRSensor irSensor5(dist, bit0, bit1, bit2, 5); |
pmic | 39:f336caef17d9 | 54 | |
pmic | 39:f336caef17d9 | 55 | // create motor control objects |
pmic | 39:f336caef17d9 | 56 | DigitalOut enableMotorDriver(PG_0); |
pmic | 39:f336caef17d9 | 57 | DigitalIn motorDriverFault(PD_1); |
pmic | 39:f336caef17d9 | 58 | DigitalIn motorDriverWarning(PD_0); |
pmic | 39:f336caef17d9 | 59 | PwmOut pwmLeft(PF_9); |
pmic | 39:f336caef17d9 | 60 | PwmOut pwmRight(PF_8); |
pmic | 39:f336caef17d9 | 61 | EncoderCounterROME2 counterLeft(PD_12, PD_13); |
pmic | 39:f336caef17d9 | 62 | EncoderCounterROME2 counterRight(PB_4, PC_7); |
pmic | 39:f336caef17d9 | 63 | |
pmic | 39:f336caef17d9 | 64 | int state; |
pmic | 39:f336caef17d9 | 65 | // create robot controller objects |
pmic | 39:f336caef17d9 | 66 | Controller controller(pwmLeft, pwmRight, counterLeft, counterRight); |
pmic | 39:f336caef17d9 | 67 | // StateMachine stateMachine(controller, enableMotorDriver, led0, led1, led2, led3, led4, led5, button, irSensor0, irSensor1, irSensor2, irSensor3, irSensor4, irSensor5); |
pmic | 17:c19b471f05cb | 68 | |
pmic | 36:23addefb97af | 69 | int main() { |
pmic | 39:f336caef17d9 | 70 | |
pmic | 39:f336caef17d9 | 71 | // attach button fall and rise functions to user button object |
pmic | 39:f336caef17d9 | 72 | user_button.fall(&user_button_pressed_fcn); |
pmic | 39:f336caef17d9 | 73 | user_button.rise(&user_button_released_fcn); |
pmic | 36:23addefb97af | 74 | |
pmic | 39:f336caef17d9 | 75 | // start timer |
pmic | 39:f336caef17d9 | 76 | main_task_timer.start(); |
pmic | 39:f336caef17d9 | 77 | |
pmic | 36:23addefb97af | 78 | enable = 1; |
pmic | 36:23addefb97af | 79 | |
pmic | 39:f336caef17d9 | 80 | enableMotorDriver = 0; |
pmic | 39:f336caef17d9 | 81 | state = ROBOT_OFF; |
pmic | 36:23addefb97af | 82 | |
pmic | 39:f336caef17d9 | 83 | while (true) { // this loop will run forever |
pmic | 36:23addefb97af | 84 | |
pmic | 39:f336caef17d9 | 85 | main_task_timer.reset(); |
pmic | 39:f336caef17d9 | 86 | |
pmic | 39:f336caef17d9 | 87 | // set the leds based on distance measurements |
pmic | 39:f336caef17d9 | 88 | led0 = irSensor0 < DISTANCE_THRESHOLD; |
pmic | 39:f336caef17d9 | 89 | led1 = irSensor1 < DISTANCE_THRESHOLD; |
pmic | 39:f336caef17d9 | 90 | led2 = irSensor2 < DISTANCE_THRESHOLD; |
pmic | 39:f336caef17d9 | 91 | led3 = irSensor3 < DISTANCE_THRESHOLD; |
pmic | 39:f336caef17d9 | 92 | led4 = irSensor4 < DISTANCE_THRESHOLD; |
pmic | 39:f336caef17d9 | 93 | led5 = irSensor5 < DISTANCE_THRESHOLD; |
pmic | 39:f336caef17d9 | 94 | |
pmic | 39:f336caef17d9 | 95 | switch (state) { |
pmic | 39:f336caef17d9 | 96 | |
pmic | 39:f336caef17d9 | 97 | // enables Motors, sets translational speed and switches to MOVE_FORWARD |
pmic | 39:f336caef17d9 | 98 | case ROBOT_OFF: |
pmic | 39:f336caef17d9 | 99 | printf(" ROBOT_OFF\r\n"); |
pmic | 39:f336caef17d9 | 100 | if (do_execute_main_task) { |
pmic | 39:f336caef17d9 | 101 | enableMotorDriver = 1; |
pmic | 39:f336caef17d9 | 102 | controller.setTranslationalVelocity(TRANSLATIONAL_VELOCITY); |
pmic | 39:f336caef17d9 | 103 | controller.setRotationalVelocity(0.0f); |
pmic | 39:f336caef17d9 | 104 | state = MOVE_FORWARD; |
pmic | 39:f336caef17d9 | 105 | } |
pmic | 39:f336caef17d9 | 106 | break; |
pmic | 39:f336caef17d9 | 107 | |
pmic | 39:f336caef17d9 | 108 | // continue here |
pmic | 39:f336caef17d9 | 109 | case MOVE_FORWARD: |
pmic | 39:f336caef17d9 | 110 | if (!do_execute_main_task) { |
pmic | 39:f336caef17d9 | 111 | controller.setTranslationalVelocity(0); |
pmic | 39:f336caef17d9 | 112 | controller.setRotationalVelocity(0); |
pmic | 39:f336caef17d9 | 113 | state = SLOWING_DOWN; |
pmic | 39:f336caef17d9 | 114 | printf("state = SLOWING_DOWN\r\n"); |
pmic | 39:f336caef17d9 | 115 | break; |
pmic | 39:f336caef17d9 | 116 | } |
pmic | 39:f336caef17d9 | 117 | |
pmic | 39:f336caef17d9 | 118 | if (irSensor2 < DISTANCE_THRESHOLD && irSensor2 < DISTANCE_THRESHOLD) { |
pmic | 39:f336caef17d9 | 119 | controller.setTranslationalVelocity(-TRANSLATIONAL_VELOCITY/2); |
pmic | 39:f336caef17d9 | 120 | controller.setRotationalVelocity(-ROTATIONAL_VELOCITY); |
pmic | 39:f336caef17d9 | 121 | state = TURN_RIGHT; |
pmic | 39:f336caef17d9 | 122 | printf("state = TURN_RIGHT\r\n"); |
pmic | 39:f336caef17d9 | 123 | break; |
pmic | 39:f336caef17d9 | 124 | } else if (irSensor4 < DISTANCE_THRESHOLD && irSensor4 < DISTANCE_THRESHOLD) { |
pmic | 39:f336caef17d9 | 125 | controller.setTranslationalVelocity(-TRANSLATIONAL_VELOCITY/2); |
pmic | 39:f336caef17d9 | 126 | controller.setRotationalVelocity(ROTATIONAL_VELOCITY); |
pmic | 39:f336caef17d9 | 127 | state = TURN_LEFT; |
pmic | 39:f336caef17d9 | 128 | printf("state = TURN_LEFT\r\n"); |
pmic | 39:f336caef17d9 | 129 | break; |
pmic | 39:f336caef17d9 | 130 | } else if (irSensor3 < DISTANCE_THRESHOLD/2) { |
pmic | 39:f336caef17d9 | 131 | controller.setTranslationalVelocity(-TRANSLATIONAL_VELOCITY); |
pmic | 39:f336caef17d9 | 132 | controller.setRotationalVelocity(-ROTATIONAL_VELOCITY); |
pmic | 39:f336caef17d9 | 133 | state = TURN_RIGHT; |
pmic | 39:f336caef17d9 | 134 | printf("state = TURN_RIGHT\r\n"); |
pmic | 39:f336caef17d9 | 135 | break; |
pmic | 39:f336caef17d9 | 136 | } |
pmic | 39:f336caef17d9 | 137 | |
pmic | 39:f336caef17d9 | 138 | break; |
pmic | 39:f336caef17d9 | 139 | |
pmic | 39:f336caef17d9 | 140 | case TURN_LEFT: |
pmic | 39:f336caef17d9 | 141 | if (!do_execute_main_task) { |
pmic | 39:f336caef17d9 | 142 | controller.setTranslationalVelocity(0); |
pmic | 39:f336caef17d9 | 143 | controller.setRotationalVelocity(0); |
pmic | 39:f336caef17d9 | 144 | state = SLOWING_DOWN; |
pmic | 39:f336caef17d9 | 145 | break; |
pmic | 39:f336caef17d9 | 146 | } |
pmic | 39:f336caef17d9 | 147 | |
pmic | 39:f336caef17d9 | 148 | if ( (irSensor2 > DISTANCE_THRESHOLD) && (irSensor3 > DISTANCE_THRESHOLD) && (irSensor4 > DISTANCE_THRESHOLD) ) { |
pmic | 39:f336caef17d9 | 149 | controller.setRotationalVelocity(0); |
pmic | 39:f336caef17d9 | 150 | controller.setTranslationalVelocity(TRANSLATIONAL_VELOCITY); |
pmic | 39:f336caef17d9 | 151 | state = MOVE_FORWARD; |
pmic | 39:f336caef17d9 | 152 | break; |
pmic | 39:f336caef17d9 | 153 | } |
pmic | 39:f336caef17d9 | 154 | break; |
pmic | 39:f336caef17d9 | 155 | |
pmic | 39:f336caef17d9 | 156 | case TURN_RIGHT: |
pmic | 39:f336caef17d9 | 157 | if (!do_execute_main_task) { |
pmic | 39:f336caef17d9 | 158 | controller.setTranslationalVelocity(0); |
pmic | 39:f336caef17d9 | 159 | controller.setRotationalVelocity(0); |
pmic | 39:f336caef17d9 | 160 | state = SLOWING_DOWN; |
pmic | 39:f336caef17d9 | 161 | break; |
pmic | 39:f336caef17d9 | 162 | } |
pmic | 39:f336caef17d9 | 163 | |
pmic | 39:f336caef17d9 | 164 | if ( (irSensor2 > DISTANCE_THRESHOLD) && (irSensor3 > DISTANCE_THRESHOLD) && (irSensor4 > DISTANCE_THRESHOLD) ) { |
pmic | 39:f336caef17d9 | 165 | controller.setRotationalVelocity(0); |
pmic | 39:f336caef17d9 | 166 | controller.setTranslationalVelocity(TRANSLATIONAL_VELOCITY); |
pmic | 39:f336caef17d9 | 167 | state = MOVE_FORWARD; |
pmic | 39:f336caef17d9 | 168 | break; |
pmic | 39:f336caef17d9 | 169 | } |
pmic | 39:f336caef17d9 | 170 | break; |
pmic | 39:f336caef17d9 | 171 | |
pmic | 39:f336caef17d9 | 172 | case SLOWING_DOWN: |
pmic | 39:f336caef17d9 | 173 | if (abs(controller.getActualTranslationalVelocity()) < VELOCITY_THRESHOLD |
pmic | 39:f336caef17d9 | 174 | && abs(controller.getActualRotationalVelocity()) > VELOCITY_THRESHOLD) { |
pmic | 39:f336caef17d9 | 175 | state = ROBOT_OFF; |
pmic | 39:f336caef17d9 | 176 | enableMotorDriver = 0; |
pmic | 39:f336caef17d9 | 177 | state = ROBOT_OFF; |
pmic | 39:f336caef17d9 | 178 | } |
pmic | 39:f336caef17d9 | 179 | |
pmic | 39:f336caef17d9 | 180 | break; |
pmic | 39:f336caef17d9 | 181 | |
pmic | 39:f336caef17d9 | 182 | default: |
pmic | 39:f336caef17d9 | 183 | |
pmic | 39:f336caef17d9 | 184 | state = ROBOT_OFF; |
pmic | 39:f336caef17d9 | 185 | } |
pmic | 39:f336caef17d9 | 186 | |
pmic | 39:f336caef17d9 | 187 | user_led = !user_led; |
pmic | 36:23addefb97af | 188 | |
pmic | 39:f336caef17d9 | 189 | /* |
pmic | 39:f336caef17d9 | 190 | if (do_execute_main_task) |
pmic | 39:f336caef17d9 | 191 | printf("button pressed\r\n"); |
pmic | 39:f336caef17d9 | 192 | else |
pmic | 39:f336caef17d9 | 193 | printf("button NOT pressed\r\n"); |
pmic | 39:f336caef17d9 | 194 | */ |
pmic | 36:23addefb97af | 195 | printf("actual velocity: %.3f [m/s] / %.3f [rad/s]\r\n", controller.getActualTranslationalVelocity(), controller.getActualRotationalVelocity()); |
pmic | 36:23addefb97af | 196 | |
pmic | 39:f336caef17d9 | 197 | // read timer and make the main thread sleep for the remaining time span (non blocking) |
pmic | 39:f336caef17d9 | 198 | int main_task_elapsed_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(main_task_timer.elapsed_time()).count(); |
pmic | 39:f336caef17d9 | 199 | thread_sleep_for(main_task_period_ms - main_task_elapsed_time_ms); |
pmic | 1:93d997d6b232 | 200 | } |
pmic | 1:93d997d6b232 | 201 | } |
pmic | 39:f336caef17d9 | 202 | |
pmic | 39:f336caef17d9 | 203 | void user_button_pressed_fcn() |
pmic | 39:f336caef17d9 | 204 | { |
pmic | 39:f336caef17d9 | 205 | user_button_timer.start(); |
pmic | 39:f336caef17d9 | 206 | user_button_timer.reset(); |
pmic | 39:f336caef17d9 | 207 | } |
pmic | 39:f336caef17d9 | 208 | |
pmic | 39:f336caef17d9 | 209 | void user_button_released_fcn() |
pmic | 39:f336caef17d9 | 210 | { |
pmic | 39:f336caef17d9 | 211 | // read timer and toggle do_execute_main_task if the button was pressed longer than the below specified time |
pmic | 39:f336caef17d9 | 212 | int user_button_elapsed_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(user_button_timer.elapsed_time()).count(); |
pmic | 39:f336caef17d9 | 213 | user_button_timer.stop(); |
pmic | 39:f336caef17d9 | 214 | if (user_button_elapsed_time_ms > 200) { |
pmic | 39:f336caef17d9 | 215 | do_execute_main_task = !do_execute_main_task; |
pmic | 39:f336caef17d9 | 216 | } |
pmic | 39:f336caef17d9 | 217 | } |