Rome robot excercise
Dependencies: PM2_Libary Eigen
main.cpp
- Committer:
- johnschray2001
- Date:
- 2022-05-23
- Revision:
- 53:054f42566910
- Parent:
- 52:19de6d1cce3a
File content as of revision 53:054f42566910:
#include <mbed.h> #include <math.h> #include <vector> #include "PM2_Libary.h" #include "Eigen/Dense.h" #include "IRSensor.h" // Initial /** * Note: Hardware related differences * - IRSensor class is not available in PM2_Libary * - ROME2 Robot uses different PINS than PES board */ /* ------------------------------------------------------------------------------------------- * * -- Defines * ------------------------------------------------------------------------------------------- */ # define M_PI 3.14159265358979323846 /* ------------------------------------------------------------------------------------------- * * -- Global Variables * ------------------------------------------------------------------------------------------- */ // logical variable main task bool robot_turned_on = false; // this variable will be toggled via the user button (blue button) // 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) EventFlags event_flags; const uint32_t main_task_flag = 1; /* ------------------------------------------------------------------------------------------- * * -- Constants and Parameters * ------------------------------------------------------------------------------------------- */ // kinematic parameters const float r_wheel = 0.0766f / 2.0f; // wheel radius const float L_wheel = 0.176f; // distance from wheel to wheel // 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] /* ########################################################################################### * * ### BEGIN STUDENT CODE: TASK 4: States of the state machine * ### * ### Please define the states of the state machine below. * ### The states can either be of type "const int" or can be defined as an enum. * ### */ // discrete states of this state machine const int INIT = 0; const int go = 1; const int turn = 2; const int stop = 3; /* ### * * ### END STUDENT CODE: TASK 4 * ########################################################################################### */ /* ------------------------------------------------------------------------------------------- * * -- Function Declarations * ------------------------------------------------------------------------------------------- */ static void user_button_pressed_fcn(); // custom functions which gets executed when user button gets pressed and released, definition below static void user_button_released_fcn(); void main_task_trigger(); // triggers the main task each ticker period /* ------------------------------------------------------------------------------------------- * * -- Main Function * ------------------------------------------------------------------------------------------- */ int main() { /* ------------------------------------------------------------------------------------------- * * -- Setup: I/O * ------------------------------------------------------------------------------------------- */ // led on nucleo board DigitalOut user_led(LED1); // create DigitalOut object to command user led // create DigitalOut objects for leds DigitalOut enable_leds(PC_1); 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 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}; // attach button fall and rise functions to user button object user_button.fall(&user_button_pressed_fcn); user_button.rise(&user_button_released_fcn); // 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); /* ------------------------------------------------------------------------------------------- * * -- Setup: Motion Controller * ------------------------------------------------------------------------------------------- */ // 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(999.0f); // adjust max. acceleration for smooth movement speedControllers[1]->setMaxAccelerationRPS(999.0f); /* ------------------------------------------------------------------------------------------- * * -- Setup: Robot Kinematics * ------------------------------------------------------------------------------------------- */ /* ########################################################################################### * * ### BEGIN STUDENT CODE: TASK 1: Forward and inverse kinematic matrix * ### * ### Define the forward and backward kinematic matrix (named Cwheel2robot resp. Cwheel2robot) * ### using the kinematic parameters r_wheel (wheel radius) and L_wheel (wheel distance). * ### The matrices transform between the robot speed vector [x_dt, alpha_dt] in [m/s] and [rad/s] * ### and the wheel speed vector [w_R, w_L] in [rad/s] * ### You can find the docs for the linear algebra library here: * ### https://eigen.tuxfamily.org/dox/group__TutorialMatrixClass.html * ### https://eigen.tuxfamily.org/dox/group__TutorialLinearAlgebra.html#title4 * ### */ // forward kinematics matrix (transforms wheel speeds to robot speeds) Eigen::Matrix2f Cwheel2robot; // still need to define the values Cwheel2robot << -r_wheel / 2.0f , r_wheel / 2.0f , -r_wheel / L_wheel, -r_wheel / L_wheel; // inverse kinematic matrix (transform robot speeds to wheel speeds) Eigen::Matrix2f Crobot2wheel; // still need to define the values Crobot2wheel = Cwheel2robot.inverse(); /* ### * * ### END STUDENT CODE: TASK 1 * ########################################################################################### */ // define kinematic variables Eigen::Vector2f robot_speed_desired; // Robot speed vector [x_dt, alpha_dt] in [m/s] and [rad/s] Eigen::Vector2f wheel_speed_desired; // Wheel speeds [w_R, w_L] in [rad/s] Eigen::Vector2f wheel_speed_smooth; // Wheel speeds limited and smoothed Eigen::Vector2f wheel_speed_actual; // Measured wheel speeds Eigen::Vector2f robot_speed_actual; // Measured robot speed robot_speed_desired.setZero(); wheel_speed_desired.setZero(); wheel_speed_smooth.setZero(); robot_speed_actual.setZero(); wheel_speed_actual.setZero(); /* ------------------------------------------------------------------------------------------- * * -- Setup: State Machine * ------------------------------------------------------------------------------------------- */ // while loop gets executed every main_task_period const float main_task_period = 10e-3; // define main task period time in ms e.g. 50 ms -> main task runns 20 times per second Ticker ticker; // calls a fuction with a precise period ticker.attach(main_task_trigger, std::chrono::microseconds(static_cast<int>(main_task_period*1e6))); // call the main task trigger every period // set initial state machine state, enalbe leds, disable motors int state = INIT; while (true) { // this loop will run forever /* ------------------------------------------------------------------------------------------- * * -- Wait for the next Cycle * ------------------------------------------------------------------------------------------- */ event_flags.wait_any(main_task_flag); /* ------------------------------------------------------------------------------------------- * * -- Read Sensors * ------------------------------------------------------------------------------------------- */ // 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; } // measure the actual wheel speed wheel_speed_actual << speedControllers[0]->getSpeedRPS(), speedControllers[1]->getSpeedRPS(); /* ########################################################################################### * * ### BEGIN STUDENT CODE: TASK 2: Compute the speed of the robot from wheel wheel measurements * ### * ### Every loop of the main task starts by reading the actual wheel speeds into the vector * ### wheel_speed_actual with [w_R, w_L] in [rad/s]. Convert these measurements now into a * ### robot speed vector (robot_speed_actual) with [x_dt, alpha_dt] in [m/s] and [rad/s]. * ### * ### */ // transform wheel speed to robot coordinates robot_speed_actual = Cwheel2robot * wheel_speed_actual; /* ### * * ### END STUDENT CODE: TASK 2 * ########################################################################################### */ /* ------------------------------------------------------------------------------------------- * * -- State Machine * ------------------------------------------------------------------------------------------- */ /* ########################################################################################### * * ### BEGIN STUDENT CODE: TASK 5: Implementing the State Machine * ### * ### The program's logic is implemented here. The robot should now: * ### - Wait for the user button (the blue one) to be pressed * ### - If the button is pressed, then enable the motors and drive forward * ### - If there is an obstacle, turn away from it and continue to drive forward * ### - If the user button is pressed again, set the speed to zero * ### - Wait until the robot is not moving anymore before disabling the motors * ### * ### Most importantly, do not block the program's execution inside the state machine. * ### * ### Following variables are used: * ### robot_turned_on, // boolen that is toggled by the blue button * ### enable_leds, // to enable the led output * ### enable_motors, // to enable the motor driver * ### irSensors, // to measure the distance to obstacles * ### robot_speed_desired // to set the robot speed * ### * ### */ // example of setting the robot speed // robot_speed_desired(0) = TRANSLATIONAL_VELOCITY; // robot_speed_desired(1) = 0.0f; switch(state) { case INIT: // wait for button to be pressed // if button pressed, next state is forward // make sure speed is 0 robot_speed_desired(0) = 0.0f; robot_speed_desired(1) = 0.0f; // button pressed if (robot_turned_on){ enable_motors = 1; enable_leds = 1; state = go; } break; case go: // if an object is seen, turn // if button pressed, stop // continue if object not seen // button pressed if (!robot_turned_on){ state = stop; break; } // go forward robot_speed_desired(0) = TRANSLATIONAL_VELOCITY; robot_speed_desired(1) = 0.0f; // if any of the leds are lit up, too close to an object and turn if ((irSensor0.read() < DISTANCE_THRESHOLD) || (irSensor1.read() < DISTANCE_THRESHOLD) || (irSensor5.read() < DISTANCE_THRESHOLD)) { state = turn; } break; case turn: // keep turning if object seen // if object no longer seen, stop turning // if button pressed, stop // button pressed if (!robot_turned_on){ state = stop; break; } // make robot stop moving forward robot_speed_desired(0) = 0.0f; // turn left if ((irSensor0.read() < DISTANCE_THRESHOLD) || (irSensor1.read() < DISTANCE_THRESHOLD)) { robot_speed_desired(1) = ROTATIONAL_VELOCITY; } // turn right else if (irSensor5.read() < DISTANCE_THRESHOLD) { robot_speed_desired(1) = -1.0f * ROTATIONAL_VELOCITY; } // no sensors close enough, go forward else { robot_speed_desired(1) = 0.0f; state = go; } break; case stop: // if button pressed, go forward // stay stopped otherwise // stop robot robot_speed_desired(0) = 0.0f; robot_speed_desired(1) = 0.0f; // button pressed if (robot_turned_on){ state = go; } break; } /* ### * * ### END STUDENT CODE: TASK 5 * ########################################################################################### */ /* ------------------------------------------------------------------------------------------- * * -- Inverse Kinematics * ------------------------------------------------------------------------------------------- */ /* ########################################################################################### * * ### BEGIN STUDENT CODE: TASK 3: Compute the desired wheel speeds * ### * ### In the main task, the robot_speed_desired vector [x_dt, alpha_dt] in [m/s] and [rad/s] * ### is set. From that we need to compute the desired wheel speeds (wheel_speed_desired) * ### that are handed to the speed controller * ### */ // transform robot coordinates to wheel speed wheel_speed_desired = Cwheel2robot.inverse() * robot_speed_desired; /* ### * * ### END STUDENT CODE: TASK 3 * ########################################################################################### */ // smooth desired wheel_speeds trajectoryPlaners[0]->incrementToVelocity(wheel_speed_desired(0) / (2.0f * M_PI), main_task_period); trajectoryPlaners[1]->incrementToVelocity(wheel_speed_desired(1) / (2.0f * M_PI), main_task_period); wheel_speed_smooth << trajectoryPlaners[0]->getVelocity(), trajectoryPlaners[1]->getVelocity(); // command speedController objects 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; /* ------------------------------------------------------------------------------------------- * * -- Printing to Console * ------------------------------------------------------------------------------------------- */ // 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)); } } static void user_button_pressed_fcn() { user_button_timer.start(); user_button_timer.reset(); } static void user_button_released_fcn() { // read timer and toggle robot_turned_on 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) { robot_turned_on = !robot_turned_on; } } void main_task_trigger() { // set the trigger to resume the waiting main task event_flags.set(main_task_flag); }