File content as of revision 21:2dcd6d0d004d:

#include "mbed.h"
#include <ros.h>
#include <std_msgs/Empty.h>
#include <std_msgs/Int32.h>
#include <std_msgs/String.h>
#include <mbed_custom_msgs/motor_msg.h>
#include <motordriver.h>
#include "QEI.h"
#include <cstdlib>
#include "Sensor.h"

// Set up serial to pc
//Serial pc(SERIAL_TX, SERIAL_RX); 

//Threading for sensor readings
Thread thread1;
Thread thread2;
Mutex SerialMutex;
        
// Set up I²C on the STM32 NUCLEO-401RE 
#define addr1   (0x29) 
#define addr2   (0x2A)  
#define addr3   (0x2B)
#define addr4   (0x2C)

#define S1 PC_8
#define S2 PC_9
#define S3 PC_10
#define S4 PC_11
#define S5 PC_12
#define S6 PD_2
#define S7 PG_2
#define S8 PG_3 

// VL6180x sensors
Sensor sensor_back(I2C_SDA, I2C_SCL, S1);
Sensor sensor_left(I2C_SDA, I2C_SCL, S3);
Sensor sensor_forward(I2C_SDA, I2C_SCL, S5);
Sensor sensor_right(I2C_SDA, I2C_SCL, S7);

// Floats for sensor readings
float sensor_forward_reading;
float sensor_back_reading;
float sensor_left_reading;
float sensor_right_reading;

//Initialised motor speed
double speed=0.6;
//used to change control to set distances of continous movement
bool control_type = 1;
//used for the switch cases and the distance travelled
int32_t motor_option;
int32_t pulse = 6000;

// Set motorpins for driving
Motor A(PB_13, PB_15, PC_6, 1); // pwm, fwd, rev, can brake
Motor B(PB_4, PC_7, PA_15, 1); // pwm, fwd, rev, can brake
QEI wheel_A (PB_12, PA_4, NC, 3576, QEI::X4_ENCODING); //counts per rev = 12 * 298:1, use quadrature encoding
QEI wheel_B (PB_5, PB_3, NC, 3576, QEI::X4_ENCODING); //counts per rev = 12 * 298:1, use quadrature encoding

//Thread function to update sensor readings
void flip() {
    while(1) {
        SerialMutex.lock();
        
        // Sensor readings
        sensor_forward_reading = sensor_forward.read();
        sensor_back_reading = sensor_back.read();
        sensor_left_reading = sensor_left.read();
        sensor_right_reading = sensor_right.read(); 
        
        SerialMutex.unlock();
        thread1.wait(2);
    }
}

void motor_callback(const mbed_custom_msgs::motor_msg& msg)
{
    motor_option = msg.dir;
    if( (msg.distance <= 0) or (msg.distance > 120) ) {
        pulse = 6000;
    } else {
        pulse = msg.distance * 200; // Est 200 pulses per cm
    }
    
    thread2.signal_set(1);
}

// Function to move robot
void move(float motorA, float motorB, int distance) {
    float current_pulse_reading = 0;
    
    // Set motors speed
    A.speed(motorA);
    B.speed(motorB);
    
    while(abs(current_pulse_reading) <= distance) {
        current_pulse_reading = wheel_A.getPulses();
        //Stops Robot moving forward if front sensor detects
        if (sensor_back_reading <255 and motorA>0 and motorB>0){
            return;
        }

        //Stops robot reversing if back sensors detects
        if (sensor_forward_reading <255 and motorA<0 and motorB<0){
            return;
        }
    }
}

void square()
{
    float current_pulse_reading = 0;
    
    for(int i = 0; i<4 ; i++) {
        current_pulse_reading = 0;
        while(abs(current_pulse_reading) <= 6000){
            A.speed(-speed);
            B.speed(-speed);
            
            current_pulse_reading = wheel_A.getPulses();
            //Stops Robot moving forward if front sensor detects
            if (sensor_forward_reading <255){
                return;
            }
        }
        // need to set motor speed to 0 
        A.speed(0);
        B.speed(0);
        wait(0.05);
        
        A.speed(-speed);
        B.speed(speed);
        while(abs(current_pulse_reading) <= 8688){
            current_pulse_reading = wheel_A.getPulses();
        }
        
        wheel_B.reset();
        wheel_A.reset();
        
    }
    
    // Stop motors
    A.speed(0);
    B.speed(0);
    wait(0.05);
}

void dance()
{
    wait(5);// required for audacity to play music
    
    for (int i =0; i<5; i++) {
        int distance  = (rand()%16000+2000);
        float A_speed = (rand()%2-1);
        float B_speed = (rand()%2-1);
        if((A_speed < 0.4) and (A_speed > -0.4)){
            A_speed = 0.4;
        }
        if((B_speed < 0.4) and (B_speed > -0.4)){
            B_speed = -0.4;
        }
        move(A_speed,B_speed,distance);
        wheel_B.reset();
        wheel_A.reset();
    }    
}

