main.cpp

00001 #include <mbed.h>
00002 #include <math.h>
00003 #include <vector>
00004 
00005 #include "PM2_Libary.h"
00006 #include "Eigen/Dense.h"
00007 
00008 #include "IRSensor.h"
00009 
00010 
00011 
00012 /**
00013  * Note: Hardware related differences
00014  * - IRSensor class is not available in PM2_Libary
00015  * - ROME2 Robot uses different PINS than PES board
00016  */
00017 
00018 /* ------------------------------------------------------------------------------------------- *
00019  * -- Defines
00020  * ------------------------------------------------------------------------------------------- */
00021  
00022 # define M_PI 3.14159265358979323846
00023 
00024 /* ------------------------------------------------------------------------------------------- *
00025  * -- Global Variables
00026  * ------------------------------------------------------------------------------------------- */
00027 
00028 // logical variable main task
00029 bool robot_turned_on = false;  // this variable will be toggled via the user button (blue button)
00030 
00031 // user button on nucleo board
00032 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)
00033 InterruptIn user_button(PC_13);     // create InterruptIn interface object to evaluate user button falling and rising edge (no blocking code in ISR)
00034 
00035 EventFlags event_flags;
00036 const uint32_t main_task_flag = 1;
00037 
00038 /* ------------------------------------------------------------------------------------------- *
00039  * -- Constants and Parameters
00040  * ------------------------------------------------------------------------------------------- */
00041 
00042 // kinematic parameters
00043 const float r_wheel = 0.0766f / 2.0f;   // wheel radius
00044 const float L_wheel = 0.176f;           // distance from wheel to wheel
00045 
00046 // default parameters for robots movement
00047 const float DISTANCE_THRESHOLD = 0.2f;        // minimum allowed distance to obstacle in [m]
00048 const float TRANSLATIONAL_VELOCITY = 0.4f;    // translational velocity in [m/s]
00049 const float ROTATIONAL_VELOCITY = 1.6f;       // rotational velocity in [rad/s]
00050 const float VELOCITY_THRESHOLD = 0.05;        // velocity threshold before switching off, in [m/s] and [rad/s]
00051 
00052 /* ########################################################################################### *
00053  * ### BEGIN STUDENT CODE: TASK 4: States of the state machine
00054  * ###
00055  * ### Please define the states of the state machine below.
00056  * ### The states can either be of type "const int" or can be defined as an enum.
00057  * ###                                                                                         */
00058 
00059 // discrete states of this state machine
00060 const int INIT          = 0;
00061 const int ROBOT_OFF     = 1;      
00062 const int MOVE_FORWARD  = 2;
00063 const int TURN_LEFT     = 3;
00064 const int TURN_RIGHT    = 4;
00065 const int SLOWING_DOWN  = 5;
00066 
00067 
00068 /* ###                                                                                         *
00069  * ### END STUDENT CODE: TASK 4
00070  * ########################################################################################### */
00071 
00072 /* ------------------------------------------------------------------------------------------- *
00073  * -- Function Declarations
00074  * ------------------------------------------------------------------------------------------- */
00075 static void user_button_pressed_fcn();     // custom functions which gets executed when user button gets pressed and released, definition below
00076 static void user_button_released_fcn();
00077 
00078 void main_task_trigger();                  // triggers the main task each ticker period
00079 
00080 /* ------------------------------------------------------------------------------------------- *
00081  * -- Main Function
00082  * ------------------------------------------------------------------------------------------- */
00083 int main()
00084 {
00085     /* ------------------------------------------------------------------------------------------- *
00086      * -- Setup: I/O
00087      * ------------------------------------------------------------------------------------------- */
00088 
00089     // led on nucleo board
00090     DigitalOut user_led(LED1);      // create DigitalOut object to command user led
00091 
00092     // create DigitalOut objects for leds
00093     DigitalOut enable_leds(PC_1);
00094     DigitalOut led0(PC_8);
00095     DigitalOut led1(PC_6);
00096     DigitalOut led2(PB_12);
00097     DigitalOut led3(PA_7);
00098     DigitalOut led4(PC_0);
00099     DigitalOut led5(PC_9);
00100     std::vector<DigitalOut> leds = {led0, led1, led2, led3, led4, led5};
00101 
00102     // create IR sensor objects  
00103     AnalogIn dist(PB_1);
00104     DigitalOut bit0(PH_1);
00105     DigitalOut bit1(PC_2);
00106     DigitalOut bit2(PC_3);
00107     IRSensor irSensor0(dist, bit0, bit1, bit2, 0);
00108     IRSensor irSensor1(dist, bit0, bit1, bit2, 1);
00109     IRSensor irSensor2(dist, bit0, bit1, bit2, 2);
00110     IRSensor irSensor3(dist, bit0, bit1, bit2, 3);
00111     IRSensor irSensor4(dist, bit0, bit1, bit2, 4);
00112     IRSensor irSensor5(dist, bit0, bit1, bit2, 5);
00113     std::vector<IRSensor> irSensors = {irSensor0, irSensor1, irSensor2, irSensor3, irSensor4, irSensor5};
00114 
00115     // attach button fall and rise functions to user button object
00116     user_button.fall(&user_button_pressed_fcn);
00117     user_button.rise(&user_button_released_fcn);
00118 
00119     // 19:1 Metal Gearmotor 37Dx68L mm 12V with 64 CPR Encoder (Helical Pinion)
00120     DigitalOut enable_motors(PB_2);
00121     DigitalIn motorDriverFault(PB_14);
00122     DigitalIn motorDriverWarning(PB_15);
00123 
00124     /* ------------------------------------------------------------------------------------------- *
00125      * -- Setup: Motion Controller
00126      * ------------------------------------------------------------------------------------------- */
00127 
00128     // create SpeedController objects
00129     FastPWM pwm_M1(PA_9);  // motor M1 is closed-loop speed controlled (angle velocity)
00130     FastPWM pwm_M2(PA_8);  // motor M2 is closed-loop speed controlled (angle velocity)
00131     EncoderCounter  encoder_M1(PA_6, PC_7); // create encoder objects to read in the encoder counter values
00132     EncoderCounter  encoder_M2(PB_6, PB_7);
00133     const float max_voltage = 12.0f;                // define maximum voltage of battery packs, adjust this to 6.0f V if you only use one batterypack
00134     const float counts_per_turn = 64.0f * 19.0f;    // define counts per turn at gearbox end: counts/turn * gearratio
00135     const float kn = 530.0f / 12.0f;                // define motor constant in rpm per V
00136     
00137     // create Motion objects (trajectory planner)
00138     Motion* trajectoryPlaners[2];
00139     trajectoryPlaners[0] = new Motion;
00140     trajectoryPlaners[1] = new Motion;
00141     trajectoryPlaners[0]->setProfileVelocity(max_voltage * kn / 60.0f);
00142     trajectoryPlaners[1]->setProfileVelocity(max_voltage * kn / 60.0f);
00143     trajectoryPlaners[0]->setProfileAcceleration(10.0f);
00144     trajectoryPlaners[1]->setProfileAcceleration(10.0f);
00145     trajectoryPlaners[0]->setProfileDeceleration(10.0f);
00146     trajectoryPlaners[1]->setProfileDeceleration(10.0f);
00147 
00148     // create SpeedController objects
00149     SpeedController* speedControllers[2];
00150     speedControllers[0] = new SpeedController(counts_per_turn, kn, max_voltage, pwm_M1, encoder_M1);
00151     speedControllers[1] = new SpeedController(counts_per_turn, kn, max_voltage, pwm_M2, encoder_M2);
00152     speedControllers[0]->setSpeedCntrlGain(0.04f);          // adjust speedcontroller gains
00153     speedControllers[1]->setSpeedCntrlGain(0.04f);
00154     speedControllers[0]->setMaxAccelerationRPS(999.0f);     // adjust max. acceleration for smooth movement
00155     speedControllers[1]->setMaxAccelerationRPS(999.0f);
00156 
00157     /* ------------------------------------------------------------------------------------------- *
00158      * -- Setup: Robot Kinematics
00159      * ------------------------------------------------------------------------------------------- */
00160     
00161     /* ########################################################################################### *
00162      * ### BEGIN STUDENT CODE: TASK 1: Forward and inverse kinematic matrix
00163      * ###
00164      * ### Define the forward and backward kinematic matrix (named Cwheel2robot resp. Cwheel2robot)
00165      * ### using the kinematic parameters r_wheel (wheel radius) and L_wheel (wheel distance). 
00166      * ### The matrices transform between the robot speed vector [x_dt, alpha_dt] in [m/s] and [rad/s]
00167      * ### and the wheel speed vector [w_R, w_L] in [rad/s]
00168      * ### You can find the docs for the linear algebra library here: 
00169      * ### https://eigen.tuxfamily.org/dox/group__TutorialMatrixClass.html
00170      * ### https://eigen.tuxfamily.org/dox/group__TutorialLinearAlgebra.html#title4
00171      * ###                                                                                         */
00172 
00173     // forward kinematics matrix (transforms wheel speeds to robot speeds)
00174     Eigen::Matrix2f Cwheel2robot;                               
00175     Cwheel2robot << -r_wheel / 2.0f   ,  r_wheel / 2.0f   ,
00176                     -r_wheel / L_wheel, -r_wheel / L_wheel;
00177 
00178     // inverse kinematic matrix (transform robot speeds to wheel speeds)
00179     auto Crobot2wheel = Cwheel2robot.inverse();
00180     
00181     /* ###                                                                                         *
00182      * ### END STUDENT CODE: TASK 1
00183      * ########################################################################################### */
00184 
00185     // define kinematic variables
00186     Eigen::Vector2f robot_speed_desired;    // Robot speed vector [x_dt, alpha_dt] in [m/s] and [rad/s]
00187     Eigen::Vector2f wheel_speed_desired;    // Wheel speeds [w_R, w_L] in [rad/s]
00188     Eigen::Vector2f wheel_speed_smooth;     // Wheel speeds limited and smoothed
00189     Eigen::Vector2f wheel_speed_actual;     // Measured wheel speeds
00190     Eigen::Vector2f robot_speed_actual;     // Measured robot speed
00191 
00192     robot_speed_desired.setZero();
00193     wheel_speed_desired.setZero();
00194     wheel_speed_smooth.setZero();
00195     robot_speed_actual.setZero();
00196     wheel_speed_actual.setZero();
00197 
00198     /* ------------------------------------------------------------------------------------------- *
00199      * -- Setup: State Machine
00200      * ------------------------------------------------------------------------------------------- */
00201     
00202     // while loop gets executed every main_task_period
00203     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
00204 
00205     
00206     Ticker ticker;                                      // calls a fuction with a precise period
00207     ticker.attach(main_task_trigger, std::chrono::microseconds(static_cast<int>(main_task_period*1e6))); // call the main task trigger every period
00208 
00209     // set initial state machine state, enalbe leds, disable motors
00210     int state = INIT;
00211     
00212     while (true) { // this loop will run forever
00213 
00214         /* ------------------------------------------------------------------------------------------- *
00215          * -- Wait for the next Cycle
00216          * ------------------------------------------------------------------------------------------- */
00217         event_flags.wait_any(main_task_flag);
00218 
00219         
00220         /* ------------------------------------------------------------------------------------------- *
00221          * -- Read Sensors
00222          * ------------------------------------------------------------------------------------------- */
00223 
00224         // set leds according to DISTANCE_THRESHOLD
00225         for (uint8_t i = 0; i < leds.size(); i++) {
00226             if (irSensors[i].read() > DISTANCE_THRESHOLD)
00227                 leds[i] =  0;
00228             else
00229                 leds[i] = 1;
00230         }
00231 
00232         // measure the actual wheel speed
00233         wheel_speed_actual << speedControllers[0]->getSpeedRPS(), speedControllers[1]->getSpeedRPS();
00234 
00235         /* ########################################################################################### *
00236          * ### BEGIN STUDENT CODE: TASK 2: Compute the speed of the robot from wheel wheel measurements
00237          * ###
00238          * ### Every loop of the main task starts by reading the actual wheel speeds into the vector
00239          * ### wheel_speed_actual with [w_R, w_L] in [rad/s]. Convert these measurements now into a
00240          * ### robot speed vector (robot_speed_actual) with [x_dt, alpha_dt] in [m/s] and [rad/s].
00241          * ###
00242          * ###                                                                                         */
00243         
00244         // transform wheel speed to robot coordinates
00245         robot_speed_actual =  Cwheel2robot * wheel_speed_actual;
00246 
00247         /* ###                                                                                         *
00248          * ### END STUDENT CODE: TASK 2
00249          * ########################################################################################### */
00250 
00251 
00252         /* ------------------------------------------------------------------------------------------- *
00253          * -- State Machine
00254          * ------------------------------------------------------------------------------------------- */
00255 
00256         /* ########################################################################################### *
00257          * ### BEGIN STUDENT CODE: TASK 5: Implementing the State Machine
00258          * ###
00259          * ### The program's logic is implemented here. The robot should now:
00260          * ### - Wait for the user button (the blue one) to be pressed
00261          * ### - If the button is pressed, then enable the motors and drive forward
00262          * ### - If there is an obstacle, turn away from it and continue to drive forward
00263          * ### - If the user button is pressed again, set the speed to zero
00264          * ### - Wait until the robot is not moving anymore before disabling the motors
00265          * ###
00266          * ### Most importantly, do not block the program's execution inside the state machine.
00267          * ###
00268          * ### Following variables are used: 
00269          * ###    robot_turned_on,          // boolen that is toggled by the blue button
00270          * ###    enable_leds,              // to enable the led output
00271          * ###    enable_motors,            // to enable the motor driver
00272          * ###    irSensors,                // to measure the distance to obstacles
00273          * ###    robot_speed_desired       // to set the robot speed
00274          * ###    
00275          * ###                                                                                         */
00276 
00277         // state machine
00278         switch (state) {
00279 
00280             case INIT:
00281 
00282                 enable_leds = 1;
00283                 enable_motors = 0;
00284 
00285                 state = ROBOT_OFF;  // default transition
00286                 break;
00287             
00288             case ROBOT_OFF:
00289                 
00290                 if (robot_turned_on) {
00291                     enable_motors = 1;
00292                     robot_speed_desired(0) = TRANSLATIONAL_VELOCITY;
00293                     robot_speed_desired(1) = 0.0f;
00294                     state = MOVE_FORWARD;
00295                 }
00296                 break;
00297                 
00298             case MOVE_FORWARD:
00299 
00300                 if (!robot_turned_on) {
00301                     robot_speed_desired(0) = 0.0f;
00302                     robot_speed_desired(1) = 0.0f;
00303                     state = SLOWING_DOWN;
00304                 } else if ((irSensors[0].read() < DISTANCE_THRESHOLD) || (irSensors[1].read() < DISTANCE_THRESHOLD)) {
00305                     robot_speed_desired(0) = 0.0f;
00306                     robot_speed_desired(1) =  ROTATIONAL_VELOCITY;
00307                     state = TURN_LEFT;
00308                 } else if (irSensors[5].read() < DISTANCE_THRESHOLD) {
00309                     robot_speed_desired(0) = 0.0f;
00310                     robot_speed_desired(1) = -ROTATIONAL_VELOCITY;
00311                     state = TURN_RIGHT;
00312                 } else {
00313                     // leave it driving
00314                 }
00315                 break;
00316                 
00317             case TURN_LEFT:
00318 
00319                 if (!robot_turned_on) {
00320                     robot_speed_desired(1) = 0.0f;
00321                     state = SLOWING_DOWN;
00322 
00323                 } else if ((irSensors[0].read() > DISTANCE_THRESHOLD) && (irSensors[1].read() > DISTANCE_THRESHOLD) && (irSensors[5].read() > DISTANCE_THRESHOLD)) {
00324                     robot_speed_desired(0) = TRANSLATIONAL_VELOCITY;
00325                     robot_speed_desired(1) = 0.0f;
00326                     state = MOVE_FORWARD;
00327                 }
00328                 break;
00329                 
00330             case TURN_RIGHT:
00331 
00332                 if (!robot_turned_on) {
00333                     robot_speed_desired(1) = 0.0f;
00334                     state = SLOWING_DOWN;
00335                 } else if ((irSensors[0].read() > DISTANCE_THRESHOLD) && (irSensors[1].read() > DISTANCE_THRESHOLD) && (irSensors[5].read() > DISTANCE_THRESHOLD)) {
00336                     robot_speed_desired(0) = TRANSLATIONAL_VELOCITY;
00337                     robot_speed_desired(1) = 0.0f;
00338                     state = MOVE_FORWARD;
00339                 }
00340                 break;
00341                 
00342             case SLOWING_DOWN:
00343                 
00344                 if ((fabs(robot_speed_actual(0)) < VELOCITY_THRESHOLD) && (fabs(robot_speed_actual(1)) < VELOCITY_THRESHOLD)) {
00345                     enable_motors = 0;
00346                     state = ROBOT_OFF;
00347                 }
00348                 
00349                 break;
00350 
00351         }
00352 
00353         /* ###                                                                                         *
00354          * ### END STUDENT CODE: TASK 5
00355          * ########################################################################################### */
00356 
00357         /* ------------------------------------------------------------------------------------------- *
00358          * -- Inverse Kinematics
00359          * ------------------------------------------------------------------------------------------- */
00360     
00361         /* ########################################################################################### *
00362          * ### BEGIN STUDENT CODE: TASK 3: Compute the desired wheel speeds
00363          * ###
00364          * ### In the main task, the robot_speed_desired vector [x_dt, alpha_dt] in [m/s] and [rad/s] 
00365          * ### is set. From that we need to compute the desired wheel speeds (wheel_speed_desired) 
00366          * ### that are handed to the speed controller
00367          * ###                                                                                         */
00368 
00369         // transform robot coordinates to wheel speed
00370         wheel_speed_desired = Crobot2wheel * robot_speed_desired;
00371 
00372         /* ###                                                                                         *
00373          * ### END STUDENT CODE: TASK 3
00374          * ########################################################################################### */
00375 
00376         // smooth desired wheel_speeds
00377         trajectoryPlaners[0]->incrementToVelocity(wheel_speed_desired(0) / (2.0f * M_PI), main_task_period);
00378         trajectoryPlaners[1]->incrementToVelocity(wheel_speed_desired(1) / (2.0f * M_PI), main_task_period);
00379         wheel_speed_smooth << trajectoryPlaners[0]->getVelocity(), trajectoryPlaners[1]->getVelocity();
00380 
00381         // command speedController objects
00382         speedControllers[0]->setDesiredSpeedRPS(wheel_speed_smooth(0)); // set a desired speed for speed controlled dc motors M1
00383         speedControllers[1]->setDesiredSpeedRPS(wheel_speed_smooth(1)); // set a desired speed for speed controlled dc motors M2
00384         
00385         user_led = !user_led;
00386 
00387         /* ------------------------------------------------------------------------------------------- *
00388          * -- Printing to Console
00389          * ------------------------------------------------------------------------------------------- */
00390         
00391         // do only output via serial what's really necessary (this makes your code slow)
00392         //printf("%f, %f\r\n", wheel_speed_actual(0), wheel_speed_actual(1));
00393     }
00394 }
00395 
00396 static void user_button_pressed_fcn()
00397 {
00398     user_button_timer.start();
00399     user_button_timer.reset();
00400 }
00401 
00402 static void user_button_released_fcn()
00403 {
00404     // read timer and toggle do_execute_main_task if the button was pressed longer than the below specified time
00405     int user_button_elapsed_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(user_button_timer.elapsed_time()).count();
00406     user_button_timer.stop();
00407     if (user_button_elapsed_time_ms > 200) {
00408         robot_turned_on = !robot_turned_on;
00409     }
00410 }
00411 
00412 void main_task_trigger()
00413 {
00414     // set the trigger to resume the waiting main task
00415     event_flags.set(main_task_flag);
00416 }