diff --git a/Core/Inc/monitor.h b/Core/Inc/monitor.h
index 263ff2e..bff50aa 100644
--- a/Core/Inc/monitor.h
+++ b/Core/Inc/monitor.h
@@ -14,6 +14,10 @@ void vIMUMonitor(void *pv_params);
 extern osThreadId_t imu_monitor_handle;
 extern const osThreadAttr_t imu_monitor_attributes;
 
+void vTOFMonitor(void *pv_params);
+extern osThreadId_t tof_monitor_handle;
+extern const osThreadAttr_t tof_monitor_attributes;
+
 void vShockpotMonitor(void *pv_params);
 extern osThreadId_t shockpot_monitor_handle;
 extern const osThreadAttr_t shockpot_monitor_attributes;
diff --git a/Core/Inc/msb.h b/Core/Inc/msb.h
index ce72873..76ce537 100644
--- a/Core/Inc/msb.h
+++ b/Core/Inc/msb.h
@@ -6,6 +6,7 @@
 #include "sht30.h"
 #include "stm32f405xx.h"
 #include "cmsis_os.h"
+#include "vl6180x_api.h"
 
 typedef enum
 {
@@ -20,6 +21,7 @@ typedef struct
     I2C_HandleTypeDef *hi2c;
     sht30_t *temp_sensor;
     lsm6dso_t *imu;
+    VL6180xDev_t tof;
     ADC_HandleTypeDef *adc1;
     uint32_t adc1_buf[3];
     GPIO_TypeDef *debug_led1_gpio;
@@ -40,6 +42,8 @@ int8_t read_accel(msb_t *msb, uint16_t accel[3]);
 
 int8_t read_gyro(msb_t *msb, uint16_t gyro[3]);
 
+int8_t read_distance(msb_t *msb, int32_t *range_mm);
+
 int8_t write_debug1(msb_t *msb, bool status);
 
 int8_t write_debug2(msb_t *msb, bool status);
diff --git a/Core/Src/main.c b/Core/Src/main.c
index c05bda7..0f8a8b5 100644
--- a/Core/Src/main.c
+++ b/Core/Src/main.c
@@ -195,6 +195,8 @@ int main(void)
   assert(temp_monitor_handle);
   imu_monitor_handle = osThreadNew(vIMUMonitor, msb, &imu_monitor_attributes);
   assert(imu_monitor_handle);
+  tof_monitor_handle = osThreadNew(vIMUMonitor, msb, &tof_monitor_attributes);
+  assert(tof_monitor_handle);
   shockpot_monitor_handle = osThreadNew(vIMUMonitor, msb, &shockpot_monitor_attributes);
   assert(shockpot_monitor_handle);
   strain_monitor_handle = osThreadNew(vIMUMonitor, msb, &strain_monitor_attributes);
diff --git a/Core/Src/monitor.c b/Core/Src/monitor.c
index a001f04..9cff2f5 100644
--- a/Core/Src/monitor.c
+++ b/Core/Src/monitor.c
@@ -169,6 +169,46 @@ void vIMUMonitor(void *pv_params)
     }
 }
 
+osThreadId_t tof_monitor_handle;
+const osThreadAttr_t tof_monitor_attributes = {
+    .name = "TOFMonitor",
+    .stack_size = 32 * 8,
+    .priority = (osPriority_t)osPriorityHigh,
+};
+
+void vTOFMonitor(void *pv_params)
+{
+    msb_t *msb = (msb_t *)pv_params;
+
+    can_msg_t range_msg = {.id = convert_can(CANID_TOF, msb->device_loc), .len = 4, .data = {0}};
+
+    int32_t range;
+
+    for (;;)
+    {
+        if (read_distance(msb, &range))
+        {
+            serial_print("failed to read distance!");
+            continue;
+        }
+
+#ifdef LOG_VERBOSE
+        serial_print("Range is: %d", range);
+#endif
+
+        endian_swap(&range, sizeof(range));
+
+        memcpy(range_msg.data, &range, range_msg.len);
+        /* Send CAN message */
+        if (queue_can_msg(range_msg))
+        {
+            serial_print("Failed to send CAN message");
+        }
+
+        osDelay(DELAY_TOF_REFRESH);
+    }
+}
+
 osThreadId_t shockpot_monitor_handle;
 const osThreadAttr_t shockpot_monitor_attributes = {
     .name = "ShockpotMonitor",
diff --git a/Core/Src/msb.c b/Core/Src/msb.c
index 75d8581..c09efe7 100644
--- a/Core/Src/msb.c
+++ b/Core/Src/msb.c
@@ -1,5 +1,6 @@
 #include "msb.h"
 #include <assert.h>
+#include <serial_monitor.h>
 #include <string.h>
 #include <stdlib.h>
 #include "lsm6dso.h"
@@ -38,6 +39,13 @@ msb_t *init_msb(I2C_HandleTypeDef *hi2c, ADC_HandleTypeDef *adc1, GPIO_TypeDef *
 	assert(msb->imu);
 	assert(!lsm6dso_init(msb->imu, msb->hi2c)); /* This is always connected */
 
+	/* Initialize the ToF sensor */
+	msb->tof = malloc(sizeof(VL6180xDev_t));
+	assert(msb->tof);
+	assert(!VL6180x_WaitDeviceBooted(msb->tof));
+	assert(!VL6180x_InitData(msb->tof));
+	assert(!VL6180x_Prepare(msb->tof));
+
 	assert(!HAL_ADC_Start_DMA(msb->adc1, msb->adc1_buf, sizeof(msb->adc1_buf) / sizeof(uint32_t)));
 
 	/* Create Mutexes */
@@ -129,6 +137,28 @@ int8_t read_gyro(msb_t *msb, uint16_t gyro[3])
 	return 0;
 }
 
+VL6180x_RangeData_t *range;
+int8_t read_distance(msb_t *msb, int32_t *range_mm) {
+	if (!msb)
+		return -1;
+
+	osStatus_t mut_stat = osMutexAcquire(msb->i2c_mutex, osWaitForever);
+	if (mut_stat)
+		return mut_stat;
+
+	VL6180x_RangePollMeasurement(msb->tof, range);
+	if (range->errorStatus) {
+		serial_print("Error in range %f", VL6180x_RangeGetStatusErrString(range->errorStatus));
+		return range->errorStatus;
+	}
+
+	memcpy(range_mm, &range->range_mm, sizeof(range->range_mm));
+	
+
+	osMutexRelease(msb->i2c_mutex);
+	return 0;
+}
+
 int8_t write_debug1(msb_t *msb, bool status)
 {
 	if (!msb)
diff --git a/Makefile b/Makefile
index eda2195..13f3aa2 100644
--- a/Makefile
+++ b/Makefile
@@ -74,6 +74,8 @@ Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_spi.c \
 Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_uart.c \
 Drivers/Embedded-Base/platforms/stm32f405/src/can.c \
 Drivers/Embedded-Base/general/src/lsm6dso.c \
+Drivers/Embedded-Base/general/src/vl6180x_api.c \
+Drivers/Embedded-Base/general/src/vl6180x_i2c.c \
 Drivers/Embedded-Base/middleware/src/c_utils.c \
 Drivers/Embedded-Base/middleware/general/src/sht30.c \
 Middlewares/Third_Party/FreeRTOS/Source/croutine.c \