File content as of revision 16:11282b7ee726:

#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/lidar_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;
double speed=0.6;
bool control_type = 1;;
int32_t motor_option;

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

}

// Function to move robot (motor speed, motor speed, pulses to travel)
void move(float motorA, float motorB, int distance)
{
    float current_pulse_reading = 0;
    
    // - is forward on our robot
    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 motor_callback(const std_msgs::Int32& motor_dir)
{
    motor_option = motor_dir.data;
    thread2.signal_set(1);
}

void control_motor(void) {
    while (1)
        {   
            thread2.signal_wait(1);
            switch(motor_option) {
                //Forward 30cm
                case 0://49
                    if (control_type == 1){
                        move(-speed, -speed, 5960);
                        }
                    else if (control_type ==0){
                        move(-speed, -speed, 50);                    
                        } 
                    break;
            
                //Left 30 degrees
                case 1://50
                    if (control_type == 1 ){
                        move(speed, -speed, 894);
                        }
                    else if (control_type == 0){
                        move(speed, -speed, 50);
                        } 
                    break;
                    
                //Right 30 degrees
                case 2://51
                    if (control_type == 1 ){
                        move(-speed, speed, 894);
                        }
                    else if (control_type == 0 ){
                        move(-speed, speed, 30);
                        }     
                    break;
                    
                // Reverse  30cm
                case 3://52
                    if (control_type ==1 ){
                        move(speed, speed, 5960);
                        }
                    else if (control_type ==0 ){
                        move(speed, speed, 50); 
                        }
                    break;
                
                case 4://speed up
                    if (speed<1.0){
                        speed += 0.2;
                        }
                    break;
                    
                case 5://speed down
                    if (speed>0.2){
                        speed -= 0.2;
                        }
                    break;
    
                case 6: // switch control type
                    control_type =! control_type;
                    break;
                }
    
                /*
                case 8: //control type 3
                    control_type = 3;
                */
            //Reset speed and pulse count
            A.speed(0);
            B.speed(0);
            
            wheel_B.reset();
            wheel_A.reset();
            //nh.spinOnce();
    }
}

int main() 
{
    ros::NodeHandle  nh;
    nh.initNode();
    
    // ROS subscriber for motors control
    ros::Subscriber<std_msgs::Int32> 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();
        
    }
  
    
}