#include <Wire.h>
#include <VL53L0X.h>
#include <SoftwareSerial.h>
VL53L0X sensor;
VL53L0X sensor2;
VL53L0X sensor3;
SoftwareSerial mySerial(A4, A5); // RX, TX
String CommandFromPC;
void setup()
{

  pinMode(A6, OUTPUT);
  pinMode(A7, OUTPUT);
  
  pinMode(A1, OUTPUT);
  pinMode(A2, OUTPUT);
  pinMode(A3, OUTPUT);
  
  digitalWrite(A6, LOW);
    digitalWrite(A7, LOW);
  
  digitalWrite(A1, LOW);
  digitalWrite(A2, LOW);
  digitalWrite(A3, LOW);

  delay(500);
  Wire.begin();




  //SENSOR
  pinMode(A3, INPUT);
  delay(150);
  sensor.init(true);
  delay(100);
  sensor.setAddress((uint8_t)22);

  //SENSOR 2
  pinMode(A2, INPUT);
  delay(150);
  sensor2.init(true);
  delay(100);
  sensor2.setAddress((uint8_t)25);

  //SENSOR 3
  pinMode(A1, INPUT);
  delay(150);
  sensor3.init(true);
  delay(100);
  sensor3.setAddress((uint8_t)28);





  sensor.setTimeout(500);
  sensor2.setTimeout(500);
  sensor3.setTimeout(500);

 mySerial.begin(38400);
//Serial.begin(9600);
  while (!Serial);    // wait for the serial port to open

}

void loop()
{
  //CHECK DISTANCES
  long L = (sensor.readRangeSingleMillimeters());
  long M = (sensor2.readRangeSingleMillimeters());
  
  long R = (sensor3.readRangeSingleMillimeters());
if ( L < 200 ){ digitalWrite(A6,HIGH); }
else {digitalWrite(A6,LOW);                                                                                                                                  }
  //FWD OR SENSOR
//mySerial.print(L);
//mySerial.print("  ");
//mySerial.print(M);
//mySerial.print("  ");
//mySerial.print(R);
//mySerial.print("  ");
//mySerial.println("");
 
   if (mySerial.available() > 0)
 {


   CommandFromPC = mySerial.readStringUntil('\n');
         if (CommandFromPC == "x") {
  
     mySerial.println(L);
 
     }
         if (CommandFromPC == "y") {

  
    mySerial.println(M);
 
     }
         if (CommandFromPC == "z") {
  
     mySerial.println(R); 
 
     }
 }
  delay(100);//can change to a lower time like 100
}