#include <ESP32Servo.h>
//#include "Metrics.h"
// Habilita debug para la impresion por el puerto serial ...
//----------------------------------------------
#define SERIAL_DEBUG_ENABLED 1
#if SERIAL_DEBUG_ENABLED
#define DebugPrint(str) \
{ \
Serial.println(str); \
}
#else
#define DebugPrint(str)
#endif
// Imprime estado actual con timestamp
#define DebugPrintState() \
{ \
String str; \
str = "****************************"; \
DebugPrint(str); \
str = " CURRENT STATE -> [" + states_s[current_state] + "]"; \
DebugPrint(str); \
str = "****************************"; \
DebugPrint(str); \
}
// Imprime evento detectado con timestamp
#define DebugPrintEvent() \
{ \
String str; \
str = "****************************"; \
DebugPrint(str); \
str = " EVENT DETECTED -> [" + events_s[current_event] + "]"; \
DebugPrint(str); \
str = "****************************"; \
DebugPrint(str); \
}
//----------------------------------------------
//----------------------------------------------
//Pin map
#define PIN_TRIGGER1 27
#define PIN_ECHO1 26
#define PIN_TRIGGER2 22
#define PIN_ECHO2 21
#define PIN_SERVO 33
#define PIN_COLOR_SENSOR1 32
#define PIN_COLOR_SENSOR2 23
#define PIN_DCENGINE_PWM 19 // Pin PWM para el motor 1 (Led Rojo)
#define PIN_DIR1_DCENGINE 18 // Pin de dirección 1 para el motor 1 (Led Azul)
#define PIN_DIR2_DCENGINE 5 // Pin de dirección 2 para el motor 1 (Led Amarillo)
#define PIN_RESTART_BUTTON 12
#define PIN_STOP_BUTTON 14
#define PIN_LED_STOP_ERROR 2
#define PIN_LED_MOVING 15
#define PIN_LED_WAITING 4
// Sensor and act configurations
#define SERIAL_SPEED 9600
#define SOUND_SPEED_DIV_2 0.01723
#define TIME_ULTRASONIC_2 2
#define TIME_ULTRASONIC_10 10
#define FRECUENCE_PWM 1000
#define RESOLUTION_PWM 8
#define CHANEL_PWM_ENGINE 4
#define SPEED_0 0
#define THRESHOLD_TIME_OUT 500
#define DISTANCE_SENSOR1 0
#define DISTANCE_SENSOR2 1
#define DISTANCE_SENSORS 2
#define MAX_TIME_TO_COLOR_SENSOR 5000 // ms -> 5 segundos
#define MAX_TIME_TO_END 8000 // ms -> 8 segundos
#define DISTANCE_THRESHOLD 5 //umbral de distancia
#define SERVO_POS_COLOR1 45
#define SERVO_POS_COLOR2 135
#define TIME_TO_OBJECT_0 0
#define LED_TASK_PRIORITY 1
#define LED_QUEUE_SIZE 1
#define STACK_SIZE 1024
#define MIN_PWM_SERVO_WIDTH 500
#define MAX_PWM_SERVO_WIDTH 2500
#define BLINK_LED_DELAY_MS 200
#define TASK_SENSOR_DELAY 500
#define SENSORS_PRIORITY 1
#define SENSORS_QUEUE_SIZE 10
// --- global variables
long t_start_moving = 0; // Marca de tiempo cuando comienza a moverse
bool waiting_color = false;
bool waiting_end = false;
int engine_speed = 150;
Servo Servo1;
struct stDistanceSensor
{
int pin_echo;
int pin_trigger;
long current_value;
long prev_value;
};
stDistanceSensor distanceSensors[DISTANCE_SENSORS];
enum DetectedColor {
NONE,
RED,
BLUE
};
// Functions declaration---------------------------------------------
void turn_on();
void start_moving();
void move_to_color1();
void move_to_color2();
void error_stop();
void manual_stop();
void restart();
void finish();
void none();
/******************************************************
State Machine
******************************************************/
#define MAX_STATES 5
#define MAX_EVENTS 9
enum states { ST_IDLE,
ST_MOVING,
ST_MANUAL_STOP,
ST_ERROR,
ST_COLOR_DETECTED} current_state;
String states_s [] = { "ST_IDLE",
"ST_MOVING",
"ST_MANUAL_STOP",
"ST_ERROR",
"ST_COLOR_DETECTED"};
enum events {EV_OBJECT_DETECTED,
EV_COLOR1_DETECTED,
EV_COLOR2_DETECTED,
EV_COLOR_ERROR,
EV_STOP_BUTTON,
EV_RESTART_BUTTON,
EV_TIMEOUT,
EV_OBJECT_AT_END,
EV_CONT } current_event;
String events_s [] = {"EV_OBJECT_DETECTED",
"EV_COLOR1_DETECTED",
"EV_COLOR2_DETECTED",
"EV_COLOR_ERROR",
"EV_STOP_BUTTON",
"EV_RESTART_BUTTON",
"EV_TIMEOUT",
"EV_OBJECT_AT_END",
"EV_CONT" };
typedef void (*transition)();
transition state_table[MAX_STATES][MAX_EVENTS] =
{
//EV_OBJECT_DETECTED EV_COLOR1_DETECTED EV_COLOR2_DETECTED EV_COLOR_ERROR EV_STOP_BUTTON EV_RESTART_BUTTON EV_TIMEOUT EV_OBJECT_AT_END EV_CONT
{ start_moving , none , none , none , none , none , none , none , none }, // ST_IDLE
{ none , move_to_color1 , move_to_color2 , error_stop , manual_stop , none , error_stop , none , none }, // ST_MOVING
{ none , none , none , none , none , restart , none , none , none }, // ST_MANUAL_STOP
{ none , none , none , none , none , restart , none , none , none }, // ST_ERROR
{ none , none , none , none , manual_stop , none , error_stop , finish , none } // ST_COLOR_DETECTED
};
/******************************************************
End State Machine
******************************************************/
/******************************************************
FREERTOS TASKS
******************************************************/
typedef enum {
LED_CMD_ON,
LED_CMD_OFF,
LED_CMD_BLINK
} LedCommand;
QueueHandle_t xQueueLed;
//-------- LED TASK------------------------------
void led_task(void *parameter) {
LedCommand cmd;
while (true) {
// espera hasta recibir un comando (bloqueado sin uso de CPU)
if (xQueueReceive(xQueueLed, &cmd, portMAX_DELAY)) {
switch (cmd) {
case LED_CMD_ON:
led_red_on();
break;
case LED_CMD_OFF:
led_red_off();
break;
case LED_CMD_BLINK:
do {
led_red_on();
vTaskDelay(pdMS_TO_TICKS(BLINK_LED_DELAY_MS));
led_red_off();
vTaskDelay(pdMS_TO_TICKS(BLINK_LED_DELAY_MS));
// revisa si hay un nuevo comando sin bloquear
} while (xQueueReceive(xQueueLed, &cmd, 0) == pdFALSE);
continue;
}
}
}
}
//--------Sensors task---------------------------------
QueueHandle_t eventQueue;
void sensors_task(void *pv) {
static int prevRestart = LOW;
while (true) {
// Distancia inicio
if (verifyObjectAtStart()) {
events ev = EV_OBJECT_DETECTED;
xQueueSend(eventQueue, &ev, portMAX_DELAY);
}
// Distancia fin
if (verifyObjectAtEnd()) {
events ev = EV_OBJECT_AT_END;
xQueueSend(eventQueue, &ev, portMAX_DELAY);
}
// Timeout
if (verifySensorsTimeout()) {
events ev = EV_TIMEOUT;
xQueueSend(eventQueue, &ev, portMAX_DELAY);
}
// Color
DetectedColor detected;
if (verifyColorSensor(detected)) {
events ev = (detected == RED) ? EV_COLOR1_DETECTED : EV_COLOR2_DETECTED;
xQueueSend(eventQueue, &ev, portMAX_DELAY);
}
// Restart button (polling con flanco)
int currentRestart = digitalRead(PIN_RESTART_BUTTON);
if (currentRestart == HIGH && prevRestart == LOW) {
events ev = EV_RESTART_BUTTON;
xQueueSend(eventQueue, &ev, portMAX_DELAY);
}
prevRestart = currentRestart;
vTaskDelay(pdMS_TO_TICKS(TASK_SENSOR_DELAY));
}
}
//---------------ISR BUtton STOP -------------------
// IRAM_ATTR coloca la ISR en RAM para que se ejecute rápido y seguro
void IRAM_ATTR stopButtonISR() {
events ev = EV_STOP_BUTTON;
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
xQueueSendFromISR(eventQueue, &ev, &xHigherPriorityTaskWoken);
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}
/******************************************************
END FREERTOS TASKS
******************************************************/
/******************************************************
Initialization
******************************************************/
void start(){
Serial.begin(SERIAL_SPEED);
//startMetrics();
pinMode(PIN_TRIGGER1, OUTPUT);
pinMode(PIN_ECHO1, INPUT);
pinMode(PIN_TRIGGER2, OUTPUT);
pinMode(PIN_ECHO2, INPUT);
pinMode(PIN_DIR1_DCENGINE, OUTPUT);
pinMode(PIN_DIR2_DCENGINE, OUTPUT);
pinMode(PIN_LED_STOP_ERROR, OUTPUT);
pinMode(PIN_LED_MOVING, OUTPUT);
pinMode(PIN_LED_WAITING, OUTPUT);
pinMode(PIN_RESTART_BUTTON, INPUT);
pinMode(PIN_STOP_BUTTON, INPUT);
pinMode(PIN_COLOR_SENSOR1, INPUT);
pinMode(PIN_COLOR_SENSOR2, INPUT);
//SERVO INITIALIZATION
Servo1.attach(PIN_SERVO, MIN_PWM_SERVO_WIDTH, MAX_PWM_SERVO_WIDTH);
distanceSensors[DISTANCE_SENSOR1].pin_trigger = PIN_TRIGGER1;
distanceSensors[DISTANCE_SENSOR1].pin_echo = PIN_ECHO1;
distanceSensors[DISTANCE_SENSOR2].pin_trigger = PIN_TRIGGER2;
distanceSensors[DISTANCE_SENSOR2].pin_echo = PIN_ECHO2;
// --- PWM DCENGINE ---
ledcAttachChannel(PIN_DCENGINE_PWM, FRECUENCE_PWM, RESOLUTION_PWM, CHANEL_PWM_ENGINE);
xQueueLed = xQueueCreate(LED_QUEUE_SIZE, sizeof(LedCommand));
eventQueue = xQueueCreate(SENSORS_QUEUE_SIZE, sizeof(events));
xTaskCreate(led_task, "led_task", STACK_SIZE, NULL, LED_TASK_PRIORITY, NULL);
xTaskCreate(sensors_task, "sensors_task", STACK_SIZE, NULL, SENSORS_PRIORITY, NULL);
// ISR Stop button
pinMode(PIN_STOP_BUTTON, INPUT);
attachInterrupt(digitalPinToInterrupt(PIN_STOP_BUTTON), stopButtonISR, RISING);
turn_on();
}
/******************************************************
End Initialization
*******************************************************/
/******************************************************
State Machine Action Functions
******************************************************/
void none(){
}
void turn_on(){
turn_green_led_off();
turn_red_led_off();
turn_yellow_led_on();
stop_engine();
current_state = ST_IDLE;
DebugPrintState();
}
void start_moving(){
DebugPrintEvent();
turn_yellow_led_off();
turn_green_led_on();
start_engine();
t_start_moving = millis();
waiting_color = true;
waiting_end = true;
current_state = ST_MOVING;
DebugPrintState();
}
void move_to_color1(){
DebugPrintEvent();
move_servo(SERVO_POS_COLOR1);
current_state = ST_COLOR_DETECTED;
waiting_color = false; // Flag color sensado
DebugPrintState();
}
void move_to_color2(){
DebugPrintEvent();
move_servo(SERVO_POS_COLOR2);
current_state = ST_COLOR_DETECTED;
waiting_color = false; // Flag color sensado
DebugPrintState();
}
void error_stop(){
DebugPrintEvent();
stop_engine();
turn_green_led_off();
blink_red_led();
current_state = ST_ERROR;
DebugPrintState();
}
void manual_stop(){
DebugPrintEvent();
stop_engine();
turn_green_led_off();
turn_red_led_on();
current_state = ST_MANUAL_STOP;
DebugPrintState();
}
void restart(){
DebugPrintEvent();
stop_engine();
turn_green_led_off();
turn_red_led_off();
turn_yellow_led_on();
waiting_color = false;
waiting_end = false;
current_state = ST_IDLE;
DebugPrintState();
}
void finish(){
restart();
}
/******************************************************
Emd State Machine Action Functions
******************************************************/
/******************************************************
Actuator Control Functions
******************************************************/
void move_servo(int angle){
Servo1.write(angle);
}
void turn_green_led_on(){
digitalWrite(PIN_LED_MOVING, HIGH);
}
void turn_green_led_off(){
digitalWrite(PIN_LED_MOVING, LOW);
}
void turn_red_led_on(){
LedCommand cmd = LED_CMD_ON;
xQueueOverwrite(xQueueLed, &cmd);
}
void blink_red_led(){
LedCommand cmd = LED_CMD_BLINK;
xQueueOverwrite(xQueueLed, &cmd);
}
void turn_red_led_off(){
LedCommand cmd = LED_CMD_OFF;
xQueueOverwrite(xQueueLed, &cmd);
}
void led_red_on(){
digitalWrite(PIN_LED_STOP_ERROR, HIGH);
}
void led_red_off(){
digitalWrite(PIN_LED_STOP_ERROR, LOW);
}
void turn_yellow_led_on(){
digitalWrite(PIN_LED_WAITING, HIGH);
}
void turn_yellow_led_off(){
digitalWrite(PIN_LED_WAITING, LOW);
}
void start_engine(){
//Gira izq
digitalWrite(PIN_DIR1_DCENGINE, HIGH);
digitalWrite(PIN_DIR2_DCENGINE, LOW);
analogWrite(PIN_DCENGINE_PWM, engine_speed);
}
void stop_engine(){
digitalWrite(PIN_DIR1_DCENGINE, LOW);
digitalWrite(PIN_DIR2_DCENGINE, LOW);
analogWrite(PIN_DCENGINE_PWM, SPEED_0);
}
/******************************************************
End Actuator Control Functions
******************************************************/
/******************************************************
Sensors Functions
******************************************************/
//COLOR SENSOR------------------------------------------------------
DetectedColor readColorSensor() {
if (digitalRead(PIN_COLOR_SENSOR1) == HIGH) return RED;
if (digitalRead(PIN_COLOR_SENSOR2) == HIGH) return BLUE;
return NONE;
}
bool verifyColorSensor(DetectedColor &detected) {
static bool colorAlreadyReported = false;
DetectedColor current = readColorSensor();
if ((current == RED || current == BLUE) && !colorAlreadyReported) {
detected = current;
colorAlreadyReported = true; // no repetir hasta que salga
return true;
}
if (current == NONE) {
colorAlreadyReported = false; // reset cuando ya no hay objeto
}
return false;
}
//END COLOR SENSOR------------------------------------------------------
//DISTANCE SENSORS------------------------------------------------------
int readDistanceSensor(stDistanceSensor distance_sensor)
{
int time_to_object = TIME_TO_OBJECT_0 ;
int pin_echo = distance_sensor.pin_echo;
int pin_trigger = distance_sensor.pin_trigger;
//Desactiva el trigger
digitalWrite(pin_trigger, LOW);
delayMicroseconds(TIME_ULTRASONIC_2);
//activa el Trigger por 10 microsegundos
digitalWrite(pin_trigger, HIGH);
delayMicroseconds(TIME_ULTRASONIC_10);
//Desactiva el trigger
digitalWrite(pin_trigger, LOW);
//Leo el pin de Echo, y obtengo el tiempo al objeto en microsegundos
time_to_object = pulseIn(pin_echo, HIGH);
return calcDistance(time_to_object);
}
bool verifyObjectAtStart(){
distanceSensors[DISTANCE_SENSOR1].current_value=readDistanceSensor(distanceSensors[DISTANCE_SENSOR1]);
int current_value = distanceSensors[DISTANCE_SENSOR1].current_value;
int prev_value = distanceSensors[DISTANCE_SENSOR1].prev_value;
if(current_value != prev_value){
distanceSensors[DISTANCE_SENSOR1].prev_value = current_value;
if(current_value <= DISTANCE_THRESHOLD){
return true;
}
}
return false;
}
bool verifyObjectAtEnd(){
distanceSensors[DISTANCE_SENSOR2].current_value=readDistanceSensor(distanceSensors[DISTANCE_SENSOR2]);
int current_value = distanceSensors[DISTANCE_SENSOR2].current_value;
int prev_value = distanceSensors[DISTANCE_SENSOR2].prev_value;
if(current_value != prev_value){
distanceSensors[DISTANCE_SENSOR2].prev_value = current_value;
if(current_value <= DISTANCE_THRESHOLD){
return true;
}
}
return false;
}
int calcDistance(int time_to_object)
{
//calculo la distancia al objeto aplicando la formula
int distanceToObject=SOUND_SPEED_DIV_2*time_to_object;
return distanceToObject;
}
//END DISTANCE SENSORS------------------------------------------------------
/******************************************************
End Sensors Functions
******************************************************/
// TIMEOUT VERIFICATION ------------------------------------------------------------
bool verifySensorsTimeout(){
if(current_state != ST_ERROR && current_state != ST_MANUAL_STOP)
{
// Chequeo de timeout de color
if (waiting_color && (millis() - t_start_moving > MAX_TIME_TO_COLOR_SENSOR)) {
DebugPrint("TIMEOUT -> No se detectó color a tiempo!");
return true;
}
// Chequeo de timeout de final
if (waiting_end && (millis() - t_start_moving > MAX_TIME_TO_END)) {
DebugPrint("TIMEOUT -> El objeto no llegó al sensor final a tiempo!");
return true;
}
}
return false;
}
//END TIMEOUT VERIFICATION ------------------------------------------------------------
/******************************************************
Get new EVENT
******************************************************/
void get_new_event( )
{
events new_event;
//se bloquea en espera de un nuevo evento
if( xQueueReceive(eventQueue, &new_event, portMAX_DELAY) == pdPASS )
{
if (new_event != current_event) {
current_event = new_event;
}
}else {
current_event = EV_CONT; // seguridad
}
}
/******************************************************
END Get new EVENT
******************************************************/
/******************************************************
State Machine Function
******************************************************/
void fsm( )
{
get_new_event();
if( (current_event >= 0) && (current_event < MAX_EVENTS) && (current_state >= 0) && (current_state < MAX_STATES) )
{
state_table[current_state][current_event]();
}
else
{
current_state = ST_ERROR;
}
}
/******************************************************
END State Machine Function
******************************************************/
void setup()
{
start();
}
void loop()
{
fsm();
}
SENSOR DISTANCIA
INICIO CINTA
5v (15ma)
fuente 5v
fuente 3,3v
servo 5v (500ma)
SIMULA SENSOR
DE COLOR
PULSADORES
MOTOR DC
PUENTE H
CONTROL PWM
SIMULADO CON LEDS
RESTART
>> LA LOGICA SERIA:
>>ENERGIZO Y EL SISTEMA SE ACTIVA.
>>CUANDO EL SENSOR DE DISTANCIA AL INICIO DE LA CINTA DETECTE UN OBJETO
COMIENZA A GIRAR EL MOTOR.
>> DETECTA COLOR Y MUEVE SERVO.
DEBO PRESIONAR BOTON ROJO O AZUL QUE SIMULAN LOS COLORES
>>EN LA SALIDA DE LA CINTA DETECTA QUE EL
OBJETO PASO Y APAGA MOTOR
>> SI EL OBJETO INGRESA A LA CINTA PERO NUNCA LLEGA AL FINAL ENCIENDE LED
ROJO DE ERROR
>> SI SE PRESIONA BOTON DE STOP LA CINTA SE DETIENE.
Y DEBO PRESIONAR RESTART PARA QUE VUELVA A IDLE
SENSOR DISTANCIA
FIN CINTA
5v (15ma)
MOVING
STOP
STOP_ERROR
IDLE