Buenas, estoy haciendo un mini-proyecto para la universidad donde tengo que calcular la velocidad y aceleración de una pequeña pelota al dejarse caer por una barra que se mueve en diferentes ángulos. He escrito este código, pero cuando lo compilo nada funciona ni la pantalla LCD, ni la barra se mueve ni los sensores ir calculan nada. Alguien puede ayudarme?
Aquí les adjunto el código:
#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();
}