Michael Ernst Peter / Mbed OS PM2_Example_ROME2_Robots

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?

UserRevisionLine numberNew 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 }