Example project
Dependencies: PM2_Libary Eigen
Diff: main.cpp
- Revision:
- 42:d2d2db5974c9
- Parent:
- 40:924bdbc33391
- Child:
- 44:dd746bf0e81f
--- a/main.cpp Wed May 11 09:44:25 2022 +0200 +++ b/main.cpp Sat May 14 15:27:12 2022 +0200 @@ -1,79 +1,113 @@ -#include <stdio.h> #include <mbed.h> +#include <math.h> #include <vector> +#include "PM2_Libary.h" +#include "Eigen/Dense.h" + #include "IRSensor.h" -#include "EncoderCounterROME2.h" -#include "Controller.h" - -#include "Eigen/Dense.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(BUTTON1); // create InterruptIn interface object to evaluate user button falling and rising edge (no blocking code in ISR) +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(); -// while loop gets executed every main_task_period_ms milliseconds -int main_task_period_ms = 50; // 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 +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 - -static const int ROBOT_OFF = 0; // discrete states of this state machine -static const int MOVE_FORWARD = 1; -static const int TURN_LEFT = 2; -static const int TURN_RIGHT = 3; -static const int SLOWING_DOWN = 4; + // led on nucleo board + DigitalOut user_led(LED1); // create DigitalOut object to command user led -const float DISTANCE_THRESHOLD = 0.4f; // minimum allowed distance to obstacle in [m] -const float TRANSLATIONAL_VELOCITY = 1.0f; // translational velocity in [m/s] -const float ROTATIONAL_VELOCITY = 1.0f; // rotational velocity in [rad/s] -const float VELOCITY_THRESHOLD = 0.01; // velocity threshold before switching off, in [m/s] and [rad/s] + // 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; -DigitalOut led0(PD_4); -DigitalOut led1(PD_3); -DigitalOut led2(PD_6); -DigitalOut led3(PD_2); -DigitalOut led4(PD_7); -DigitalOut led5(PD_5); -std::vector<DigitalOut> leds = {led0, led1, led2, led3, led4, led5}; + // 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(PA_0); -DigitalOut enable(PG_1); -DigitalOut bit0(PF_0); -DigitalOut bit1(PF_1); -DigitalOut bit2(PF_2); + // 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}; -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); + // 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 motor control objects -DigitalOut enableMotorDriver(PG_0); -DigitalIn motorDriverFault(PD_1); -DigitalIn motorDriverWarning(PD_0); -PwmOut pwmLeft(PF_9); -PwmOut pwmRight(PF_8); -EncoderCounterROME2 counterLeft(PD_12, PD_13); -EncoderCounterROME2 counterRight(PB_4, PC_7); + // 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 + + 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); // adjust max. acceleration for smooth movement + speedControllers[1]->setMaxAccelerationRPS(10.0f); -int state; -// create robot controller objects -Controller controller(pwmLeft, pwmRight, counterLeft, counterRight); -// StateMachine stateMachine(controller, enableMotorDriver, led0, led1, led2, led3, led4, led5, button, irSensor0, irSensor1, irSensor2, irSensor3, irSensor4, irSensor5); - -int main() { + // 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_actual; + Eigen::Vector2f robot_coord_actual; + robot_coord.setZero(); + wheel_speed.setZero(); + wheel_speed_actual.setZero(); + robot_coord_actual.setZero(); // attach button fall and rise functions to user button object user_button.fall(&user_button_pressed_fcn); @@ -81,159 +115,110 @@ // start timer main_task_timer.start(); - - enable = 1; - - enableMotorDriver = 0; - state = ROBOT_OFF; - Eigen::Matrix<float, 6, 1> irSensor_distance; // transform wheel to robot + // 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(); - // read the distance sensors - irSensor_distance << irSensor0.read(), - irSensor1.read(), - irSensor2.read(), - irSensor3.read(), - irSensor4.read(), - irSensor5.read(); - - // set the leds based on distance measurements - ///* - // iterator based foor loop - int cnt = 0; - for(auto it = leds.begin(); it != leds.end(); it++){ - *it = irSensor_distance(cnt) < DISTANCE_THRESHOLD; - cnt++; + // 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; } - //*/ - /* - // index based for loop - for (int i = 0; i < leds.size(); i++) { - leds[i] = irSensor_distance(i) < DISTANCE_THRESHOLD; - } - */ - /* - // range based for loop - int cnt = 0; - for(DigitalOut led : leds){ - led = irSensor_distance(cnt) < DISTANCE_THRESHOLD; - cnt++; - } - /* - led0 = irSensor_distance(0) < DISTANCE_THRESHOLD; - led1 = irSensor_distance(1) < DISTANCE_THRESHOLD; - led2 = irSensor_distance(2) < DISTANCE_THRESHOLD; - led3 = irSensor_distance(3) < DISTANCE_THRESHOLD; - led4 = irSensor_distance(4) < DISTANCE_THRESHOLD; - led5 = irSensor_distance(5) < DISTANCE_THRESHOLD; - */ + // 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) { - // enables Motors, sets translational speed and switches to MOVE_FORWARD - case ROBOT_OFF: + case ROBOT_OFF: + if (do_execute_main_task) { - enableMotorDriver = 1; - controller.setTranslationalVelocity(TRANSLATIONAL_VELOCITY); - controller.setRotationalVelocity(0.0f); + enable_motors = 1; + robot_coord(0) = TRANSLATIONAL_VELOCITY; + robot_coord(1) = 0.0f; state = MOVE_FORWARD; } break; - - // continue here + case MOVE_FORWARD: + if (!do_execute_main_task) { - controller.setTranslationalVelocity(0); - controller.setRotationalVelocity(0); + robot_coord(0) = 0.0f; + robot_coord(1) = 0.0f; state = SLOWING_DOWN; - break; - } - - // ??? - if (irSensor_distance(2) < DISTANCE_THRESHOLD && irSensor_distance(2) < DISTANCE_THRESHOLD) { - controller.setTranslationalVelocity(-TRANSLATIONAL_VELOCITY/2); - controller.setRotationalVelocity(-ROTATIONAL_VELOCITY); + } 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; - break; - // ??? - } else if (irSensor_distance(4) < DISTANCE_THRESHOLD && irSensor_distance(4) < DISTANCE_THRESHOLD) { - controller.setTranslationalVelocity(-TRANSLATIONAL_VELOCITY/2); - controller.setRotationalVelocity(ROTATIONAL_VELOCITY); - state = TURN_LEFT; - break; - } else if (irSensor_distance(3) < DISTANCE_THRESHOLD/2) { - controller.setTranslationalVelocity(-TRANSLATIONAL_VELOCITY); - controller.setRotationalVelocity(-ROTATIONAL_VELOCITY); - state = TURN_RIGHT; - break; + } else { + // leave it driving } - break; case TURN_LEFT: + if (!do_execute_main_task) { - controller.setTranslationalVelocity(0); - controller.setRotationalVelocity(0); + robot_coord(1) = 0.0f; state = SLOWING_DOWN; - break; - } - if ( (irSensor_distance(2) > DISTANCE_THRESHOLD) && (irSensor_distance(3) > DISTANCE_THRESHOLD) && (irSensor_distance(4) > DISTANCE_THRESHOLD) ) { - controller.setRotationalVelocity(0); - controller.setTranslationalVelocity(TRANSLATIONAL_VELOCITY); + } 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; } break; - case TURN_RIGHT: + case TURN_RIGHT: + if (!do_execute_main_task) { - controller.setTranslationalVelocity(0); - controller.setRotationalVelocity(0); + robot_coord(1) = 0.0f; state = SLOWING_DOWN; - break; - } - - if ( (irSensor_distance(2) > DISTANCE_THRESHOLD) && (irSensor_distance(3) > DISTANCE_THRESHOLD) && (irSensor_distance(4) > DISTANCE_THRESHOLD) ) { - controller.setRotationalVelocity(0); - controller.setTranslationalVelocity(TRANSLATIONAL_VELOCITY); + } 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; } break; case SLOWING_DOWN: - if (abs(controller.getActualTranslationalVelocity()) < VELOCITY_THRESHOLD - && abs(controller.getActualRotationalVelocity()) > VELOCITY_THRESHOLD) { - state = ROBOT_OFF; - enableMotorDriver = 0; + + if ((fabs(robot_coord_actual(0)) < VELOCITY_THRESHOLD) && (fabs(robot_coord_actual(1)) < VELOCITY_THRESHOLD)) { + enable_motors = 0; state = ROBOT_OFF; } break; - - default: - - state = ROBOT_OFF; + } + // transform robot coordinates to wheel speed + wheel_speed = Cwheel2robot.inverse() * robot_coord; + + // command speedController objects + speedControllers[0]->setDesiredSpeedRPS(wheel_speed(0) / (2.0f * M_PI)); // set a desired speed for speed controlled dc motors M1 + speedControllers[1]->setDesiredSpeedRPS(wheel_speed(1) / (2.0f * M_PI)); // set a desired speed for speed controlled dc motors M2 + user_led = !user_led; - /* - if (do_execute_main_task) - printf("button pressed\r\n"); - else - printf("button NOT pressed\r\n"); - */ - // printf("actual velocity: %.3f [m/s] / %.3f [rad/s]\r\n", controller.getActualTranslationalVelocity(), controller.getActualRotationalVelocity()); - 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)); + // 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) - auto main_task_elapsed_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(main_task_timer.elapsed_time()).count(); + 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); } }