TOF050C-VL6180X Distance Sensor Time-Of-Flight high precision

#include <Wire.h>

const uint8_t SENSOR_ADDR = 0x29;
const uint8_t REG_SYSRANGE_START = 0x18;
const uint8_t REG_RESULT_RANGE_VAL = 0x62;
const uint8_t REG_SYSTEM_INTERRUPT_CLEAR = 0x15;
const uint8_t REG_SYSTEM_INTERRUPT_CONFIG_GPIO = 0x14;
const uint8_t REG_SYSTEM_MODE_GPIO1 = 0x11;

const int INT_PIN = 2;
const int SHUT_PIN = 3;

void writeReg8(uint8_t reg, uint8_t value) {
  Wire.beginTransmission(SENSOR_ADDR);
  Wire.write(reg);
  Wire.write(value);
  Wire.endTransmission();
}

uint8_t readReg8(uint8_t reg) {
  Wire.beginTransmission(SENSOR_ADDR);
  Wire.write(reg);
  Wire.endTransmission(false);
  Wire.requestFrom(SENSOR_ADDR, (uint8_t)1);
  if (Wire.available()) return Wire.read();
  return 0;
}

void initSensor() {
  // Basic startup sequence for VL6180X-compatible modules.
  writeReg8(0x0207, 0x01);
  writeReg8(0x0208, 0x01);
  writeReg8(0x0096, 0x00);
  writeReg8(0x0097, 0xfd);
  writeReg8(0x00e3, 0x00);
  writeReg8(0x00e4, 0x04);
  writeReg8(0x00e5, 0x02);
  writeReg8(0x00e6, 0x01);
  writeReg8(0x00e7, 0x03);
  writeReg8(0x00f5, 0x02);
  writeReg8(0x00d9, 0x05);
  writeReg8(0x00db, 0xce);
  writeReg8(0x00dc, 0x03);
  writeReg8(0x00dd, 0xf8);
  writeReg8(0x009f, 0x00);
  writeReg8(0x00a3, 0x3c);
  writeReg8(0x00b7, 0x00);
  writeReg8(0x00bb, 0x3c);
  writeReg8(0x00b2, 0x09);
  writeReg8(0x00ca, 0x09);
  writeReg8(0x0198, 0x01);
  writeReg8(0x01b0, 0x17);
  writeReg8(0x01ad, 0x00);
  writeReg8(0x00ff, 0x05);
  writeReg8(0x0100, 0x05);
  writeReg8(0x0199, 0x05);
  writeReg8(0x01a6, 0x1b);
  writeReg8(0x01ac, 0x3e);
  writeReg8(0x01a7, 0x1f);
  writeReg8(0x0030, 0x00);

  writeReg8(REG_SYSTEM_INTERRUPT_CONFIG_GPIO, 0x04); // New sample ready interrupt
  writeReg8(REG_SYSTEM_MODE_GPIO1, 0x10);            // Active low interrupt pin
  writeReg8(0x0016, 0x01);                           // Range averaging / timing control
  writeReg8(0x0018, 0x01);                           // Start continuous ranging
}

uint8_t readDistanceMm() {
  writeReg8(REG_SYSRANGE_START, 0x01);
  delay(20);

  while (digitalRead(INT_PIN) == HIGH) {
    delay(1);
  }

  uint8_t range = readReg8(REG_RESULT_RANGE_VAL);
  writeReg8(REG_SYSTEM_INTERRUPT_CLEAR, 0x07); // Clear interrupt
  return range;
}

void setup() {
  pinMode(INT_PIN, INPUT_PULLUP);
  pinMode(SHUT_PIN, OUTPUT);

  Serial.begin(115200);
  while (!Serial) {
    ;
  }

  digitalWrite(SHUT_PIN, LOW);
  delay(10);
  digitalWrite(SHUT_PIN, HIGH);
  delay(20);

  Wire.begin();
  Serial.println("VL6180X time-of-flight sensor demo");

  initSensor();
  Serial.println("Sensor initialized. Reading distance in mm...");
}

void loop() {
  uint8_t distance = readDistanceMm();

  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.println(" mm");

  delay(200);
}