#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();
}