/*    float current_pulse_reading = 0;
    while(abs(current_pulse_reading) <= 5388)
    {
    current_pulse_reading = wheel_A.getPulses();
    A.speed(-speed);
    B.speed(0.0);
    }
    wait(0.05);
    current_pulse_reading = 0;
    wheel_B.reset();
    wheel_A.reset();
    while(abs(current_pulse_reading) <= 5388)
    {
    current_pulse_reading = wheel_A.getPulses();
    A.speed(speed);
    B.speed(0);
    }
    wait(0.05);
    wheel_B.reset();
    wheel_A.reset();
    current_pulse_reading = 0;
    while(abs(current_pulse_reading) <= 5388)
    {
    current_pulse_reading = wheel_B.getPulses();
    A.speed(0);
    B.speed(-speed);
    }
    wait(0.05);
    wheel_B.reset();
    wheel_A.reset();
    current_pulse_reading = 0;
    while(abs(current_pulse_reading) <=5388)
    {
    current_pulse_reading = wheel_B.getPulses();
    A.speed(0);
    B.speed(speed);
    }
    wait(0.05);
    wheel_B.reset();
    wheel_A.reset();
    current_pulse_reading = 0;
    while(abs(current_pulse_reading) <21504)
    {
    current_pulse_reading = wheel_A.getPulses();
    A.speed(-speed);
    B.speed(speed);
    }
    wait(0.05);
    wheel_B.reset();
    wheel_A.reset();
    current_pulse_reading = 0;
    while(abs(current_pulse_reading) <6000)
    {
    current_pulse_reading = wheel_A.getPulses();
    A.speed(-speed);
    B.speed(-speed);
    }
    wait(0.05);
    wheel_B.reset();
    wheel_A.reset();
    current_pulse_reading = 0;
    while(abs(current_pulse_reading) <6000)
    {
    current_pulse_reading = wheel_A.getPulses();
    A.speed(speed);
    B.speed(speed);
    }
    wait(0.05);
    A.speed(0);
    B.speed(0);
*/
    

// Function to set robot direction and distance according to data published on motor_control topic
void control_motor(void) {
    
    while (1){   
        thread2.signal_wait(1);
        
        switch(motor_option) {
            
        // Do nothing - default value published on motor_contorl topic
        case 0: 
            break;
            
        //Forward
        case 1://49
            if (control_type == 1){
                move(-speed, -speed, pulse);
            }
            else if (control_type ==0){
                move(-speed, -speed, 50);                    
            } 
            break;

        //Left 
        case 2://50
            if (control_type == 1 ){
                move(speed, -speed, pulse/6.7); // Divide by 6.7 to convert to degrees
            }
            else if (control_type == 0){
                move(speed, -speed, 100);
            } 
            break;
                    
        //Right 
        case 3://51
            if (control_type == 1 ){
                move(-speed, speed, pulse/6.7);
            }
            else if (control_type == 0 ){
                move(-speed, speed, 60);
            }     
            break;
                    
        // Reverse
        case 4://52
            if (control_type ==1 ){
                move(speed, speed, pulse);
            }
            else if (control_type ==0 ){
                move(speed, speed, 50); 
            }
            break;
               
        // Speed up        
        case 5:
            if (speed < 1.0){
                speed += 0.2;
            }
            break;
               
        // Speed down     
        case 6:
            if (speed>0.2){
                 speed -= 0.2;
            }
            break;
    
        // Switch control type
        case 7: 
            control_type =! control_type;
            break;
                
        // Turn around
        case 8:
            if (control_type == 1 ){
                move(-speed, speed, 5376);
            }
            break; 
    
        // Draw a square        
        case 9:
            square();
            break;
        
        // Random dance
        case 10:
            dance();
            break;
        }
                   
        //Reset speed and pulse count
        A.speed(0);
        B.speed(0);
            
        wheel_B.reset();
        wheel_A.reset();
    }
}

int main() 
{
    ros::NodeHandle  nh;
    nh.initNode();
    
    // ROS subscriber for motors control
    ros::Subscriber<mbed_custom_msgs::motor_msg> motor_sub("motor_control", &motor_callback);
    nh.subscribe(motor_sub);
    
    // load settings onto VL6180X sensors 
    sensor_forward.init();
    // change default address of sensor 2
    sensor_forward.changeAddress(addr2);
    
    sensor_right.init();
    // change default address of sensor 3
    sensor_right.changeAddress(addr3);

    sensor_back.init();
    // change default address of sensor 4
    sensor_back.changeAddress(addr4);
    
    sensor_left.init();
    
    thread1.start(callback(&flip));
    thread2.start(callback(&control_motor));
    
    while(1) {
        nh.spinOnce(); 
    }
}