Michael Ernst Peter / Mbed OS PM2_Example_ROME2_Robots

Example project

Dependencies:   PM2_Libary Eigen

main.cpp@41:7484471403aa, 2022-05-11 (annotated)

Committer:
pmic
Date:
Wed May 11 09:44:25 2022 +0200
Revision:
41:7484471403aa
Parent:
40:924bdbc33391
Child:
42:d2d2db5974c9
Delete StateMachine

Who changed what in which revision?

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