#include <LiquidCrystal_I2C.h>  
      
#include <Wire.h>

#include <Servo.h>

double servo2;
double vel;
double acc;
double t1;
double t2;
double t3;
double vel1;
double t4;
double vel2;
double vel3;

boolean b_s1;
boolean b_s2;
boolean b_s3;
boolean b_s4;

LiquidCrystal_I2C lcd(0x27,16,2);
unsigned long task_time_ms=0;

Servo servo_4;

Servo servo_5;

unsigned long time_timer=millis();


void serial() {
  if((millis()-task_time_ms)>=2000){
    task_time_ms=millis();
    Serial.print(String("Velocitat:  "));
    Serial.println(vel);
    Serial.print(String("Acceleracio: "));
    Serial.println(acc);
  }
}

void LCD() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(String("Vel"));
  lcd.setCursor(8, 0);
  lcd.print(String("Acc"));
  lcd.setCursor(0, 1);
  lcd.print(vel);
  lcd.setCursor(8, 1);
  lcd.print(acc);
}

void servo() {
  for (servo2 = 72; servo2 >= 60; servo2=servo2-2) {
    servo_4.write(servo2);
    delay(100);
  }
  servo_5.write(45);
  delay(2000);
  servo_5.write(0);
  delay(100);
  for (servo2 = 60; servo2 <= 72; servo2=servo2+2) {
    servo_4.write(servo2);
    delay(100);
  }
  delay(3000);
  for (servo2 = 72; servo2 >= 52; servo2=servo2-2) {
    servo_4.write(servo2);
    delay(100);
  }
  servo_5.write(45);
  delay(2000);
  servo_5.write(0);
  delay(100);
  for (servo2 = 52; servo2 <= 72; servo2=servo2+2) {
    servo_4.write(servo2);
    delay(100);
  }
  delay(3000);
  for (servo2 = 72; servo2 >= 44; servo2=servo2-2) {
    servo_4.write(servo2);
    delay(100);
  }
  servo_5.write(45);
  delay(2000);
  servo_5.write(0);
  delay(100);
  for (servo2 = 44; servo2 <= 72; servo2=servo2+2) {
    servo_4.write(servo2);
    delay(100);
  }
}

void sensors() {
  b_s1 = digitalRead(8);
  b_s2 = digitalRead(9);
  b_s3 = digitalRead(10);
  b_s4 = digitalRead(11);
  if ((!b_s1)) {
    time_timer=millis();
    t1 = millis();
    t1 = (t1 / 1000);

  }
  if ((!b_s2)) {
    t2 = millis();
    t2 = (t2 / 1000);
    vel1 = (0.26 / ((t2 - t1)));

  }
  if ((!b_s3)) {
    t3 = millis();
    t3 = (t3 / 1000);
    vel2 = (0.5 / ((t3 - t2)));

  }
  if ((!b_s4)) {
    t4 = millis();
    t4 = (t4 / 1000);
    vel3 = (0.26 / ((t4 - t3)));
    vel = (((vel1 + ((vel2 + vel3)))) / 3);
    acc = (vel / ((t4 - t1)));

  }
}

void setup()
{
  lcd.init();
  lcd.noCursor();
  lcd.backlight();
Serial.begin(9600); Serial.flush(); while(Serial.available()>0)Serial.read();

servo_4.attach(4);

servo_5.attach(5);

pinMode(8, INPUT);
pinMode(9, INPUT);
pinMode(10, INPUT);
pinMode(11, INPUT);
  b_s1 = true;
  b_s2 = true;
  b_s3 = true;
  b_s4 = true;
  t1 = 0;
  t2 = 0;
  t3 = 0;
  t4 = 0;
  vel3 = 0;
  vel = 0;
  acc = 0;
  servo2 = 72;

}


void loop()
{

    sensors();
    LCD();
    serial();
    servo();
}