#include <Servo.h>
#include <LiquidCrystal_I2C.h>
#include <Stepper.h> //Incluim les tres llibreries necesaries

const int trigPin = 7;
const int echoPin = 6;
long duration;
int distance; //Identifiquem els pins del ECHO i TRIG, tambe posem nom a les variables de la duracio i la distancia

bool gir_dret = true; //Creem la variable gir_dret com un bolea

LiquidCrystal_I2C lcd(0x27,16,2); //Identifiquem la pantalla LCD I2C amb el protocol de comunicacio 0x27 i les columnes

Servo servoMotor;
int angle = 0; //Identifiquem el motor servo, i asignem una variables per l'angle
Servo servoMotor2;
int angle2 = 90;

const int stepsPerRevolution = 100;
Stepper myStepper(stepsPerRevolution, 8, 9, 10, 11); //Identifiquem el motor stepper amb els seus pins, i creem la variable per les  revolucions

void setup() {
pinMode (trigPin, OUTPUT);
pinMode (echoPin, INPUT); //Posem el trigPin com la sortida de la senyal i el echoPin com l'entrada

servoMotor.attach(3); //Posem el pin en el que esta posat el servo
servoMotor2.attach(4);

lcd.init(); //Iniciem la pantalla
lcd.backlight(); //Iniciem la llum de fons de la pantalla
lcd.clear(); //Borrem tot el que hi ha a la pantalla
lcd.setCursor(0,0); //Posem desde on es comensa a escriure a la pantalla (Primera fila)
lcd.print("Distancia: "); //Escribim a la pantalla a la primera fila

myStepper.setSpeed(100); //Posem a quina velocitat anira el motor stepper



Serial.begin(9600); //Encenem la consola
}

void loop() {


  
 
  servoMotor.write(angle); // Posem el servomotor a l'angle establert (angle = 0)


  digitalWrite(trigPin, LOW); //Apagem el trigPin i esperem 2 milisegons
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH); //Activem el trigPin i esperem 10 milisegons
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW); //Desactivem el triPin

  duration = pulseIn(echoPin, HIGH); // Donem valor a la variable duration activant en echoPin

  distance = duration*0.0342/2; // Fem els calculs pertinents per aconseguir la distancia (en cm)

  Serial.print("La distancia es de: ");
  Serial.println(distance); //Escribim a la consola la distancia
  
  lcd.setCursor(0,1); // Posem desde on es comensa a escriure a la pantalla (Segona fila)
  lcd.print(distance); // Escribim a la pantalla la distancia

  if (distance != 0){ // Escribim un comand en el cual si la distancia no es 0 que s'escrigui l'angle en el que esta el motor servo a la pantalla (segona fila)
    
    lcd.setCursor(5,1);
    lcd.print(angle); 
  }

  if (distance < 200){ // Escribim un comand en el cual si la distancia es menor a 200 escriu a la pantalla "Choques" (segona fila)
    
    lcd.setCursor(9,1);
    lcd.print("Choques");
  } 
  if (distance > 200){ // Escribim un comand en el cual si la distancia es major a 200 escriu a la pantalla "Vas be" (segona fila)
    lcd.print("Segueix");
  }
  

  
  if (gir_dret){ //Escribim un comand en el cual si "gir_dret" no a canvaiat i es true l'angle comensara a abançar de 2 en 2 i sino començara a retrocedir de 2 en 2
    angle+= 2; //angle +=5;

  } else{
    angle-= 2;
  }

  if (angle >= 180){ // Escribim un comand en el cual si l'angle es mes gran o igual que 180 la variable "gir_dret" sera false i si l'angle es mes petit o igual que 0 sera true

    gir_dret = false;
  } else if (angle <=0){

    gir_dret = true;
  }
  delay(40);

  myStepper.step(stepsPerRevolution); //Aquest comand activa el motor stepper

  

}