#include <ESP32Servo.h>
#define okR 27
#define okG 32
#define okB 33
const int servoLidarPin = 17;
const int servoRojoPin = 2;
const int servoVerdePin = 4;
const int servoAzulPin = 16;
int bolasRpedidas = 3;
int bolasGpedidas = 3;
int bolasBpedidas = 3;
volatile int bolasRservidas = 0;
volatile int bolasGservidas = 0;
volatile int bolasBservidas = 0;
int listo = LOW;
int reset = 0;
int estado; //0=Vacio, 1= Trabajando, 2= Listo, 3= Error...)
int rLevel;
int gLevel;
int bLevel;
unsigned long startTime = 0;
const int timeDebounce = 500;
int tiempoCaida = 1000;
int timeDebug = 1000;
int reintentoBola = 0;
int gradosReposo = 0;
int gradosR = 60;
int gradosG = 120;
int gradosB = 180;
int servoR0 = 0;
int servoR1 = 180;
int posRojo = 0;
int servoG0 = 0;
int servoG1 = 180;
int posVerde = 0;
int servoB0 = 0;
int servoB1 = 180;
bool posAzul = 0;
unsigned long int tiempoMedida = 0;
int tiempoEntreMedidas = 10000;
int delayServo = 1;
int alturaMedida;
Servo servoLidar;
Servo servoRojo;
Servo servoVerde;
Servo servoAzul;
void IRAM_ATTR detectadoR() { //IRAM_ATTR sirve para que se ejecute en la ram en lugar de en la flash y así es más rápida.
if (millis() - startTime > timeDebounce) {
bolasRservidas++;
startTime = millis();
}
}
void IRAM_ATTR detectadoG() {
if (millis() - startTime > timeDebounce) {
bolasGservidas++;
startTime = millis();
}
}
void IRAM_ATTR detectadoB() {
if (millis() - startTime > timeDebounce) {
bolasBservidas++;
startTime = millis();
}
}
void IRAM_ATTR cambioListo() { //pulsador que fuerza listo desde el plc
if (millis() - startTime > timeDebounce) {
listo = !listo;
startTime = millis();
}
}
void setup() {
pinMode(okR, INPUT_PULLUP);
pinMode(okG, INPUT_PULLUP);
pinMode(okB, INPUT_PULLUP);
pinMode(34, INPUT); //pin para pruebas de lectura de nivel con entrada analogica.
pinMode(18, INPUT_PULLUP);// pin para flip/flop de listo en PLC
attachInterrupt(digitalPinToInterrupt(okR), detectadoR, RISING);
attachInterrupt(digitalPinToInterrupt(okG), detectadoG, RISING);
attachInterrupt(digitalPinToInterrupt(okB), detectadoB, RISING);
attachInterrupt(digitalPinToInterrupt(18), cambioListo, RISING);
servoLidar.attach(servoLidarPin);
servoRojo.attach(servoRojoPin);
servoVerde.attach(servoVerdePin);
servoAzul.attach(servoAzulPin);
Serial.begin(115200);
servoLidar.write(gradosReposo);
servoRojo.write(servoR1 - servoR0);
servoVerde.write(servoG1 - servoG0);
servoAzul.write(servoB1 - servoB0);
tiempoMedida = millis();
}
void loop() {
nivelTolvas();
delay(10); // this speeds up the simulation
Serial.print("Rojas: ");
Serial.print(bolasRservidas);
Serial.print("/");
Serial.println(bolasRpedidas);
Serial.print("Verdes: ");
Serial.print(bolasGservidas);
Serial.print("/");
Serial.println(bolasGpedidas);
Serial.print("Azules: ");
Serial.print(bolasBservidas);
Serial.print("/");
Serial.println(bolasBpedidas);
Serial.print("Estado: ");
Serial.println(estado);
Serial.print("Listo: ");
Serial.println(listo);
Serial.print("Reintento: ");
Serial.println(reintentoBola);
Serial.println("");
if (reset == LOW) {
switch (estado) { //etapas
case 0:
delay(timeDebug);
if (listo == HIGH) {
bolasRservidas = 0;
bolasGservidas = 0;
bolasBservidas = 0;
delay(100);
estado = 1;
reintentoBola = 0;
}
break;
case 1:
delay(timeDebug);
if (bolasRpedidas > bolasRservidas) { //Salida de bola roja
switch (posRojo) {
case 0:
servoRojo.write(servoR0);
posRojo = 1;
break;
case 1:
servoRojo.write(servoR1);
posRojo = 0;
break;
}
}
if (bolasGpedidas > bolasGservidas) { //Salida de bola verde
switch (posVerde) {
case 0:
servoVerde.write(servoG0);
posVerde = 1;
break;
case 1:
servoVerde.write(servoG1);
posVerde = 0;
break;
}
}
if (bolasBpedidas > bolasBservidas) { //Salida de bola azul
switch (posAzul) {
case 0:
servoAzul.write(servoB0);
posAzul = 1;
break;
case 1:
servoAzul.write(servoB1);
posAzul = 0;
break;
}
}
delay(tiempoCaida);
if (bolasRpedidas <= bolasRservidas && bolasGpedidas <= bolasGservidas && bolasBpedidas <= bolasBservidas) { // si estan todas las bolas pedidas
estado = 2;
}
reintentoBola++;
if (reintentoBola >= 10) estado = 3;
break;
case 2:
delay(timeDebug);
if (listo == LOW) {
estado = 0;
}
break;
case 3:
delay(timeDebug);
Serial.println("Error");
if (listo == LOW) {
estado = 0;
}
break;
}
}
}
void nivelTolvas() {
if (millis() >= tiempoMedida + tiempoEntreMedidas) {
for (int i = gradosReposo; i < gradosR; i++) {
servoLidar.write(i);
delay(delayServo);
}
medirNivel();
rLevel = alturaMedida;
for (int i = gradosR; i < gradosG; i++) {
servoLidar.write(i);
delay(delayServo);
}
medirNivel();
gLevel = alturaMedida;
for (int i = gradosG; i < gradosB; i++) {
servoLidar.write(i);
delay(delayServo);
}
medirNivel();
bLevel = alturaMedida;
for (int i = gradosB; i > gradosReposo; i--) {
servoLidar.write(i);
delay(delayServo);
}
Serial.print("Nivel Rojo: ");
Serial.println(rLevel);
Serial.print("Nivel Verde: ");
Serial.println(gLevel);
Serial.print("Nivel Azul: ");
Serial.println(bLevel);
tiempoMedida = millis();
}
}
void medirNivel() {
alturaMedida = map(analogRead(34), 0, 4095, 0, 100) ;
delay (1000);
}