#include <Servo.h>
#include "Adafruit_VL53L0X.h"
#include <Arduino.h>

Adafruit_VL53L0X lox = Adafruit_VL53L0X();
Servo servoMotor1;// Declaro la variable para controlar el servo1
Servo servoMotor2;// Declaro la variable para controlar el servo2
Servo servoMotor3;// Declaro la variable para controlar el servo3
String textresult;
int laser1 = 5;
int laser2 = 6;

//---------------- CONFIGURACION---------------
void setup() {
  Serial.begin(9600);
pinMode(12,OUTPUT);
digitalWrite(12,LOW);
pinMode(laser1,INPUT);
pinMode(laser2,INPUT);
servoMotor1.attach(2);
servoMotor2.attach(3);
servoMotor3.attach(4);
servoMotor1.write(0);
servoMotor2.write(0);
servoMotor3.write(0);
  
  if (!lox.begin()) {  // Iniciar sensor
    Serial.println(F("Error al iniciar VL53L0X"));
    while(1);
  }
  delay (500);
}


//************************* INICIO LOOP*************************************
void loop() {


//---------------- LECTURA RX SERIAL----------------
  if (Serial.available()>0){
  Serial.setTimeout (10);
char act1= Serial.read();

switch (act1){

  case '1':
  servoMotor1.write(30); // SERVO 1
  digitalWrite(12,HIGH);
  break;

  case '0':
  digitalWrite(12,LOW);
  servoMotor1.write(0);
  break;

  case '3':
  servoMotor2.write(30); // SERVO 2
  digitalWrite(12,HIGH);
  break;

  case '2':
  digitalWrite(12,LOW);
  servoMotor2.write(0);
  break;
    case '5':
  servoMotor3.write(25); // SERVO 3
  digitalWrite(12,HIGH);
  break;

  case '4':
  digitalWrite(12,LOW);
  servoMotor3.write(0);
  break;
  
}
}

//------------- LECTURA DISTANCIA------------------
  VL53L0X_RangingMeasurementData_t measure;
  lox.rangingTest(&measure, false); // si se pasa true como parametro, muestra por puerto serie datos de debug

  if (measure.RangeStatus != 4){
int medida = measure.RangeMilliMeter;
int result= 95-medida;
textresult=String(result);
    if (result<=8){
      textresult='0';
    }
  }
  else{
    textresult='0';
  }

//---------------- SEND RX BY SERIAL-------------------

  Serial.println(textresult+':'+digitalRead(laser1)+':'+digitalRead(laser2));


  delay(40);
}
