#include <Motorstep.h>
#include <Wire.h> // Per gestire il protocollo I2C
#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0x27,16,2);
// Imposta l'indirizzo del display a 0x27 per 16 caratteri per 2 linee
#include <VL53L0X.h>
#define LONG_RANGE
#define HIGH_ACCURACY
VL53L0X sensor;
// int pwrPin = 13; //pin used to power the ToF (VIN)
Motorstep Motorstep(8,9,10,11,25); // Pin utilizzati per motore pp. Ultima cifra è la pausa in ms
int x, y;
bool pulsante;
int Misura_A=0;
int Sw = 0; // Switch logico
int Val_1; int Val_2;
int Step;
void setup ()
{
 lcd.init(); // inizializza il display
 lcd.backlight(); //accende la retroilluminazione
 // Scrittura dei testi sul display
 lcd.setCursor(0, 0);
 lcd.print("CORSO ARDUINO ED");

 lcd.setCursor (0, 1);
 lcd.print ("ELEM.DI ROBOTICA");
 delay (3000);

// pinMode (pwrPin, OUTPUT);
 //digitalWrite (pwrPin, HIGH); //Power On the ToF
 Serial.begin(9600);
 Wire.begin (); // Arduino assume il ruolo di Master
 sensor.init ();
 sensor.setTimeout (500);
 #if defined LONG_RANGE
 sensor.setSignalRateLimit(0.2);
 sensor.setVcselPulsePeriod(VL53L0X ::VcselPeriodPreRange, 18);
 sensor.setVcselPulsePeriod(VL53L0X ::VcselPeriodFinalRange, 14);
 #endif
 #if defined HIGH_ACCURACY
 sensor.setMeasurementTimingBudget(200000);
 #endif
sensor.startContinuous();
pinMode (2, INPUT_PULLUP); // Per Joystick
 }
void loop ()
{
x = analogRead (0);
y = analogRead (1);
pulsante = digitalRead (2);
if (x < 300)
stepSX();
if (x > 700)
stepDX();
if (pulsante == LOW)
{
Serial.print ("Valore misurato = ");
Serial.println( Misura_A);
++Sw; // Ciclo di acquisizione di due distanze AB AC, recupero angolo motore, calcolo segmento BC
//con metodo trigonometrico
Serial.print("Sw = "); Serial.println(Sw);
 if(Sw == 1)
 {
 Val_1 = Misura_A; // Acquisisce la prima misura
 Step = 0; // Inizio a misurare l'angolo verso la seconda misura quindi devo azzerare Step
 Serial.println(Step);
 }

 if(Sw == 2)
 {
 Val_2 = Misura_A; // Acquisisce la seconda misura
 float Pigreco_rad = Step/81.21;
 Serial.print("Angolo rad. = "); Serial.println(Pigreco_rad,3);

 Serial.print("Val_1 = "); Serial.print(Val_1); Serial.print(" Val_2 = "); Serial.println(Val_2);
 float Dist = pow(Val_1,2) + pow(Val_2,2); Serial.print("Dist = ");Serial.println(Dist);
 double Minus = (2*(Val_1*Val_2)*cos(Pigreco_rad)); Serial.print("Minus = ");
Serial.println(Minus);
 float Distanza = Dist - Minus;
 Serial.print("Distanza B-C = "); Serial.println(sqrt ( Distanza));
 }
 if(Sw == 3)
 Sw = Val_1 = Val_2 = Step = 0; // Al terzo click le variabili si azzerano
 delay (2000);
}
delay (100);
}

 void stepSX()
 {
 Motorstep.orario(); Serial.print("Step = "); Serial.println(Step);++Step;
 Misura_A = sensor.readRangeContinuousMillimeters();
 Serial.println( Misura_A);
 lcd.clear(); lcd.print(Misura_A);
}

 void stepDX()
 {
 Motorstep.antiorario(); Serial.print("Step = "); Serial.println(Step); --Step;
 Misura_A = sensor.readRangeContinuousMillimeters();
 Serial.println( Misura_A);
 lcd.clear(); lcd.print(Misura_A);
}


 