File content as of revision 52:75927464bf9c:

#include <mbed.h>
#include <math.h>
#include <vector>

#include "PM2_Libary.h"
#include "Eigen/Dense.h"

#include "IRSensor.h"



/**
 * 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 ROBOT_OFF     = 1;      
const int MOVE_FORWARD  = 2;
const int TURN_LEFT     = 3;
const int TURN_RIGHT    = 4;
const int SLOWING_DOWN  = 5;


/* ###                                                                                         *
 * ### 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;                               
    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)
    auto 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
         * ###    
         * ###                                                                                         */

        // state machine
        switch (state) {

            case INIT:

                enable_leds = 1;
                enable_motors = 0;

                state = ROBOT_OFF;  // default transition
                break;
            
            case ROBOT_OFF:
                
                if (robot_turned_on) {
                    enable_motors = 1;
                    robot_speed_desired(0) = TRANSLATIONAL_VELOCITY;
                    robot_speed_desired(1) = 0.0f;
                    state = MOVE_FORWARD;
                }
                break;
                
            case MOVE_FORWARD:

                if (!robot_turned_on) {
                    robot_speed_desired(0) = 0.0f;
                    robot_speed_desired(1) = 0.0f;
                    state = SLOWING_DOWN;
                } else if ((irSensors[0].read() < DISTANCE_THRESHOLD) || (irSensors[1].read() < DISTANCE_THRESHOLD)) {
                    robot_speed_desired(0) = 0.0f;
                    robot_speed_desired(1) =  ROTATIONAL_VELOCITY;
                    state = TURN_LEFT;
                } else if (irSensors[5].read() < DISTANCE_THRESHOLD) {
                    robot_speed_desired(0) = 0.0f;
                    robot_speed_desired(1) = -ROTATIONAL_VELOCITY;
                    state = TURN_RIGHT;
                } else {
                    // leave it driving
                }
                break;
                
            case TURN_LEFT:

                if (!robot_turned_on) {
                    robot_speed_desired(1) = 0.0f;
                    state = SLOWING_DOWN;

                } else if ((irSensors[0].read() > DISTANCE_THRESHOLD) && (irSensors[1].read() > DISTANCE_THRESHOLD) && (irSensors[5].read() > DISTANCE_THRESHOLD)) {
                    robot_speed_desired(0) = TRANSLATIONAL_VELOCITY;
                    robot_speed_desired(1) = 0.0f;
                    state = MOVE_FORWARD;
                }
                break;
                
            case TURN_RIGHT:

                if (!robot_turned_on) {
                    robot_speed_desired(1) = 0.0f;
                    state = SLOWING_DOWN;
                } else if ((irSensors[0].read() > DISTANCE_THRESHOLD) && (irSensors[1].read() > DISTANCE_THRESHOLD) && (irSensors[5].read() > DISTANCE_THRESHOLD)) {
                    robot_speed_desired(0) = TRANSLATIONAL_VELOCITY;
                    robot_speed_desired(1) = 0.0f;
                    state = MOVE_FORWARD;
                }
                break;
                
            case SLOWING_DOWN:
                
                if ((fabs(robot_speed_actual(0)) < VELOCITY_THRESHOLD) && (fabs(robot_speed_actual(1)) < VELOCITY_THRESHOLD)) {
                    enable_motors = 0;
                    state = ROBOT_OFF;
                }
                
                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 = Crobot2wheel * 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 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) {
        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);
}