Height Measure using VL53L0X

#include "I2Cdev.h"
#include "MPU6050.h"
#include <Adafruit_NeoPixel.h>
#include <VL53L0X.h>
#include <LiquidCrystal.h>

// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif

int mode = 0;

MPU6050 accelgyro; // class default I2C address is 0x68
int16_t ax, ay, az;
int diffAccel = 100;

#define PIN            8
Adafruit_NeoPixel pixels = Adafruit_NeoPixel(1, PIN, NEO_GRB + NEO_KHZ800);

//LiquidCrystal lcd(RS[4], E[6], DB4[11], DB5[12], DB6[13], DB7[14]);
LiquidCrystal lcd(12, 11, 5, 4, 3, 2);
char s[16];

VL53L0X sensor;
float height;

// Uncomment this line to use long range mode. This
// increases the sensitivity of the sensor and extends its
// potential range, but increases the likelihood of getting
// an inaccurate reading because of reflections from objects
// other than the intended target. It works best in dark
// conditions.
#define LONG_RANGE


// Uncomment ONE of these two lines to get
// - higher speed at the cost of lower accuracy OR
// - higher accuracy at the cost of lower speed
//#define HIGH_SPEED
#define HIGH_ACCURACY

void setup() {
  // join I2C bus (I2Cdev library doesn't do this automatically)
  #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    Wire.begin();
  #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
    Fastwire::setup(400, true);
  #endif

  Serial.begin(38400);

  pinMode(9, INPUT_PULLUP);

  pixels.begin();
  pixels.setBrightness(50);
  pixels.setPixelColor(0, pixels.Color(0,0,0));
  pixels.show();

  lcd.begin(16, 2);
  lcd.setCursor(0, 0);
  lcd.print("x");
  lcd.setCursor(9, 0);
  lcd.print("y");
  lcd.setCursor(0, 1);
  lcd.print("Height:");
  
  sensor.init();
  sensor.setTimeout(500);

  #if defined LONG_RANGE
  // lower the return signal rate limit (default is 0.25 MCPS)
    sensor.setSignalRateLimit(0.1);
    // increase laser pulse periods (defaults are 14 and 10 PCLKs)
    sensor.setVcselPulsePeriod(VL53L0X::VcselPeriodPreRange, 18);
    sensor.setVcselPulsePeriod(VL53L0X::VcselPeriodFinalRange, 14);
  #endif
    
  #if defined HIGH_SPEED
  // reduce timing budget to 20 ms (default is about 33 ms)
    sensor.setMeasurementTimingBudget(20000);
  #elif defined HIGH_ACCURACY
    // increase timing budget to 200 ms
    sensor.setMeasurementTimingBudget(200000);
  #endif

  // initialize device
  Serial.println("Initializing I2C devices...");
  accelgyro.initialize();

  // verify connection
  Serial.println("Testing device connections...");
  Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");

  if(accelgyro.testConnection()){
    pixels.setPixelColor(0, pixels.Color(255,0,0));
    pixels.show();
  }

  // use the code below to change accel/gyro offset values
  accelgyro.setXAccelOffset(40); //0
  accelgyro.setYAccelOffset(-985);  //0
  accelgyro.setZAccelOffset(968); //16384    
}

void loop() {  
  //Measurement Mode
  while(digitalRead(9) == 0 && mode == 0){
    // read raw accel
    accelgyro.getAcceleration(&ax, &ay, &az);
    lcd.setCursor(1, 0);
    lcd.print(dtostrf(ax,6,0,s));
    lcd.setCursor(10, 0);
    lcd.print(dtostrf(ay,6,0,s));

    if(abs(ax) < diffAccel && abs(ay) < diffAccel){
      pixels.setPixelColor(0, pixels.Color(0,255,0));
      pixels.show();
      if (sensor.timeoutOccurred() == 0) {
        //Distance measurement
        height = sensor.readRangeSingleMillimeters()/10.0 + 0.1; 
        lcd.setCursor(8, 1);
        if(height <= 220.0){
          lcd.print(dtostrf(height,5,1,s));
          pixels.setPixelColor(0, pixels.Color(0,0,255));
          pixels.show();
          mode = 1;
        }
      }
    }else{
      pixels.setPixelColor(0, pixels.Color(255,0,0));
      pixels.show();
    }
  } 

  //Mode Reset
  if(digitalRead(9) == 1 && mode == 1){
    mode = 2;
    delay(100);
  }
  if(digitalRead(9) == 0 && mode == 2){
    mode = 0;
    delay(100);
  }

  //Normal Mode
  if(digitalRead(9) == 1 && mode == 0){
    // read raw accel
    accelgyro.getAcceleration(&ax, &ay, &az);
    lcd.setCursor(1, 0);
    lcd.print(dtostrf(ax,6,0,s));
    lcd.setCursor(10, 0);
    lcd.print(dtostrf(ay,6,0,s));
    
    if(abs(ax) < diffAccel && abs(ay) < diffAccel){
      pixels.setPixelColor(0, pixels.Color(0,255,0));
      pixels.show();
    }else{
      pixels.setPixelColor(0, pixels.Color(255,0,0));
      pixels.show();
    }
  
    if (sensor.timeoutOccurred() == 0) {
      //Distance measurement
      height = sensor.readRangeSingleMillimeters()/10.0 + 0.1;
      Serial.print(height); Serial.print("\t"); 
      lcd.setCursor(8, 1);
      if(height <= 220.0){
        lcd.print(dtostrf(height,5,1,s));
      }
    }
  }
}