Michael Ernst Peter / Mbed OS PM2_Example_Line_Follower

Example project for the Line Follower robot.

Dependencies:   PM2_Libary Eigen

main.cpp@33:cff70742569d, 2022-05-05 (annotated)

Committer:
pmic
Date:
Thu May 05 07:41:46 2022 +0000
Revision:
33:cff70742569d
Parent:
31:1b2a1bd1bccb
Child:
34:702246639f02
First commit line follower project

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pmic 33:cff70742569d 1 #include <mbed.h>
pmic 33:cff70742569d 2 #include <math.h>
pmic 33:cff70742569d 3
pmic 17:c19b471f05cb 4 #include "PM2_Libary.h"
pmic 6:e1fa1a2d7483 5
pmic 33:cff70742569d 6 # define M_PI 3.14159265358979323846 /* pi */
pmic 33:cff70742569d 7
pmic 24:86f1a63e35a0 8 // logical variable main task
pmic 24:86f1a63e35a0 9 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 17:c19b471f05cb 10
pmic 24:86f1a63e35a0 11 // user button on nucleo board
pmic 24:86f1a63e35a0 12 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 24:86f1a63e35a0 13 InterruptIn user_button(PC_13); // create InterruptIn interface object to evaluate user button falling and rising edge (no blocking code in ISR)
pmic 24:86f1a63e35a0 14 void user_button_pressed_fcn(); // custom functions which gets executed when user button gets pressed and released, definition below
pmic 24:86f1a63e35a0 15 void user_button_released_fcn();
pmic 6:e1fa1a2d7483 16
pmic 24:86f1a63e35a0 17 // while loop gets executed every main_task_period_ms milliseconds
pmic 33:cff70742569d 18 int main_task_period_ms = 200; // define main task period time in ms e.g. 50 ms -> main task runns 20 times per second
pmic 24:86f1a63e35a0 19 Timer main_task_timer; // create Timer object which we use to run the main task every main task period time in ms
pmic 6:e1fa1a2d7483 20
pmic 24:86f1a63e35a0 21 // led on nucleo board
pmic 24:86f1a63e35a0 22 DigitalOut user_led(LED1); // create DigitalOut object to command user led
pmic 17:c19b471f05cb 23
pmic 33:cff70742569d 24 I2C i2c(PB_9, PB_8); // I2C (PinName sda, PinName scl)
pmic 33:cff70742569d 25 SensorBar sensor_bar(i2c, 0.1175f);
pmic 33:cff70742569d 26 // PinName sda = PB_9;
pmic 33:cff70742569d 27 // PinName scl = PB_8;
pmic 33:cff70742569d 28 // SensorBar sensor_bar = SensorBar(0.1175f);
pmic 20:7e7325edcf5c 29
pmic 1:93d997d6b232 30 int main()
pmic 23:26b3a25fc637 31 {
pmic 24:86f1a63e35a0 32 // attach button fall and rise functions to user button object
pmic 24:86f1a63e35a0 33 user_button.fall(&user_button_pressed_fcn);
pmic 24:86f1a63e35a0 34 user_button.rise(&user_button_released_fcn);
pmic 17:c19b471f05cb 35
pmic 29:d6f1ccf42a31 36 // start timer
pmic 24:86f1a63e35a0 37 main_task_timer.start();
pmic 6:e1fa1a2d7483 38
pmic 33:cff70742569d 39 // sensor_bar.setBarStrobe();
pmic 33:cff70742569d 40 // sensor_bar.clearBarStrobe(); // to illuminate all the time
pmic 33:cff70742569d 41 // sensor_bar.clearInvertBits(); // to make the bar look for a dark line on a reflective surface
pmic 33:cff70742569d 42 // sensor_bar.begin();
pmic 6:e1fa1a2d7483 43
pmic 24:86f1a63e35a0 44 while (true) { // this loop will run forever
pmic 6:e1fa1a2d7483 45
pmic 24:86f1a63e35a0 46 main_task_timer.reset();
pmic 6:e1fa1a2d7483 47
pmic 33:cff70742569d 48 /*
pmic 24:86f1a63e35a0 49 if (do_execute_main_task) {
pmic 1:93d997d6b232 50 } else {
pmic 6:e1fa1a2d7483 51
pmic 33:cff70742569d 52 }
pmic 33:cff70742569d 53 */
pmic 1:93d997d6b232 54
pmic 33:cff70742569d 55 // sensor_bar.update();
pmic 33:cff70742569d 56
pmic 33:cff70742569d 57 printf("---\r\n");
pmic 6:e1fa1a2d7483 58
pmic 33:cff70742569d 59 uint8_t sensor_bar_raw_value = sensor_bar.getRaw();
pmic 33:cff70742569d 60 for( int i = 7; i >= 0; i-- ) {
pmic 33:cff70742569d 61 printf("%d", (sensor_bar_raw_value >> i) & 0x01);
pmic 33:cff70742569d 62 }
pmic 33:cff70742569d 63 printf("\r\n");
pmic 17:c19b471f05cb 64
pmic 33:cff70742569d 65 int8_t sensor_bar_binaryPosition = sensor_bar.getBinaryPosition();
pmic 33:cff70742569d 66 printf("%d\r\n", sensor_bar_binaryPosition);
pmic 6:e1fa1a2d7483 67
pmic 33:cff70742569d 68 uint8_t sensor_bar_nrOfLedsActive = sensor_bar.getNrofLedsActive();
pmic 33:cff70742569d 69 printf("%d\r\n", sensor_bar_nrOfLedsActive);
pmic 33:cff70742569d 70
pmic 33:cff70742569d 71 float sensor_bar_angleRad = sensor_bar.getAngleRad();
pmic 33:cff70742569d 72 printf("%f\r\n", sensor_bar_angleRad * 180.0f / M_PI);
pmic 6:e1fa1a2d7483 73
pmic 24:86f1a63e35a0 74 user_led = !user_led;
pmic 24:86f1a63e35a0 75
pmic 24:86f1a63e35a0 76 // do only output via serial what's really necessary (this makes your code slow)
pmic 33:cff70742569d 77 // printf("%d, %d\r\n", sensor_bar_raw_value_time_ms, sensor_bar_position_time_ms);
pmic 17:c19b471f05cb 78
pmic 24:86f1a63e35a0 79 // read timer and make the main thread sleep for the remaining time span (non blocking)
pmic 24:86f1a63e35a0 80 int main_task_elapsed_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(main_task_timer.elapsed_time()).count();
pmic 24:86f1a63e35a0 81 thread_sleep_for(main_task_period_ms - main_task_elapsed_time_ms);
pmic 1:93d997d6b232 82 }
pmic 1:93d997d6b232 83 }
pmic 6:e1fa1a2d7483 84
pmic 24:86f1a63e35a0 85 void user_button_pressed_fcn()
pmic 25:ea1d6e27c895 86 {
pmic 26:28693b369945 87 user_button_timer.start();
pmic 6:e1fa1a2d7483 88 user_button_timer.reset();
pmic 6:e1fa1a2d7483 89 }
pmic 6:e1fa1a2d7483 90
pmic 24:86f1a63e35a0 91 void user_button_released_fcn()
pmic 6:e1fa1a2d7483 92 {
pmic 24:86f1a63e35a0 93 // read timer and toggle do_execute_main_task if the button was pressed longer than the below specified time
pmic 24:86f1a63e35a0 94 int user_button_elapsed_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(user_button_timer.elapsed_time()).count();
pmic 6:e1fa1a2d7483 95 user_button_timer.stop();
pmic 24:86f1a63e35a0 96 if (user_button_elapsed_time_ms > 200) {
pmic 24:86f1a63e35a0 97 do_execute_main_task = !do_execute_main_task;
pmic 8:9bb806a7f585 98 }
pmic 6:e1fa1a2d7483 99 }