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

//#define Sensor1_newAddress 41 not required address change
#define Sensor2_newAddress 42
#define Sensor3_newAddress 43

#define XSHUT_pin3 4
#define XSHUT_pin2 3
#define XSHUT_pin1 2

#define M1_1 6
#define M1_2 5
#define M2_1 9
#define M2_2 10

VL53L0X Sensor1;
VL53L0X Sensor2;
VL53L0X Sensor3;

int napr = 0, old_napr = 0, right_S, central_S, left_S;

void setup()
{
  Serial.begin(9600);
  pinMode(XSHUT_pin1, OUTPUT);
  pinMode(XSHUT_pin2, OUTPUT);
  pinMode(XSHUT_pin3, OUTPUT);

  pinMode(A0, INPUT);
  pinMode(A1, INPUT);
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  pinMode(10, OUTPUT);
  pinMode(9, OUTPUT);

  Wire.begin();
  pinMode(XSHUT_pin3, INPUT);
  delay(10);
  Sensor3.setAddress(Sensor3_newAddress);
  pinMode(XSHUT_pin2, INPUT);
  delay(10);
  Sensor2.setAddress(Sensor2_newAddress);
  pinMode(XSHUT_pin1, INPUT);
  delay(10);

  Sensor1.init();
  Sensor2.init();
  Sensor3.init();
  Sensor1.setTimeout(500);
  Sensor2.setTimeout(500);
  Sensor3.setTimeout(500);
  Sensor1.startContinuous();
  Sensor2.startContinuous();
  Sensor3.startContinuous();
}



void loop()
{
  if (Sensor1.readRangeContinuousMillimeters() < 600)  right_S = Sensor1.readRangeContinuousMillimeters();
  else right_S = 600;
  if (Sensor2.readRangeContinuousMillimeters() < 600)  central_S = Sensor2.readRangeContinuousMillimeters();
  else central_S = 600;
  if (Sensor3.readRangeContinuousMillimeters() < 600)  left_S = Sensor3.readRangeContinuousMillimeters();
  else left_S = 600;

  Poisk();
  Line();
  /*Serial.print(napr);
  Serial.print(" ");
  Serial.print(right_S);
  Serial.print(" ");
  Serial.print(central_S);
  Serial.print(" ");
  Serial.println(left_S);*/
  if (napr == 12)
    motor(255, 255);
  else if (napr == 3)
    motor(-255, 255);
  else if (napr == 9)
    motor(255, -255);
  
  delay(25);
}
