![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Example project
Dependencies: PM2_Libary Eigen
main.cpp
- Committer:
- pmic
- Date:
- 2022-05-16
- Revision:
- 46:41c9367da539
- Parent:
- 45:d9e6e89210f9
- Child:
- 47:6693bffcdfd0
File content as of revision 46:41c9367da539:
#include <mbed.h> #include <math.h> #include <vector> #include "PM2_Libary.h" #include "Eigen/Dense.h" #include "IRSensor.h" # define M_PI 3.14159265358979323846 // number pi /** * notes: * - IRSensor class is not available in PM2_Libary * - ROME2 Robot uses different PINS than PES board */ // logical variable main task 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 // user button on nucleo board 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) InterruptIn user_button(PC_13); // create InterruptIn interface object to evaluate user button falling and rising edge (no blocking code in ISR) void user_button_pressed_fcn(); // custom functions which gets executed when user button gets pressed and released, definition below void user_button_released_fcn(); int main() { // while loop gets executed every main_task_period_ms milliseconds 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 Timer main_task_timer; // create Timer object which we use to run the main task every main task period time in ms // led on nucleo board DigitalOut user_led(LED1); // create DigitalOut object to command user led // discrete states of this state machine const int ROBOT_OFF = 0; const int MOVE_FORWARD = 1; const int TURN_LEFT = 2; const int TURN_RIGHT = 3; const int SLOWING_DOWN = 4; // default parameters for robots movement const float DISTANCE_THRESHOLD = 0.2f; // minimum allowed distance to obstacle in [m] const float TRANSLATIONAL_VELOCITY = 0.4f; // translational velocity in [m/s] const float ROTATIONAL_VELOCITY = 1.6f; // rotational velocity in [rad/s] const float VELOCITY_THRESHOLD = 0.05; // velocity threshold before switching off, in [m/s] and [rad/s] // create DigitalOut objects for leds DigitalOut led0(PC_8); DigitalOut led1(PC_6); DigitalOut led2(PB_12); DigitalOut led3(PA_7); DigitalOut led4(PC_0); DigitalOut led5(PC_9); std::vector<DigitalOut> leds = {led0, led1, led2, led3, led4, led5}; // create IR sensor objects AnalogIn dist(PB_1); DigitalOut enable_leds(PC_1); DigitalOut bit0(PH_1); DigitalOut bit1(PC_2); DigitalOut bit2(PC_3); IRSensor irSensor0(dist, bit0, bit1, bit2, 0); IRSensor irSensor1(dist, bit0, bit1, bit2, 1); IRSensor irSensor2(dist, bit0, bit1, bit2, 2); IRSensor irSensor3(dist, bit0, bit1, bit2, 3); IRSensor irSensor4(dist, bit0, bit1, bit2, 4); IRSensor irSensor5(dist, bit0, bit1, bit2, 5); std::vector<IRSensor> irSensors = {irSensor0, irSensor1, irSensor2, irSensor3, irSensor4, irSensor5}; // 19:1 Metal Gearmotor 37Dx68L mm 12V with 64 CPR Encoder (Helical Pinion) DigitalOut enable_motors(PB_2); DigitalIn motorDriverFault(PB_14); DigitalIn motorDriverWarning(PB_15); // create SpeedController objects FastPWM pwm_M1(PA_9); // motor M1 is closed-loop speed controlled (angle velocity) FastPWM pwm_M2(PA_8); // motor M2 is closed-loop speed controlled (angle velocity) EncoderCounter encoder_M1(PA_6, PC_7); // create encoder objects to read in the encoder counter values EncoderCounter encoder_M2(PB_6, PB_7); const float max_voltage = 12.0f; // define maximum voltage of battery packs, adjust this to 6.0f V if you only use one batterypack const float counts_per_turn = 64.0f * 19.0f; // define counts per turn at gearbox end: counts/turn * gearratio const float kn = 530.0f / 12.0f; // define motor constant in rpm per V // create Motion objects (trajectory planner) Motion* trajectoryPlaners[2]; trajectoryPlaners[0] = new Motion; trajectoryPlaners[1] = new Motion; trajectoryPlaners[0]->setProfileVelocity(max_voltage * kn / 60.0f); trajectoryPlaners[1]->setProfileVelocity(max_voltage * kn / 60.0f); trajectoryPlaners[0]->setProfileAcceleration(10.0f); trajectoryPlaners[1]->setProfileAcceleration(10.0f); trajectoryPlaners[0]->setProfileDeceleration(10.0f); trajectoryPlaners[1]->setProfileDeceleration(10.0f); // create SpeedController objects SpeedController* speedControllers[2]; speedControllers[0] = new SpeedController(counts_per_turn, kn, max_voltage, pwm_M1, encoder_M1); speedControllers[1] = new SpeedController(counts_per_turn, kn, max_voltage, pwm_M2, encoder_M2); speedControllers[0]->setSpeedCntrlGain(0.04f); // adjust speedcontroller gains speedControllers[1]->setSpeedCntrlGain(0.04f); //speedControllers[0]->setMaxAccelerationRPS(10.0f); // use this if you're not using the trajectoryPlaners //speedControllers[1]->setMaxAccelerationRPS(10.0f); speedControllers[0]->setMaxAccelerationRPS(999.0f); // adjust max. acceleration for smooth movement speedControllers[1]->setMaxAccelerationRPS(999.0f); // robot kinematics const float r_wheel = 0.0766f / 2.0f; // wheel radius const float L_wheel = 0.176f; // distance from wheel to wheel Eigen::Matrix2f Cwheel2robot; // transform wheel to robot //Eigen::Matrix2f Crobot2wheel; // transform robot to wheel Cwheel2robot << -r_wheel / 2.0f , r_wheel / 2.0f , -r_wheel / L_wheel, -r_wheel / L_wheel; //Crobot2wheel << -1.0f / r_wheel, -L_wheel / (2.0f * r_wheel), // 1.0f / r_wheel, -L_wheel / (2.0f * r_wheel); Eigen::Vector2f robot_coord; // contains v and w (robot translational and rotational velocities) Eigen::Vector2f wheel_speed; // w1 w2 (wheel speed) Eigen::Vector2f wheel_speed_smooth; // w1 w2 (wheel speed) Eigen::Vector2f robot_coord_actual; Eigen::Vector2f wheel_speed_actual; robot_coord.setZero(); wheel_speed.setZero(); wheel_speed_smooth.setZero(); robot_coord_actual.setZero(); wheel_speed_actual.setZero(); // attach button fall and rise functions to user button object user_button.fall(&user_button_pressed_fcn); user_button.rise(&user_button_released_fcn); // start timer main_task_timer.start(); // set initial state machine state, enalbe leds, disable motors int state = ROBOT_OFF; enable_leds = 1; enable_motors = 0; while (true) { // this loop will run forever main_task_timer.reset(); // set leds according to DISTANCE_THRESHOLD for (uint8_t i = 0; i < leds.size(); i++) { if (irSensors[i].read() > DISTANCE_THRESHOLD) leds[i] = 0; else leds[i] = 1; } // read actual wheel speed and transform it to robot coordinates wheel_speed_actual << speedControllers[0]->getSpeedRPS(), speedControllers[1]->getSpeedRPS(); robot_coord_actual = Cwheel2robot * wheel_speed_actual; // state machine switch (state) { case ROBOT_OFF: if (do_execute_main_task) { enable_motors = 1; robot_coord(0) = TRANSLATIONAL_VELOCITY; robot_coord(1) = 0.0f; state = MOVE_FORWARD; } break; case MOVE_FORWARD: if (!do_execute_main_task) { robot_coord(0) = 0.0f; robot_coord(1) = 0.0f; state = SLOWING_DOWN; } else if ((irSensors[0].read() < DISTANCE_THRESHOLD) || (irSensors[1].read() < DISTANCE_THRESHOLD)) { robot_coord(0) = 0.0f; robot_coord(1) = ROTATIONAL_VELOCITY; state = TURN_LEFT; } else if (irSensors[5].read() < DISTANCE_THRESHOLD) { robot_coord(0) = 0.0f; robot_coord(1) = -ROTATIONAL_VELOCITY; state = TURN_RIGHT; } else { // leave it driving } break; case TURN_LEFT: if (!do_execute_main_task) { robot_coord(1) = 0.0f; state = SLOWING_DOWN; } else if ((irSensors[0].read() > DISTANCE_THRESHOLD) && (irSensors[1].read() > DISTANCE_THRESHOLD) && (irSensors[5].read() > DISTANCE_THRESHOLD)) { robot_coord(0) = TRANSLATIONAL_VELOCITY; robot_coord(1) = 0.0f; state = MOVE_FORWARD; } break; case TURN_RIGHT: if (!do_execute_main_task) { robot_coord(1) = 0.0f; state = SLOWING_DOWN; } else if ((irSensors[0].read() > DISTANCE_THRESHOLD) && (irSensors[1].read() > DISTANCE_THRESHOLD) && (irSensors[5].read() > DISTANCE_THRESHOLD)) { robot_coord(0) = TRANSLATIONAL_VELOCITY; robot_coord(1) = 0.0f; state = MOVE_FORWARD; } break; case SLOWING_DOWN: if ((fabs(robot_coord_actual(0)) < VELOCITY_THRESHOLD) && (fabs(robot_coord_actual(1)) < VELOCITY_THRESHOLD)) { enable_motors = 0; state = ROBOT_OFF; } break; } // transform robot coordinates to wheel speed wheel_speed = Cwheel2robot.inverse() * robot_coord; // smooth desired wheel_speeds trajectoryPlaners[0]->incrementToVelocity(wheel_speed(0) / (2.0f * M_PI), static_cast<float>(main_task_period_ms) * 1.0e-3f); trajectoryPlaners[1]->incrementToVelocity(wheel_speed(1) / (2.0f * M_PI), static_cast<float>(main_task_period_ms) * 1.0e-3f); wheel_speed_smooth << trajectoryPlaners[0]->getVelocity(), trajectoryPlaners[1]->getVelocity(); // command speedController objects //speedControllers[0]->setDesiredSpeedRPS(wheel_speed(0) / (2.0f * M_PI)); // use this if you're not using the trajectoryPlaners //speedControllers[1]->setDesiredSpeedRPS(wheel_speed(1) / (2.0f * M_PI)); // use this if you're not using the trajectoryPlaners speedControllers[0]->setDesiredSpeedRPS(wheel_speed_smooth(0)); // set a desired speed for speed controlled dc motors M1 speedControllers[1]->setDesiredSpeedRPS(wheel_speed_smooth(1)); // set a desired speed for speed controlled dc motors M2 user_led = !user_led; // do only output via serial what's really necessary (this makes your code slow) //printf("%f, %f\r\n", wheel_speed_actual(0), wheel_speed_actual(1)); // read timer and make the main thread sleep for the remaining time span (non blocking) int main_task_elapsed_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(main_task_timer.elapsed_time()).count(); thread_sleep_for(main_task_period_ms - main_task_elapsed_time_ms); } } void user_button_pressed_fcn() { user_button_timer.start(); user_button_timer.reset(); } void user_button_released_fcn() { // read timer and toggle do_execute_main_task if the button was pressed longer than the below specified time int user_button_elapsed_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(user_button_timer.elapsed_time()).count(); user_button_timer.stop(); if (user_button_elapsed_time_ms > 200) { do_execute_main_task = !do_execute_main_task; } }