File content as of revision 14:08047fde54f6:

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

Thread thread1;
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 

/*Ticker Sensor_readings()
rangeForward = sensor_forward.read();
*/

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

float sensor_forward_reading;
float sensor_back_reading;
float sensor_left_reading;
float sensor_right_reading;

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

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

}

void move(float motorA, float motorB, int distance)
{
    // Variables to allow for any pulse reading
    float current_pulse_reading = 0;
    
    // - is forward on our robot
    A.speed(motorA);
    B.speed(motorB);
    // loop for moving forward
        while(abs(current_pulse_reading) <= distance)
        {
            //pc.printf("current_pulse_reading 1= %f\r\n", current_pulse_reading);
            current_pulse_reading = wheel_A.getPulses();
            //SerialMutex.lock();
            //pc.printf("forward = %f, back = %f\n\r", sensor_forward_reading, sensor_back_reading);
            //SerialMutex.unlock();
            if (sensor_back_reading <255 and motorA>0 and motorB>0){
                return;
                }
            if (sensor_forward_reading <255 and motorA<0 and motorB<0){
                return;
                }
        }
        
    A.speed(0);
    B.speed(0);
    
    wheel_B.reset();
    wheel_A.reset();

}


void controlMotor(const std_msgs::Int32& motor_dir)
{
    switch(motor_dir.data) {
 
        case 0://49
            move(-0.5, -0.5, 10000); 
            break;
    
        // Move left
        case 1://50
            move(0.5, -0.5, 5000); 
            break;
            
        // Move right
        case 2://51
            move(-0.5, 0.5, 5000); 
            break;
            
        // Reverse
        case 3://52
            move(0.5, 0.5, 10000);
            break;
            
        case default:
            A.speed(0);
            B.speed(0);
    }
    A.speed(0);
    B.speed(0);
//    switch(motor_dir.data) {
//        case 0:
//            motor_left.stop();
//            motor_right.stop();
//            break;
//            
//        // Move forward
//        case 1:
//            move("forward", -0.5, -0.5); 
//            break;
//    
//        // Move left
//        case 2:
//            move("left", 0.5, -0.5, 3000, -1); 
//            break;
//            
//        // Move right
//        case 3:
//            move("right", -0.5, 0.5, 3000, 1); 
//            break;
//            
//        // Reverse
//        case 4:
//            move("reverse", 0.5, 0.5, 5691, -1);
//            break;
//    }
}





int main() 
{
    ros::NodeHandle  nh;
    nh.initNode();
    
    // ROS subscriber for motors control
    ros::Subscriber<std_msgs::Int32> motor_sub("motor_control", &controlMotor);

    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));
    
    while (1)
    {   
        nh.spinOnce();
        wait(1);
  
    }
}