File content as of revision 41:7484471403aa:

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

#include "IRSensor.h"
#include "EncoderCounterROME2.h"
#include "Controller.h"

#include "Eigen/Dense.h"

# define M_PI 3.14159265358979323846  // number pi

// 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)
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

// 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;

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]

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};

// create IR sensor objects  
AnalogIn dist(PA_0);
DigitalOut enable(PG_1);
DigitalOut bit0(PF_0);
DigitalOut bit1(PF_1);
DigitalOut bit2(PF_2);

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);

// 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);

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() {

    // 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();
        
    enable = 1;
    
    enableMotorDriver = 0;
    state = ROBOT_OFF;

    Eigen::Matrix<float, 6, 1> irSensor_distance; // transform wheel to robot
    
    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++;
        }
        //*/
        /*
        // 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;
        */

        switch (state) {
            
            // enables Motors, sets translational speed and switches to MOVE_FORWARD
            case ROBOT_OFF:           
                if (do_execute_main_task) {
                    enableMotorDriver = 1;
                    controller.setTranslationalVelocity(TRANSLATIONAL_VELOCITY);
                    controller.setRotationalVelocity(0.0f);
                    state = MOVE_FORWARD;
                }
                break;

            // continue here    
            case MOVE_FORWARD:
                if (!do_execute_main_task) {
                    controller.setTranslationalVelocity(0);
                    controller.setRotationalVelocity(0);
                    state = SLOWING_DOWN;
                    break;
                }

                // ???
                if (irSensor_distance(2) < DISTANCE_THRESHOLD && irSensor_distance(2) < DISTANCE_THRESHOLD) {
                    controller.setTranslationalVelocity(-TRANSLATIONAL_VELOCITY/2);
                    controller.setRotationalVelocity(-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;
                }
                
                break;
                
            case TURN_LEFT:
                if (!do_execute_main_task) {
                    controller.setTranslationalVelocity(0);
                    controller.setRotationalVelocity(0);
                    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);
                    state = MOVE_FORWARD;
                    break;
                }
                break;
                
            case TURN_RIGHT:  
                if (!do_execute_main_task) {
                    controller.setTranslationalVelocity(0);
                    controller.setRotationalVelocity(0);
                    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);
                    state = MOVE_FORWARD;
                    break;
                }
                break;
                
            case SLOWING_DOWN:
                if (abs(controller.getActualTranslationalVelocity()) < VELOCITY_THRESHOLD 
                    && abs(controller.getActualRotationalVelocity()) > VELOCITY_THRESHOLD) {
                    state = ROBOT_OFF;
                    enableMotorDriver = 0;
                    state = ROBOT_OFF;
                }
                
                break;
                
            default:
                
                state = ROBOT_OFF;
        }

        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));
        
        // 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();
        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;
    }
}