139 lines
5.4 KiB
C++
139 lines
5.4 KiB
C++
#include <Arduino.h>
|
|
#include <Wire.h>
|
|
#include <Adafruit_PWMServoDriver.h>
|
|
|
|
// Pines de comunicación I2C para el bloque de colores
|
|
#define I2C_SDA 27
|
|
#define I2C_SCL 33
|
|
|
|
// Pines de las entradas optoacopladas (Bus de 6 bits)
|
|
const int pinBit0 = 17; // Peso 1 (Número de Servo)
|
|
const int pinBit1 = 18; // Peso 2 (Número de Servo)
|
|
const int pinBit2 = 19; // Peso 4 (Número de Servo)
|
|
const int pinBit3 = 21; // Peso 8 (Número de Servo)
|
|
const int pinEstado = 22; // 5to Bit (0 = 0 grados, 1 = 180 grados)
|
|
const int pinRun = 23; // 6to Bit (0 = Inactivo, 1 = Ejecutar Acción)
|
|
|
|
// Inicializamos el controlador de servos
|
|
Adafruit_PWMServoDriver pca = Adafruit_PWMServoDriver(0x40);
|
|
|
|
// Valores calibrados y seguros para tus servos Steren MOT-135
|
|
#define SERVOMIN 170 // Equivale a 0 grados aproximados
|
|
#define SERVOMAX 500 // Equivale a 180 grados aproximados
|
|
|
|
// Lista de los canales del PCA9685 donde conectarás tus 10 servos.
|
|
const int canalesServos[10] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9};
|
|
|
|
// Variable para rastrear si ya ejecutamos la acción del ciclo actual
|
|
bool accionEjecutada = false;
|
|
|
|
void setup() {
|
|
Serial.begin(115200);
|
|
Wire.begin(I2C_SDA, I2C_SCL);
|
|
|
|
pca.begin();
|
|
pca.setPWMFreq(50);
|
|
|
|
// Configurar las 6 entradas optoacopladas
|
|
pinMode(pinBit0, INPUT);
|
|
pinMode(pinBit1, INPUT);
|
|
pinMode(pinBit2, INPUT);
|
|
pinMode(pinBit3, INPUT);
|
|
pinMode(pinEstado, INPUT);
|
|
pinMode(pinRun, INPUT);
|
|
|
|
// Inicializar todos los servos en la posición inicial (0 grados) por seguridad
|
|
Serial.println("Colocando todos los servos a 0 grados...");
|
|
for (int i = 0; i < 10; i++) {
|
|
pca.setPWM(canalesServos[i], 0, SERVOMIN);
|
|
delay(100); // Pausa corta para no saturar la fuente
|
|
}
|
|
|
|
Serial.println("Sistema PLC-Servo Receptor de 6 Bits Listo.");
|
|
}
|
|
|
|
void loop() {
|
|
// 1. Leer el bit de ejecución (6to bit) primero
|
|
int bitRun = digitalRead(pinRun);
|
|
|
|
if (bitRun == HIGH) {
|
|
// Si la señal de RUN está activa y aún no hemos ejecutado la orden en este ciclo:
|
|
if (!accionEjecutada) {
|
|
|
|
// Leer el resto de los pines físicos
|
|
int b0 = digitalRead(pinBit0);
|
|
int b1 = digitalRead(pinBit1);
|
|
int b2 = digitalRead(pinBit2);
|
|
int b3 = digitalRead(pinBit3);
|
|
int bitEstado = digitalRead(pinEstado); // 5to bit
|
|
|
|
// Calcular el número de servo seleccionado (Bits 1 al 4)
|
|
int numeroSeleccionado = (b3 * 8) + (b2 * 4) + (b1 * 2) + (b0 * 1);
|
|
|
|
// --- NUEVA LÓGICA: COMANDOS GLOBALES (100000 y 110000) ---
|
|
// Si el número es 0, es una orden para afectar a TODOS los motores a la vez
|
|
if (numeroSeleccionado == 0) {
|
|
|
|
// El 5to bit decide a dónde van a ir todos los motores
|
|
int pulsoGlobal = (bitEstado == HIGH) ? SERVOMAX : SERVOMIN;
|
|
int gradosGlobales = (bitEstado == HIGH) ? 180 : 0;
|
|
|
|
Serial.println("--- COMANDO GLOBAL RECIBIDO ---");
|
|
Serial.print("Moviendo TODOS los servos a ");
|
|
Serial.print(gradosGlobales);
|
|
Serial.println(" grados...");
|
|
|
|
for (int i = 0; i < 10; i++) {
|
|
pca.setPWM(canalesServos[i], 0, pulsoGlobal);
|
|
delay(80); // Pausa escalonada crucial para no activar la protección de la fuente
|
|
}
|
|
|
|
accionEjecutada = true;
|
|
Serial.println("Acción global completada. Esperando que el PLC apague el bit RUN (D23)...");
|
|
|
|
}
|
|
// --- LÓGICA INDIVIDUAL: Mover un servo específico (1 al 10) ---
|
|
else if (numeroSeleccionado >= 1 && numeroSeleccionado <= 10) {
|
|
|
|
int indiceServo = numeroSeleccionado - 1;
|
|
int canalPCA = canalesServos[indiceServo];
|
|
|
|
// Determinar el pulso objetivo según el 5to bit (Estado)
|
|
int pulsoObjetivo = (bitEstado == HIGH) ? SERVOMAX : SERVOMIN;
|
|
int gradosObjetivo = (bitEstado == HIGH) ? 180 : 0;
|
|
|
|
Serial.print("--- EJECUCIÓN INDIVIDUAL RECIBIDA ---");
|
|
Serial.print("\nServo seleccionado: "); Serial.print(numeroSeleccionado);
|
|
Serial.print(" (Canal PCA: "); Serial.print(canalPCA); Serial.print(")");
|
|
Serial.print("\nAcción solicitada: Mover a "); Serial.print(gradosObjetivo); Serial.println(" grados.");
|
|
|
|
// Enviar la señal al servo seleccionado
|
|
pca.setPWM(canalPCA, 0, pulsoObjetivo);
|
|
|
|
// Damos tiempo físico para que el servo de alto torque complete el giro
|
|
delay(1200);
|
|
|
|
// Marcamos que la acción ya se hizo para bloquear repeticiones en ciclo infinito
|
|
accionEjecutada = true;
|
|
Serial.println("Acción completada. Esperando que el PLC apague el bit RUN (D23)...");
|
|
|
|
}
|
|
// Lógica de manejo de errores
|
|
else {
|
|
Serial.print("Error: RUN activo pero número de comando inválido recibido: ");
|
|
Serial.println(numeroSeleccionado);
|
|
accionEjecutada = true; // Bloquea el ciclo para evitar spam de error en el monitor serial
|
|
}
|
|
}
|
|
}
|
|
else {
|
|
// Cuando el PLC apaga el bit de RUN (D23 cambia a 0), liberamos el candado
|
|
// quedando listos para el próximo comando.
|
|
if (accionEjecutada) {
|
|
accionEjecutada = false;
|
|
Serial.println("Línea despejada. ESP32 listo para recibir el siguiente comando.");
|
|
}
|
|
}
|
|
|
|
delay(20); // Delay corto para dar estabilidad al ciclo de lectura del microcontrolador
|
|
} |