File content as of revision 13:d41144f89195:

#include "mbed.h"
#include <ros.h>
#include <cstdlib>
#include <std_msgs/Empty.h>
#include <std_msgs/Int32.h>
#include <std_msgs/String.h>
#include <mbed_custom_msgs/lidar_msg.h>

#include "Sensor.h"
#include "Motor.h"
#include "QEI.h"

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

// Motors
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
Motor motor_left(PC_6, PB_15, PB_13);
Motor motor_right(PA_15, PC_7, PB_4);

// Distance settings
int nbPulse = 5691;


void pulse(int pulse)
{
    // Variable to allow for any pulse reading   
    float current_pulse_reading = wheel_B.getPulses();
    
    while(abs(current_pulse_reading) <= pulse)
    {
        current_pulse_reading = wheel_A.getPulses();
    }       

    motor_left.stop();
    motor_right.stop();
    
    wheel_A.reset();
    wheel_B.reset();
}

void controlMotor(const std_msgs::Int32& motor_dir)
{
    switch(motor_dir.data) {
        // Stop motors
        case 0:
            motor_left.stop();
            motor_right.stop();
            break;
            
        // Move forward
        case 1:
            motor_left.moveForward();
            motor_right.moveForward();
            pulse(nbPulse); 
            break;
    
        // Move left
        case 2:
            motor_left.moveBackward();
            motor_right.moveForward();     
            pulse(3000); 
            break;
            
        // Move right
        case 3:
            motor_left.moveForward();
            motor_right.moveBackward();
            pulse(3000); 
            break;
            
        // Reverse
        case 4:
            motor_left.moveBackward();
            motor_right.moveBackward();
            pulse(nbPulse);
            break;
    }
}

void setDistance(const std_msgs::Int32& distance)
{
    nbPulse = (int) 189.7 * distance.data;   
}

int main() 
{
    ros::NodeHandle  nh;
    nh.initNode();
    
    // ROS publisher for sensor readings
    mbed_custom_msgs::lidar_msg lidar_msg;
    ros::Publisher lidar_pub("lidar_reading", &lidar_msg);
    
    // ROS subscriber for motors control
    ros::Subscriber<std_msgs::Int32> motor_sub("motor_control", &controlMotor);
    
    // ROS subscriber for distance setting
    ros::Subscriber<std_msgs::Int32> distance_sub("distance_setting", &setDistance);

    nh.advertise(lidar_pub);
    nh.subscribe(motor_sub);
    nh.subscribe(distance_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();
    
    //Set Speeds
    motor_left.setSpeed(0.5f);
    motor_right.setSpeed(0.5f);
  
    while (1)
    {   
        lidar_msg.sensor_forward = sensor_forward.read();
        lidar_msg.sensor_back = sensor_back.read();
        lidar_msg.sensor_left = sensor_left.read();
        lidar_msg.sensor_right = sensor_right.read();         
        lidar_pub.publish(&lidar_msg);
    
        nh.spinOnce();
              
        wait_ms(10); 
    } 
}