/* This example shows how to use continuous mode to take
range measurements with the 6 pin VL53L0X module. It is based on
vl53l0x_ContinuousRanging_Example.c from the VL53L0X API.

The range readings are in units of mm. 

Original source: https://github.com/adafruit/Adafruit_VL53L0X
Modified by Ahmad Shamshiri for RoboJax.com
Date modified: May 31, 2018 at 19:25 at Ajax, Ontario, Canada
Watch the instruciton video for this code https://youtu.be/S2jaAQEv3Yo

Pin connection
VL53L0X Pin  Arduino Pin
VCC         5V
GND         GND
SDA         A4 or SDA if available
SCL         A5 or SCL if available
GPIO1       leave it unconnected
XSHUT       D12 (digital 12 or pin 12)
*/

#define USESERIAL false

#include <Wire.h>
#include <VL53L0X.h>
#include <SoftwareSerial.h>

VL53L0X sensor;
int LED = 1;
SoftwareSerial mySerial(3,4);  //rx, tx

void blink_led(int time_delay){
  digitalWrite(LED, HIGH);
  delay(time_delay);
  digitalWrite(LED, LOW);
  delay(time_delay);
}

void setup()
{
  pinMode(LED, OUTPUT);
  //pinMode(12,INPUT_PULLUP);
  //digitalWrite(12,HIGH);
  if (USESERIAL){
    Serial.begin(9600);
  } else{
    mySerial.begin(9600);
  }
  
  blink_led(1000);
  blink_led(1000);
  
  Wire.begin();

  blink_led(1000);
  blink_led(1000);
  
  sensor.init();
  
  blink_led(1000);
  blink_led(1000);
  
  sensor.setTimeout(500);

  blink_led(1000);
  blink_led(1000);
  
  // Start continuous back-to-back mode (take readings as
  // fast as possible).  To use continuous timed mode
  // instead, provide a desired inter-measurement period in
  // ms (e.g. sensor.startContinuous(100)).
  sensor.startContinuous();
  
  blink_led(1000);
  blink_led(1000);
  if (USESERIAL){
    Serial.println("Start measurement...");
  } else{
    mySerial.println("Start measurement...");
  }
}

void loop()
{
  int distance = sensor.readRangeContinuousMillimeters();
  
  if (USESERIAL) {
    Serial.print("Distance: ");
    Serial.print(distance);
    Serial.print("mm");
  } else{
    mySerial.print("Distance: ");
    mySerial.print(distance);
    mySerial.print("mm");
  }
  if (sensor.timeoutOccurred()) {
    if (USESERIAL){
      Serial.print(" TIMEOUT");
    }else{
      mySerial.print(" TIMEOUT");
    }
    blink_led(20);
    blink_led(20);
  }
  
  if(distance < 100){
    digitalWrite(LED, HIGH);
  }
  else{
    digitalWrite(LED, LOW);
  }

  if (USESERIAL){
    Serial.println();
  }else{
    mySerial.println();
  }
  delay(100);
}