Revision 12:82b8fe254222, committed 2020-01-05

Comitter:

Jonathan738

Date:

Sun Jan 05 15:42:22 2020 +0000

Parent:

11:0b9098ec11c7

Commit message:

Added working version of TOF code

Changed in this revision

diff -r 0b9098ec11c7 -r 82b8fe254222 Battery_Monitor.cpp
--- a/Battery_Monitor.cpp	Thu Dec 19 00:13:38 2019 +0000
+++ b/Battery_Monitor.cpp	Sun Jan 05 15:42:22 2020 +0000
@@ -16,15 +16,14 @@
 
 void Battery_Monitor::battCheck(void)
 {
-    float Read_V_Batt = this->V_Batt.read();
+    float Read_V_Batt = this->V_Batt.read() * 16.67f;
 
     float temp_batLev = Read_V_Batt - Minimum_VBatt;
     this->Battery_Level = (Maximum_VBatt - Minimum_VBatt) / temp_batLev;
 
-
     Global_Battery_Level_Mutex.lock();
-    Global_Battery_Level = Read_V_Batt;
-    Global_Battery_Value = this->Battery_Level;
+    Global_Battery_Value = Read_V_Batt;
+    Global_Battery_Level = this->Battery_Level;
     if(Battery_Update_Flag == false)
     {
         Battery_Update_Flag = true;

diff -r 0b9098ec11c7 -r 82b8fe254222 Battery_Monitor.hpp
--- a/Battery_Monitor.hpp	Thu Dec 19 00:13:38 2019 +0000
+++ b/Battery_Monitor.hpp	Sun Jan 05 15:42:22 2020 +0000
@@ -4,7 +4,7 @@
 #include "rtos.h"
 
 #define Minimum_VBatt 3.6f
-#define Maximum_VBatt 5.0f
+#define Maximum_VBatt 4.2f
 #define PWM_Freq 200.0f
 
 #ifndef Define_ONCE_Battery_Monitoring

diff -r 0b9098ec11c7 -r 82b8fe254222 General.hpp
--- a/General.hpp	Thu Dec 19 00:13:38 2019 +0000
+++ b/General.hpp	Sun Jan 05 15:42:22 2020 +0000
@@ -26,15 +26,16 @@
 /******************************************************************************/
 /*  Defentions of Pins and Topics for ROS and attached peripherals            */
 /******************************************************************************/
-#define InPut_Topic     "/Nucelo/Command"
-#define OutPut_Topic    "/Nucleo/State"
+#define Control_Topic   "/Nucleo/cmd_vel"
+#define TOF_Topic       "/Nucleo/TOF_Ranges"
+#define Status_Topic    "/Nucleo/Status"
 
 #define Serial_Error_Tx SERIAL_TX
 #define Serial_Error_Rx SERIAL_RX
 
 #define ROS_Tx PD_5
 #define ROS_Rx PD_6
-#define ROS_BaudRate 57600
+#define ROS_BaudRate 115200
 /******************************************************************************/
 
 

diff -r 0b9098ec11c7 -r 82b8fe254222 Motors.cpp
--- a/Motors.cpp	Thu Dec 19 00:13:38 2019 +0000
+++ b/Motors.cpp	Sun Jan 05 15:42:22 2020 +0000
@@ -51,27 +51,8 @@
 int previousStateB = currentStateB;
 int nextmoveflagB = 1;
 
-//X is forward, Z is upwards, Y is sideways in this structure.
-//Order is X, Y, Z linear, then X, Y, Z rotational, from 1:6.
-void cmdvelresponse(const std_msgs::Int32MultiArray& vinput) {
-    //Extracting the only commands that will be used in the multiarray, and assuming that the int value attached to each point is 100 times
-    //the intended PWM constant for that movement command (e.g. a value of 50 in msg.data[6] would give a PWM duty cycle of 0.5 for rotation):
-    float xlin = 0.01 * vinput.data[1];
-    float zrot = 0.01 * vinput.data[6];
-    //assume rotation needs to be taken care of first, generally, then linear movement
-    if(zrot != 0) { //assume positive z is clockwise, negative is anticlockwise, A is left, B is right
-        A.speed(-zrot);
-        B.speed(zrot);
-    } else if(xlin != 0) {
-        A.speed(xlin);
-        B.speed(xlin);
-    } else {
-        A.stop(1);
-        B.stop(1);
-    }
-}
-
-void initialize() {
+void initialize()
+{
     //every time a rising or falling edge is detected for the A or B channels, call the encoder counting interrupt for the respective wheel.
     //this corresponds to the counting program for the encoders for each wheel, given that quadrature encoding is used.
     encoderAchannel1.rise(&callbackA);
@@ -93,16 +74,17 @@
     previousStateB = currentStateB;
 }
 
-void callbackA() {
+void callbackA()
+{
     ////////
     //insert the 'if X, carry out the rest of this code' debouncing check here, ensure the delay is minimal enough to avoid hindering 3576 measurements per rotation at max speed
     ////////
-    
+
     volatile int changeA = 0;
     channelAstate1 = encoderAchannel1.read();
     channelAstate2 = encoderAchannel2.read();
     currentStateA = (channelAstate1 << 1) | (channelAstate2);
-    
+
     if (((currentStateA ^ previousStateA) != INVALID) && (currentStateA != previousStateA)) {
         //2 bit state. Right hand bit of prev XOR left hand bit of current
         //gives 0 if clockwise rotation and 1 if counter clockwise rotation.
@@ -122,18 +104,19 @@
     }
 }
 
-void callbackB() {
+void callbackB()
+{
     ////////
     //insert the 'if X, carry out the rest of this code' debouncing check here, ensure the delay is minimal enough to avoid hindering 3576 measurements per rotation at max speed
     ////////
-    
+
     //every time this function is called, increment or decrement the encoder count depending on which direction the relevant wheel is moving
-    
+
     volatile int changeB = 0;
     channelBstate1 = encoderBchannel1.read();
     channelBstate2 = encoderBchannel2.read();
     currentStateB = (channelBstate1 << 1) | (channelBstate2);
-    
+
     if (((currentStateB ^ previousStateB) != INVALID) && (currentStateB != previousStateB)) {
         //2 bit state. Right hand bit of prev XOR left hand bit of current
         //gives 0 if clockwise rotation and 1 if counter clockwise rotation.
@@ -153,7 +136,8 @@
     }
 }
 
-void move(const std_msgs::Int32& dinput) {
+void move(const std_msgs::Int32& dinput)
+{
     if(nextmoveflagA == 1 && nextmoveflagB == 1) {
         //import the relevant ROS message and convert it to a usable encoder target
         float distance = dinput.data;
@@ -168,7 +152,8 @@
     }
 }
 
-void tempMove(float distance) {
+void tempMove(float distance)
+{
     if(nextmoveflagA == 1 && nextmoveflagB == 1) {
         float mtarget = distance/(2*3.142*radius)*benchmark;
         atarget = ceil(mtarget + aprevtarget);
@@ -180,7 +165,8 @@
     }
 }
 
-void rotate(const std_msgs::Int32& rinput) {
+void rotate(const std_msgs::Int32& rinput)
+{
     if(nextmoveflagA == 1 && nextmoveflagB == 1) {
         //import the relevant ROS message and convert it to usable encoder targets
         float degrees = rinput.data;
@@ -195,7 +181,8 @@
     }
 }
 
-void tempRotate(float degrees) {
+void tempRotate(float degrees)
+{
     if(nextmoveflagA == 1 && nextmoveflagB == 1) {
         float rtarget = (degrees/(360*3))*9.15*benchmark;
         atarget = ceil(rtarget + aprevtarget);
@@ -207,7 +194,8 @@
     }
 }
 
-void driveMotors() {
+void driveMotors()
+{
     //depending on the direction the wheels have to move, set their pwms to either positive or negative so each motor moves correctly
     if(atarget > aprevtarget) {
         apwm = pwm;
@@ -241,3 +229,4 @@
     aprevtarget = atarget;
     bprevtarget = btarget;
 }
+

diff -r 0b9098ec11c7 -r 82b8fe254222 Motors.hpp
--- a/Motors.hpp	Thu Dec 19 00:13:38 2019 +0000
+++ b/Motors.hpp	Sun Jan 05 15:42:22 2020 +0000
@@ -8,9 +8,12 @@
 #include "Battery_Monitor.hpp"
 #include "Pins.h"
 
+#include <std_msgs/Int32.h>
+
 #ifndef Define_ONCE_Motors
 #define Define_ONCE_Motors
 
+void Motor_Handler();
 void initialize();
 void move(const std_msgs::Int32& input);
 void rotate(const std_msgs::Int32& input);
@@ -19,6 +22,5 @@
 void driveMotors(void);
 void callbackA(void);
 void callbackB(void);
-void cmdvelresponse(const std_msgs::Int32MultiArray& vinput);
 
 #endif
\ No newline at end of file

diff -r 0b9098ec11c7 -r 82b8fe254222 ROS_Handler.cpp
--- a/ROS_Handler.cpp	Thu Dec 19 00:13:38 2019 +0000
+++ b/ROS_Handler.cpp	Sun Jan 05 15:42:22 2020 +0000
@@ -24,12 +24,11 @@
 /******************************************************************************/
 
 ros::NodeHandle_<HaseHardware>   Node_;
-void Control_CallBack(const std_msgs::Int32MultiArray& msg);
-ros::Subscriber<std_msgs::Int32MultiArray> Control_Subscriber("/cmd_vel", &Control_CallBack);
+ros::Subscriber<std_msgs::Float32MultiArray> Control_Subscriber(Control_Topic, &Control_CallBack);
 std_msgs::String State_msg;
 std_msgs::String TOF_msg;
-ros::Publisher Status_PUB("/Nucleo/State", &State_msg);
-ros::Publisher TOF_PUB("/Nucleo/TOF_data", &TOF_msg);
+ros::Publisher Status_PUB(Status_Topic, &State_msg);
+ros::Publisher TOF_PUB(TOF_Topic, &TOF_msg);
 
 extern Mutex Global_Battery_Level_Mutex;
 extern float Global_Battery_Level;
@@ -40,8 +39,14 @@
 extern bool TOF_Range_Flag;
 extern Mutex Global_TOF_Range_Mutex;
 
+float LIN_Val, ANG_Val;
+Mutex Duty_Mutex;
+
 // Main function for thread to handle ROS comms
 void ROS_Handler(void){
+    LIN_Val = 0;
+    ANG_Val = 0;
+       
     Node_.initNode();
     Node_.advertise(Status_PUB);
     Node_.advertise(TOF_PUB);
@@ -56,7 +61,9 @@
             Battery_Update_Flag = false;
             Global_Battery_Level_Mutex.unlock();
             char buffer[50];
-            sprintf(buffer, "Battery Level : %1.3fV     | Battery Voltage %1.3fV", Battery_Level, Battery_Value);
+            Duty_Mutex.lock();
+            sprintf(buffer, "Battery Level : %1.3f%     | Battery Voltage %1.3fV    | Linear Duty %1.3f     | Angular Duty %1.3f    |", (Battery_Level*100), Battery_Value, LIN_Val, ANG_Val);
+            Duty_Mutex.unlock();
             State_msg.data = buffer;
             Status_PUB.publish(&State_msg);
         } else {
@@ -85,23 +92,52 @@
     }
 }
 
-void Control_CallBack(const std_msgs::Int32MultiArray& msg)
+int Remap(float value, float start1, float stop1, float start2, float stop2) 
+{
+    return (int)(start2 + (stop2 - start2) * ((value - start1) / (stop1 - start1)));
+}
+
+
+
+void Control_CallBack(const std_msgs::Float32MultiArray& cmd_vel_msg)
 {
     /**************************************************************************/
     //Extracting the only commands that will be used in the multiarray, and assuming that the int value attached to each point is 100 times
     //the intended PWM constant for that movement command (e.g. a value of 50 in msg.data[6] would give a PWM duty cycle of 0.5 for rotation):
-    float xlin = 0.01 * msg.data[1];
-    float zrot = 0.01 * msg.data[6];
+    float xlin, zang;
+    if((cmd_vel_msg.data[0] <= 1.0f) && (cmd_vel_msg.data[0] >= -1.0f))
+    {
+        xlin = cmd_vel_msg.data[0];
+        Duty_Mutex.lock();
+        LIN_Val = xlin;
+        Duty_Mutex.unlock();;
+    }
+    if((cmd_vel_msg.data[1] <= 1.0f) && (cmd_vel_msg.data[1] >= -1.0f))
+    {
+        zang = cmd_vel_msg.data[1];
+        Duty_Mutex.lock();
+        ANG_Val = zang;
+        Duty_Mutex.unlock();
+    }
+    
     //assume rotation needs to be taken care of first, generally, then linear movement for this basic controller
-    if(zrot != 0) { //assume positive z is clockwise, negative is anticlockwise, A is left motor, B is right motor (viewed from bottom layer battery switch direction)
-        A.speed(-zrot);
-        B.speed(zrot);
+    //assume positive z is clockwise, negative is anticlockwise, A is left motor, B is right motor (viewed from bottom layer battery switch direction)
+    if(zang == 0.0f && xlin == 0.0f) {
+        A.stop(0.5f);
+        B.stop(0.5f);
+        wait(1);
+        A.coast();
+        B.coast();
+    } else if(zang != 0.0f && xlin !=0.0f) {
+        // Needs testing
+        A.speed((-xlin + zang)/2.0f);
+        B.speed((-xlin + -zang)/2.0f);
     } else if(xlin != 0) {
-        A.speed(xlin);
-        B.speed(xlin);
-    } else {
-        A.stop(1);
-        B.stop(1);
+        A.speed(-xlin);
+        B.speed(-xlin);
+    } else if(zang != 0) {
+        A.speed(zang);
+        B.speed(-zang);
     }
     /**************************************************************************/
 }

diff -r 0b9098ec11c7 -r 82b8fe254222 ROS_Handler.hpp
--- a/ROS_Handler.hpp	Thu Dec 19 00:13:38 2019 +0000
+++ b/ROS_Handler.hpp	Sun Jan 05 15:42:22 2020 +0000
@@ -4,18 +4,13 @@
 #include "rtos.h"
 #include "Pins.h"
 
-#include <std_msgs/Empty.h>
-#include <std_msgs/Int32.h>
-#include <std_msgs/Int32MultiArray.h>
 #include <std_msgs/Float32MultiArray.h>
-#include "std_msgs/String.h"
-#include <geometry_msgs/Twist.h>
-#include <geometry_msgs/Vector3.h>
+#include <std_msgs/String.h>
 
 #ifndef Define_ONCE_ROShandler
 #define Define_ONCE_ROShandler
 
 void ROS_Handler(void);
-void CallBack(const geometry_msgs::Twist& msg);
+void Control_CallBack(const std_msgs::Float32MultiArray& cmd_vel_msg);
 
 #endif
\ No newline at end of file

diff -r 0b9098ec11c7 -r 82b8fe254222 VL6180.cpp
--- a/VL6180.cpp	Thu Dec 19 00:13:38 2019 +0000
+++ b/VL6180.cpp	Sun Jan 05 15:42:22 2020 +0000
@@ -1,59 +1,229 @@
 #include "mbed.h"
-#include "General.hpp"
-#include "rtos.h"
+//#include "General.hpp"
+//#include "rtos.h"
 #include "Pins.h"
 #include "VL6180.hpp"
 #include <vector>
 
-//#define DEBUG_TOF
+#define DEBUG_TOF
 #ifdef DEBUG_TOF
 Serial debug_term(SERIAL_TX, SERIAL_RX);
 #endif
-
-
-#define ROS_TOF
+//
+////#define ROS_TOF
 int Global_TOF_Ranges[num_VL6180];
 bool TOF_Range_Flag;
 Mutex Global_TOF_Range_Mutex;
 
+BusOut SHDN_Pins(PC_9, PC_11, PD_2, PG_3);
+
+I2C i2c(I2C_SDA, I2C_SCL); // Set up I²C on the STM32 NUCLEO-401RE
+
+///////////////////////////////////////////////////////////////////
+// Split 16-bit register address into two bytes and write
+// the address + data via I²C
+///////////////////////////////////////////////////////////////////
+
+void WriteByte_Uninitialized(wchar_t reg, char data, char addr) {    
+    char data_write[3];
+    data_write[0] = (reg >> 8) & 0xFF;;
+    // MSB of register address
+    data_write[1] = reg  & 0xFF;
+    // LSB of register address
+    data_write[2] = data & 0xFF;
+    i2c.write(addr, data_write, 3);
+}
+
+///////////////////////////////////////////////////////////////////
+// Split 16-bit register address into two bytes and write
+// required register address to VL6180 and read the data back
+///////////////////////////////////////////////////////////////////
+
+char ReadByte(wchar_t reg, char addr) {
+    char data_write[2];
+    char data_read[1];
+   
+    data_write[0] = (reg >> 8) & 0xFF; // MSB of register address
+    data_write[1] = reg  & 0xFF; // LSB of register address
+   
+    i2c.write(addr, data_write, 2);
+    i2c.read(addr, data_read, 1);
+    return data_read[0];
+}
+
+///////////////////////////////////////////////////////////////////
+// load settings
+///////////////////////////////////////////////////////////////////
+
+int VL6180_Init(char addr) {    
+    char reset;    
+   
+    // check to see has it be Initialised already
+    reset = ReadByte(0x016, addr);    
+    if (reset==1)
+    {      
+        ///////////////////////////////////////////////////////////////////
+        // DEFAULT SETTINGS
+        ///////////////////////////////////////////////////////////////////
+       
+        // Mandatory : private registers
+        WriteByte_Uninitialized(0x0207, 0x01, addr);
+        WriteByte_Uninitialized(0x0208, 0x01, addr);
+        WriteByte_Uninitialized(0x0096, 0x00, addr);
+        WriteByte_Uninitialized(0x0097, 0xfd, addr);
+        WriteByte_Uninitialized(0x00e3, 0x01, addr);
+        WriteByte_Uninitialized(0x00e4, 0x03, addr);
+        WriteByte_Uninitialized(0x00e5, 0x02, addr);
+        WriteByte_Uninitialized(0x00e6, 0x01, addr);
+        WriteByte_Uninitialized(0x00e7, 0x03, addr);
+        WriteByte_Uninitialized(0x00f5, 0x02, addr);
+        WriteByte_Uninitialized(0x00d9, 0x05, addr);
+        WriteByte_Uninitialized(0x00db, 0xce, addr);
+        WriteByte_Uninitialized(0x00dc, 0x03, addr);
+        WriteByte_Uninitialized(0x00dd, 0xf8, addr);
+        WriteByte_Uninitialized(0x009f, 0x00, addr);
+        WriteByte_Uninitialized(0x00a3, 0x3c, addr);
+        WriteByte_Uninitialized(0x00b7, 0x00, addr);
+        WriteByte_Uninitialized(0x00bb, 0x3c, addr);
+        WriteByte_Uninitialized(0x00b2, 0x09, addr);
+        WriteByte_Uninitialized(0x00ca, 0x09, addr);
+        WriteByte_Uninitialized(0x0198, 0x01, addr);
+        WriteByte_Uninitialized(0x01b0, 0x17, addr);
+        WriteByte_Uninitialized(0x01ad, 0x00, addr);
+        WriteByte_Uninitialized(0x00ff, 0x05, addr);
+        WriteByte_Uninitialized(0x0100, 0x05, addr);
+        WriteByte_Uninitialized(0x0199, 0x05, addr);
+        WriteByte_Uninitialized(0x01a6, 0x1b, addr);
+        WriteByte_Uninitialized(0x01ac, 0x3e, addr);
+        WriteByte_Uninitialized(0x01a7, 0x1f, addr);
+        WriteByte_Uninitialized(0x0030, 0x00, addr);
+        WriteByte_Uninitialized(0x016, 0x00, addr); //change fresh out of set status to 0    
+    }  
+    else {
+        return -1;  
+    }  
+    return 0;
+}
+
+///////////////////////////////////////////////////////////////////
+// Start a range measurement in single shot mode
+///////////////////////////////////////////////////////////////////
+
+int VL6180_Start_Range(char addr) {    
+    WriteByte_Uninitialized(0x018,0x03, addr);  
+    return 0;
+}
+
+///////////////////////////////////////////////////////////////////
+// poll for new sample ready ready
+///////////////////////////////////////////////////////////////////
+
+int VL6180_Poll_Range(char addr) {    
+    char status;    
+    char range_status;  
+         
+    // check the status    
+    status = ReadByte(0x04f,addr);            
+    range_status = status & 0x07;
+           
+    // wait for new measurement ready status    
+    while (range_status !=  0x00)
+    {        
+        status = ReadByte(0x04f,addr);        
+        range_status = status & 0x07;        
+        //wait_ms(50); // (can be removed)        
+    }
+   
+    return 0;    
+}
+
+
+///////////////////////////////////////////////////////////////////
+// Read range result (mm)
+///////////////////////////////////////////////////////////////////
+
+int VL6180_Read_Range(char addr) {    
+    int range;    
+    range=ReadByte(0x062, addr);    
+    return range;
+}
+
+///////////////////////////////////////////////////////////////////
+// clear interrupts
+///////////////////////////////////////////////////////////////////
+
+int VL6180_Clear_Interrupts(char addr) {    
+    WriteByte_Uninitialized(0x015,0x07, addr);    
+    return 0;
+}
+
+///////////////////////////////////////////////////////////////////
+//  Main Program loop
+///////////////////////////////////////////////////////////////////
 /*------------------------------------------------------------------------------
 //  Main Program loop
 ------------------------------------------------------------------------------*/
-int TOF_Thread()
+void TOF_Handler()
 {
-    /* Pins are (PC_9, PC_11, PD_2, PG_3) */
-    I2C i2c_bus(I2C_SDA, I2C_SCL); // Set up I²C on the STM32 NUCLEO-401RE
+    SHDN_Pins = 0; // 0x0 or 0b0000
+    wait_ms(0.5);
+ 
+    SHDN_Pins = 2; // 0x2 or 0b0010
 
-    //Dynamically create VL6180 objects for the TOF sensors
-    PinName SHDN_Pins[num_VL6180] = SHDN_Pins_Cell;
-    char Addresses[num_VL6180] = Shifted_TOF_Addresses;
-    TOFsPtr * TOF = new TOFsPtr[num_VL6180];
-    for (int idx = 0; idx < num_VL6180; ++idx) {
-        TOF[idx] = new VL6180(i2c_bus, SHDN_Pins[idx], Addresses[idx]);
-    }
-
-    // Initialise all TOF sensors
-    Init_All_TOFs(TOF);
+    VL6180_Init(address1shift);
+    WriteByte_Uninitialized(0x212, address2, address1shift);
+   
+    SHDN_Pins = 6; // 0x6 or 0b0110
+    VL6180_Init(address1shift);
+    WriteByte_Uninitialized(0x212, address3, address1shift);
+   
+    SHDN_Pins = 14; // 0xE or 0b1110
+    VL6180_Init(address1shift);
+    WriteByte_Uninitialized(0x212, address4, address1shift);
+   
+    SHDN_Pins = 15; // 0xF or 0b1111
+    VL6180_Init(address1shift);
 
-    //Create range variable to stored read TOF values
-    int Range[num_VL6180] = {};
-
-    //note that sensor 1 can be left at the default address, since other sensors
-    //are already allocated. Adding more I2C devices would require it to be
-    //reassigned to a different address, however.
-    while (1) {
-        for(int IDX = 0; IDX < num_VL6180; IDX++) {
-            TOF[IDX]->Start_Range();
-            TOF[IDX]->Poll_Range();
-            Range[IDX] = TOF[IDX]->Read_Range();
-            TOF[IDX]->Clear_Interrupts();
-
-            wait_ms(10);
-        }
+    while (1)
+    {
+        Get_TOF_Reading();    
+        wait_ms(100);      
+    }
+}
+    
+void Get_TOF_Reading()
+{
+    int Range[4];
+    
+    // start range measurement
+    VL6180_Start_Range(address1shift);
+    // poll the VL6180 till new sample ready      
+    VL6180_Poll_Range(address1shift);
+    Range[0] = VL6180_Read_Range(address1shift);
+    VL6180_Clear_Interrupts(address1shift);
+    wait_ms(10);
+    
+    // read range result
+    VL6180_Start_Range(address2shift);
+    VL6180_Poll_Range(address2shift);
+    Range[1] = VL6180_Read_Range(address2shift);
+    VL6180_Clear_Interrupts(address2shift);
+    wait_ms(10);
+    
+    VL6180_Start_Range(address3shift);
+    VL6180_Poll_Range(address3shift);
+    Range[2] = VL6180_Read_Range(address3shift);
+    VL6180_Clear_Interrupts(address3shift);
+    wait_ms(10);
+    
+    VL6180_Start_Range(address4shift);
+    VL6180_Poll_Range(address4shift);
+    Range[3] = VL6180_Read_Range(address4shift);
+    VL6180_Clear_Interrupts(address4shift);        
         
 #ifdef ROS_TOF
     Global_TOF_Range_Mutex.lock();
-    for(int IDX = 0; IDX < num_VL6180; IDX++) {
+    for(int IDX = 0; IDX < 4; IDX++) {
         Global_TOF_Ranges[IDX] = Range[IDX];
     }
     if(TOF_Range_Flag == false)
@@ -62,220 +232,4 @@
     }
     Global_TOF_Range_Mutex.unlock();
 #endif
-
-#ifdef DEBUG_TOF
-        debug_term.printf("|--------------------------------\n\r"
-                          "|Ranges:                         \n\r"
-                          "| %d\n\r",Range[0]);
-                          
-                          /*
-                          "| %d\n\r"
-                          "| %d\n\r"
-                          "| %d\n\r",Range[0], Range[1], Range[2], Range[3]);
-                          */
-#endif
-
-        wait_ms(100);
-
-    }
 }
-
-
-/*------------------------------------------------------------------------------
-    Function turns TOF sensor ON or OFF
-------------------------------------------------------------------------------*/
-bool VL6180::TOF_PWR(bool State)
-{
-    this->shutdown = State;
-    return State;   
-}
-
-/*------------------------------------------------------------------------------
-    Split 16-bit register address into two bytes and write
-    the address + data via I²C
-------------------------------------------------------------------------------*/
-void VL6180::WriteByte(wchar_t reg, char data)
-{
-    char data_write[3];
-    data_write[0] = (reg >> 8) & 0xFF;;
-    // MSB of register address
-    data_write[1] = reg  & 0xFF;
-    // LSB of register address
-    data_write[2] = data & 0xFF;
-    this->i2c.write(addr, data_write, 3);
-}
-void VL6180::WriteByte_Uninitialized(wchar_t reg, char data, char Uninitialized_Address) {    
-    char data_write[3];
-    data_write[0] = (reg >> 8) & 0xFF;;
-    // MSB of register address
-    data_write[1] = reg  & 0xFF;
-    // LSB of register address
-    data_write[2] = data & 0xFF;
-    this->i2c.write(Uninitialized_Address, data_write, 3);
-}
-
-/*------------------------------------------------------------------------------
-    Split 16-bit register address into two bytes and write
-    required register address to VL6180 and read the data back
-------------------------------------------------------------------------------*/
-char VL6180::ReadByte(wchar_t reg)
-{
-    char data_write[2];
-    char data_read[1];
-
-    data_write[0] = (reg >> 8) & 0xFF; // MSB of register address
-    data_write[1] = reg  & 0xFF; // LSB of register address
-
-    this->i2c.write(addr, data_write, 2);
-    this->i2c.read(addr, data_read, 1);
-    return data_read[0];
-}
-char VL6180::ReadByte_Uninitialized(wchar_t reg, char Uninitialized_Address) {
-    char data_write[2];
-    char data_read[1];
-   
-    data_write[0] = (reg >> 8) & 0xFF; // MSB of register address
-    data_write[1] = reg  & 0xFF; // LSB of register address
-   
-    this->i2c.write(Uninitialized_Address, data_write, 2);
-    this->i2c.read(Uninitialized_Address, data_read, 1);
-    return data_read[0];
-}
-
-/*------------------------------------------------------------------------------
-    Start a range measurement in single shot mode
-------------------------------------------------------------------------------*/
-int VL6180::Start_Range(void)
-{
-    this->WriteByte(0x018,0x03);
-    return 0;
-}
-
-/*------------------------------------------------------------------------------
-    poll for new sample ready ready
-------------------------------------------------------------------------------*/
-int VL6180::Poll_Range(void)
-{
-    char status;
-    char range_status;
-
-    // check the status
-    status = this->ReadByte(0x04f);
-    range_status = status & 0x07;
-
-    // wait for new measurement ready status
-    while (range_status !=  0x00) {
-        status = this->ReadByte(0x04f);
-        range_status = status & 0x07;
-        //wait_ms(50); // (can be removed)
-    }
-
-    return 0;
-}
-
-
-/*------------------------------------------------------------------------------
-    Read range result (mm)
-------------------------------------------------------------------------------*/
-int VL6180::Read_Range(void)
-{
-    int range;
-    range = this->ReadByte(0x062);
-    return range;
-}
-
-/*------------------------------------------------------------------------------
-    clear interrupts
-------------------------------------------------------------------------------*/
-
-int VL6180::Clear_Interrupts(void)
-{
-    this->WriteByte(0x015,0x07);
-    return 0;
-}
-
-/*------------------------------------------------------------------------------
-    load settings
-------------------------------------------------------------------------------*/
-bool VL6180::Init(void)
-{
-    // check to see has if the TOF sensor has been Initialised already
-    char reset = this->ReadByte_Uninitialized(0x016, 0x82);    
-    if (reset==1)
-    {            
-        // Mandatory : private registers
-        this->WriteByte_Uninitialized(0x0207, 0x01, 0x82);
-        this->WriteByte_Uninitialized(0x0208, 0x01, 0x82);
-        this->WriteByte_Uninitialized(0x0096, 0x00, 0x82);
-        this->WriteByte_Uninitialized(0x0097, 0xfd, 0x82);
-        this->WriteByte_Uninitialized(0x00e3, 0x01, 0x82);
-        this->WriteByte_Uninitialized(0x00e4, 0x03, 0x82);
-        this->WriteByte_Uninitialized(0x00e5, 0x02, 0x82);
-        this->WriteByte_Uninitialized(0x00e6, 0x01, 0x82);
-        this->WriteByte_Uninitialized(0x00e7, 0x03, 0x82);
-        this->WriteByte_Uninitialized(0x00f5, 0x02, 0x82);
-        this->WriteByte_Uninitialized(0x00d9, 0x05, 0x82);
-        this->WriteByte_Uninitialized(0x00db, 0xce, 0x82);
-        this->WriteByte_Uninitialized(0x00dc, 0x03, 0x82);
-        this->WriteByte_Uninitialized(0x00dd, 0xf8, 0x82);
-        this->WriteByte_Uninitialized(0x009f, 0x00, 0x82);
-        this->WriteByte_Uninitialized(0x00a3, 0x3c, 0x82);
-        this->WriteByte_Uninitialized(0x00b7, 0x00, 0x82);
-        this->WriteByte_Uninitialized(0x00bb, 0x3c, 0x82);
-        this->WriteByte_Uninitialized(0x00b2, 0x09, 0x82);
-        this->WriteByte_Uninitialized(0x00ca, 0x09, 0x82);
-        this->WriteByte_Uninitialized(0x0198, 0x01, 0x82);
-        this->WriteByte_Uninitialized(0x01b0, 0x17, 0x82);
-        this->WriteByte_Uninitialized(0x01ad, 0x00, 0x82);
-        this->WriteByte_Uninitialized(0x00ff, 0x05, 0x82);
-        this->WriteByte_Uninitialized(0x0100, 0x05, 0x82);
-        this->WriteByte_Uninitialized(0x0199, 0x05, 0x82);
-        this->WriteByte_Uninitialized(0x01a6, 0x1b, 0x82);
-        this->WriteByte_Uninitialized(0x01ac, 0x3e, 0x82);
-        this->WriteByte_Uninitialized(0x01a7, 0x1f, 0x82);
-        this->WriteByte_Uninitialized(0x0030, 0x00, 0x82);
-        this->WriteByte_Uninitialized(0x016, 0x00, 0x82); //change fresh out of set status to 0 
-    } else {
-        return true;
-    }
-    return false;
-}
-
-
-/*------------------------------------------------------------------------------
-    Function to Initialise all TOF sensors
-------------------------------------------------------------------------------*/
-bool Init_All_TOFs(TOFsPtr *tof)
-{
-    bool Err_Flag = false;
-    
-    char Non_Shifted_Addresses[num_VL6180] = TOF_Addresses;
-    
-    for(int IDX = 0; IDX < num_VL6180; IDX++)
-    {
-        tof[IDX]->TOF_PWR(false);
-    }
-    
-    wait_ms(0.5);
-
-    if(num_VL6180 > 1)
-    {
-        for(int IDX = 1; IDX < num_VL6180; IDX++)
-        {
-            tof[IDX]->TOF_PWR(true);
-            
-            if(tof[IDX]->Init() == true)
-            {
-                Err_Flag = true;    
-            }
-            tof[IDX]->WriteByte(0x212, Non_Shifted_Addresses[IDX]);
-
-        }
-    }
-    
-    tof[0]->TOF_PWR(true);         
-    tof[0]->Init();
-    tof[0]->WriteByte(0x212, Non_Shifted_Addresses[0]); 
-
-    return Err_Flag;
-}

diff -r 0b9098ec11c7 -r 82b8fe254222 VL6180.hpp
--- a/VL6180.hpp	Thu Dec 19 00:13:38 2019 +0000
+++ b/VL6180.hpp	Sun Jan 05 15:42:22 2020 +0000
@@ -1,6 +1,6 @@
 #include "mbed.h"
-#include "General.hpp"
-#include "rtos.h"
+//#include "General.hpp"
+//#include "rtos.h"
 #include "Pins.h"
 
 #ifndef Define_ONCE_VL6180
@@ -11,39 +11,30 @@
 #define TOF_Addresses {0x29} //, 0x2A, 0x2B, 0x2C}
 #define Shifted_TOF_Addresses {0x82} //, 0x84, 0x86, 0x88}
 
-// Thread that runs TOF sensors
-int TOF_Thread();
+
+//Addresses
+#define address1   (0x29)
+#define address2   (0x2A)
+#define address3   (0x2B)
+#define address4   (0x2C)
 
-// Class Terminal expects tx and rx pins and is used for controlling a serialy conected terminal
-class VL6180
-{
-public:
-    VL6180(I2C& comm_bus, PinName SHDWN, char Address) : shutdown(SHDWN), i2c(comm_bus){addr = Address;};
+//Shifted addresses, so the R/W command can be added
+#define address1shift   (address1<<1)
+#define address2shift   (address2<<1)
+#define address3shift   (address3<<1)
+#define address4shift   (address4<<1)
+
 
-    bool Init(void);
-    bool TOF_PWR(bool State);
-    void WriteByte(wchar_t reg, char data);
-    void WriteByte_Uninitialized(wchar_t reg, char data, char Uninitialized_Address);
-    int Start_Range(void);
-    int Poll_Range(void);
-    int Read_Range(void);
-    int Clear_Interrupts(void);
-    
-private:
-    // I2C Bus TOF is connected to
-    I2C& i2c;
-    // Private functions
-    char ReadByte(wchar_t reg);
-    char ReadByte_Uninitialized(wchar_t reg, char Uninitialized_Address);
-    void WriteByte_Uninitialized(wchar_t reg, char data);
-    // Private Variables
-    char addr;
-    // IOs
-    DigitalOut shutdown;
-};
+// Thread that runs TOF sensors
+void TOF_Handler();
 
-// TypeDef for pointer to the TOF class (needed for dynamic creation of objects)
-typedef VL6180* TOFsPtr;
-// Function to Initialise all TOF sensors
-bool Init_All_TOFs(TOFsPtr *tof);
+int VL6180_Init(char addr);
+void WriteByte_Uninitialized(wchar_t reg, char data, char addr);
+char ReadByte(wchar_t reg, char addr);
+int VL6180_Start_Range(char addr);
+int VL6180_Poll_Range(char addr);
+int VL6180_Read_Range(char addr);
+int VL6180_Clear_Interrupts(char addr);
+void Get_TOF_Reading(void);
+
 #endif

diff -r 0b9098ec11c7 -r 82b8fe254222 main.cpp
--- a/main.cpp	Thu Dec 19 00:13:38 2019 +0000
+++ b/main.cpp	Sun Jan 05 15:42:22 2020 +0000
@@ -17,14 +17,15 @@
 #include <motordriver.h>
 #include "math.h"
 #include "Motors.hpp"
-//#include "VL6180.hpp"
+#include "VL6180.hpp"
 
 DigitalOut debug_LED(LED1);
 
 // Threads
 Thread ROS_Thread(osPriorityRealtime);   // Create THREAD with highest priority for ROS
+Thread TOF_Thread(osPriorityNormal);
 //Thread Debug_Thread(osPriorityNormal);
-//Thread Movement_Thread(osPriorityNormal);
+//Thread Motor_Thread(osPriorityNormal);
 
 // Thread ID for the Main function (CMSIS API)
 osThreadId tidMain;
@@ -32,20 +33,20 @@
 // main starts all threads
 int main(void)
 {
-    //TOF_Thread();
     // Main thread ID
     tidMain = Thread::gettid();
 
     // Start each thread
     ROS_Thread.start(ROS_Handler);
+    TOF_Thread.start(TOF_Handler);
+    //Motor_Thread.start(Motor_Handler);
     //Debug_Thread.start(TerminalThread);
-    //Thread2.start(main3);
     
     // Creates an object to monitor and handle changes in battery level using an RGB LED as an OutPut
-    Battery_Monitor VBatt_Monitor(ADC_VBAT, 0.5f);
+    Battery_Monitor VBatt_Monitor(ADC_VBAT, 1.0f);
 
     /**************************************************************************/
-    initialize(); //initialize(); only needs to be called once, at the start of the program.
+    //initialize(); //initialize(); only needs to be called once, at the start of the program.
     //the encoder interrupts should be able to handle themselves from there, and do not require
     //resetting.
     /**************************************************************************/
@@ -54,21 +55,8 @@
     {
         //Flag_Error(warning, "Flashing LED\n\r");
         debug_LED = 1;
-        wait(1);
+        wait_ms(500);
         debug_LED = 0;
-        wait(1);
-
-        
-        /**********************************************************************/
-        //careful, putting this in a while loop will mean the bot moves in an infinite square!
-        /**********************************************************************/
-        //tempMove(40);
-        //tempRotate(-90);
-        //tempMove(40);
-        //tempRotate(-90);
-        //tempMove(40);
-        //tempRotate(-90);
-        //tempMove(40);
-        //tempRotate(-90);
+        wait_ms(500);
     }
 }