ROBOT SIGUE LINEAS - SENSOR QTR8A - MOTORES SOBRE PCB

 

DISEÑO FRITZING


*Diseñado para sensor QTR8a.

*Puede usar motores Amarillos de 220 RPM o motores N20, N30.

*Motores directamente sobre el PCB, requiere porta motores metálicos. El porta sensor es en impresión 3D y se atornilla al PCB.


CODIGO ARDUINO


//ROBOT SCRAWLER - BATERIA 2S
//SIN CONTROL ADAPTATIVO DE VELOCIDAD
//TIEMPO: 11:27
#include <QTRSensors.h>
QTRSensors qtra;  // Objeto para manejar el array de sensores QTR

// Configuración del número de sensores (8 sensores en total)
const uint8_t SensorCount = 8;
uint16_t sensorValues[SensorCount];  // Array para almacenar lecturas de sensores

// MAPEO DE PINES PARA DRIVER DE MOTORES TB6612FNG
#define STBY 3    // Pin de standby del driver (HIGH = activo)
#define AIN1 8    // Control de dirección motor A (derecho)
#define AIN2 9    // Control de dirección motor A (derecho)
#define PWMB 6    // Control PWM motor B (izquierdo)
#define PWMA 5    // Control PWM motor A (derecho)
#define BIN1 4    // Control de dirección motor B (izquierdo)
#define BIN2 7    // Control de dirección motor B (izquierdo)
#define LED 12    // LED indicador de estados
#define LED2 13  
#define PINBOTON 2    // Botón para iniciar calibración y operación (PULL-UP)
#define PINBUZZER 10  // Buzzer para señales auditivas

// CONSTANTES DEL CONTROLADOR PID
float KP = 0.1;      // Ganancia proporcional - respuesta inmediata al error
float KD = 0.5;      // Ganancia derivativa - predice cambios futuros
float Ki = 0.0006;   // Ganancia integral - corrige errores acumulados
int Velmax = 250;    // Velocidad máxima de los motores (0-255)

// CONSTANTE PARA VELOCIDAD DE BÚSQUEDA (simplificación)
const int VELOCIDAD_BUSQUEDA = 200;  // Velocidad fija para búsqueda

// VARIABLES PARA IMPLEMENTAR TÉRMINO INTEGRAL CON HISTORIAL
int error1 = 0;  // Error más reciente
int error2 = 0;  // Error anterior 1
int error3 = 0;  // Error anterior 2
int error4 = 0;  // Error anterior 3
int error5 = 0;  // Error anterior 4
int error6 = 0;  // Error más antiguo - mantiene 6 muestras para el integral

// FUNCIÓN PARA DETECTAR CURVA DE 90° A LA IZQUIERDA
bool shouldTurnLeft() {
  // MAPEO: SENSOR 0 = EXTREMO DERECHO, SENSOR 7 = EXTREMO IZQUIERDO
  // Lógica: Para curva izquierda, sensores derechos ven blanco, izquierdos ven línea
  if (sensorValues[0] < 100 && sensorValues[1] < 100 &&    // Extremo derecho ve superficie blanca
      sensorValues[6] > 401 && sensorValues[7] > 401) {    // Extremo izquierdo ve línea negra
    return true;  // Confirma presencia de curva 90° izquierda
  }
  return false;   // No hay curva detectada
}

// FUNCIÓN PARA DETECTAR BIFURCACIÓN Y DECIDIR TOMAR RAMA IZQUIERDA
bool shouldTakeLeftBranch() {
  // BIFURCACIÓN: Múltiples sensores ven línea simultáneamente
  // Condición: lado izquierdo ve línea, lado derecho ve blanco (favorece rama izquierda)
  if (sensorValues[6] > 600 &&        // Centro-izquierdo detecta línea (umbral alto)
      sensorValues[7] > 600 &&        // Extremo izquierdo detecta línea
      sensorValues[5] > 600 &&        // Otro sensor centro-izquierdo detecta línea
      sensorValues[0] < 400) {        // Extremo derecho ve blanco (no hay rama derecha)
    return true;  // Condiciones para tomar bifurcación izquierda
  }
  return false;   // No hay bifurcación izquierda o condiciones no se cumplen
}

// FUNCIÓN PARA CONTROLAR MOTOR IZQUIERDO (MOTOR B)
void Motoriz(int value) {
  if (value >= 0) {  // Movimiento hacia adelante
    digitalWrite(BIN1, HIGH);  // Configuración para giro adelante
    digitalWrite(BIN2, LOW);   // BIN1=HIGH, BIN2=LOW = adelante
  } else {           // Movimiento hacia atrás
    digitalWrite(BIN1, LOW);   // Configuración para giro atrás
    digitalWrite(BIN2, HIGH);  // BIN1=LOW, BIN2=HIGH = atrás
    value *= -1;     // Convertir valor negativo a positivo para PWM
  }
  analogWrite(PWMB, value);    // Aplicar velocidad (0-255) al motor izquierdo
}

// FUNCIÓN PARA CONTROLAR MOTOR DERECHO (MOTOR A)
void Motorde(int value) {  
  if (value >= 0) {  // Movimiento hacia adelante
    digitalWrite(AIN1, HIGH);  // Configuración para giro adelante
    digitalWrite(AIN2, LOW);   // AIN1=HIGH, AIN2=LOW = adelante
  } else {           // Movimiento hacia atrás
    digitalWrite(AIN1, LOW);   // Configuración para giro atrás
    digitalWrite(AIN2, HIGH);  // AIN1=LOW, AIN2=HIGH = atrás
    value *= -1;     // Convertir valor negativo a positivo para PWM
  }    
  analogWrite(PWMA, value);    // Aplicar velocidad (0-255) al motor derecho
}

// FUNCIÓN PARA CONTROLAR AMBOS MOTORES SIMULTÁNEAMENTE
void Motor(int left, int right) {
  digitalWrite(STBY, HIGH);  // Activar driver de motores (salir de standby)
  Motoriz(left);             // Configurar motor izquierdo con velocidad 'left'
  Motorde(right);            // Configurar motor derecho con velocidad 'right'
}

void setup() {
  Serial.begin(9600);  // Inicializar comunicación serial para debug
 
  // CONFIGURACIÓN DE PINES COMO SALIDAS
  pinMode(LED, OUTPUT);      // LED indicador
  pinMode(LED2, OUTPUT);     // LED2 indicador
  digitalWrite(LED, HIGH);   // Encender LED al inicio
  digitalWrite(LED2, LOW);   // LED2 apagado al inicio
  pinMode(BIN2, OUTPUT);     // Dirección motor izquierdo
  pinMode(STBY, OUTPUT);     // Standby del driver
  pinMode(BIN1, OUTPUT);     // Dirección motor izquierdo
  pinMode(PWMB, OUTPUT);     // PWM motor izquierdo
  pinMode(AIN1, OUTPUT);     // Dirección motor derecho
  pinMode(AIN2, OUTPUT);     // Dirección motor derecho
  pinMode(PWMA, OUTPUT);     // PWM motor derecho
  pinMode(PINBUZZER, OUTPUT); // Buzzer como salida
 
  // CONFIGURACIÓN DEL BOTÓN CON PULL-UP INTERNO
  pinMode(PINBOTON, INPUT_PULLUP);  // Botón con resistencia pull-up interna activada
 
  // CONFIGURACIÓN DEL ARRAY DE SENSORES QTR
  qtra.setTypeAnalog();      // Configurar sensores como analógicos
  // Mapear pines analógicos A0-A7 a los 8 sensores
  qtra.setSensorPins((const uint8_t[]){A0, A1, A2, A3, A4, A5, A6, A7}, SensorCount);
  qtra.setEmitterPin(11);    // Pin que controla LEDs infrarrojos de los sensores
 
  // ESPERAR PRESIÓN DE BOTÓN PARA INICIAR CALIBRACIÓN
  WaitBoton();  
 
  // PROCESO DE CALIBRACIÓN AUTOMÁTICA (70 muestras)
  for (uint16_t i = 0; i < 70; i++) {
    digitalWrite(LED, HIGH);  // LED ON durante calibración
    delay(20);                // Pausa entre muestras
    qtra.calibrate();         // Tomar muestra de calibración
    digitalWrite(LED, LOW);   // LED OFF
    delay(20);                // Pausa entre muestras
  }
 
  // SEÑALES AUDITIVAS DE CONFIRMACIÓN
  tone(PINBUZZER, 1500, 50);  // Primer beep
  delay(70);
  tone(PINBUZZER, 1500, 50);  // Segundo beep - calibración completa
  delay(70);
 
  // ESPERAR SEGUNDA PRESIÓN DE BOTÓN PARA INICIAR OPERACIÓN
  WaitBoton();
}

// VARIABLES DEL CONTROLADOR PID
int proporcional = 0;  // Error actual (diferencia entre posición deseada y real)
int integral = 0;      // Suma de errores pasados (corrige offset permanente)
int derivativo = 0;    // Cambio del error (predice tendencia)
int diferencial = 0;   // Salida final del PID (corrección a aplicar)
int last_prop = 0;     // Error anterior para calcular derivativo - INICIALIZADO CORRECTAMENTE
int Target = 3500;     // Posición objetivo (centro del array de sensores)

void loop() {
  // LEER POSICIÓN DE LA LÍNEA (0-7000, donde 3500 = centro)
  uint16_t position = qtra.readLineBlack(sensorValues);
 
  // DETECCIÓN DE BIFURCACIÓN PRIORITARIA
  if (shouldTakeLeftBranch()) {
    // BIFURCACIÓN DETECTADA - ejecutar maniobra de giro pronunciado
    digitalWrite(LED, HIGH);   // LED ON indica maniobra especial
    //Motor(240, 0);            // Motor izq a 240, derecho parado = giro derecha fuerte
    //delay(50);                // Mantener giro por 50ms para comprometerse con rama
  }
 
  // DETECCIÓN DE CURVA 90° (solo si no hay bifurcación)
  else if (shouldTurnLeft()) {
    // CURVA 90° DETECTADA - ejecutar giro normal
    //digitalWrite(LED, LOW);    // LED OFF indica giro normal
    //Motor(240, 0);            // Motor izq a 240, derecho parado = giro derecha
    //delay(50);                // Mantener giro por 50ms
  }
  else {
    // CONTROL PID NORMAL - solo cuando no hay maniobras especiales
    digitalWrite(LED, LOW);    // LED OFF indica operación normal
   
    // CALCULAR ERROR PROPORCIONAL
    proporcional = ((int)position) - Target;  // Error = posición_actual - posición_deseada
   
    // SISTEMA DE BÚSQUEDA ACTIVA SIMPLIFICADO
    if (proporcional <= -Target) {      // Línea perdida hacia la DERECHA (position = 0)
        // BUSCAR GIRANDO DERECHA: motor izq parado, derecho a velocidad fija
        Motor(0, VELOCIDAD_BUSQUEDA);   // Velocidad de búsqueda constante (200)
       
    } else if (proporcional >= Target) {  // Línea perdida hacia la IZQUIERDA (position = 7000)
        // BUSCAR GIRANDO IZQUIERDA: motor izq a velocidad fija, derecho parado
        Motor(VELOCIDAD_BUSQUEDA, 0);   // Velocidad de búsqueda constante (200)
    }
    else {
        // CONTROL PID NORMAL - EJECUTA SOLO CUANDO NO ESTÁ EN BÚSQUEDA
       
        // CALCULAR TÉRMINO DERIVATIVO (predice tendencia del error)
        derivativo = proporcional - last_prop;
       
        // CALCULAR TÉRMINO INTEGRAL CON ANTI-WINDUP (suma errores pasados)
        integral = error1 + error2 + error3 + error4 + error5 + error6;
       
        // ANTI-WINDUP: Resetear integral cuando robot cruza la línea
        // Detecta cambio de signo del error (cruzó de un lado al otro)
        if ((proporcional > 0 && last_prop < 0) || (proporcional < 0 && last_prop > 0)) {
          integral = 0;  // Resetear integral - elimina "memoria tóxica"
          // Limpiar historial completo para empezar fresco
          error1 = error2 = error3 = error4 = error5 = error6 = 0;
        }
       
        // LIMITAR INTEGRAL para evitar saturación y sobrecorrección
        integral = constrain(integral, -1000, 1000);
       
        // Guardar error actual para próximo cálculo derivativo
        last_prop = proporcional;

        // ACTUALIZAR HISTORIAL DE ERRORES (registro circular)
        error6 = error5;  // El más antiguo se descarta
        error5 = error4;
        error4 = error3;
        error3 = error2;
        error2 = error1;
        error1 = proporcional;  // El más reciente entra

        // CÁLCULO FINAL PID: combinar los tres términos
        diferencial = (proporcional * KP) + (derivativo * KD) + (integral * Ki);
       
        // LIMITAR SALIDA PID a rangos seguros del motor
        if (diferencial > Velmax) diferencial = Velmax;    // No exceder velocidad máxima
        if (diferencial < -Velmax) diferencial = -Velmax;  // No exceder en reversa

        // APLICAR CORRECCIÓN DIFERENCIAL A LOS MOTORES
        if (diferencial < 0) {
          // Error negativo: robot va hacia izquierda, corregir hacia derecha
          // Reducir velocidad motor izquierdo, mantener derecho al máximo
          Motor(Velmax + diferencial, Velmax);  // diferencial negativo reduce vel izq
        } else {
          // Error positivo: robot va hacia derecha, corregir hacia izquierda  
          // Mantener motor izquierdo al máximo, reducir derecho
          Motor(Velmax, Velmax - diferencial);  // diferencial positivo reduce vel der
        }
    }
  }
}

// FUNCIÓN QUE ESPERA PRESIÓN DEL BOTÓN CON CONFIRMACIÓN AUDITIVA
// CONFIGURACIÓN PULL-UP: Estado de reposo = HIGH, presionado = LOW
void WaitBoton() {
  // Esperar a que el botón NO esté presionado inicialmente (estado estable)
  while (digitalRead(PINBOTON) == LOW) {
    delay(10);  // Si está presionado al inicio, esperar que se suelte
  }
 
  // Ahora esperar a que se presione el botón
  while (digitalRead(PINBOTON) == HIGH) {
    delay(10);  // Bucle hasta que botón se presione (cambie a LOW)
  }
 
  // Debounce: esperar un momento para estabilizar la lectura
  delay(50);
 
  // Confirmar que sigue presionado (evitar falsos positivos por ruido)
  if (digitalRead(PINBOTON) == LOW) {
    // Esperar que se suelte el botón para evitar múltiples activaciones
    while (digitalRead(PINBOTON) == LOW) {
      delay(10);
    }
   
    delay(50);  // Debounce adicional al soltar
    tone(PINBUZZER, 2000, 100);  // Beep de confirmación
  }
}

Comentarios

Publicar un comentario

Entradas populares de este blog

CLASIFICADOR DE COLORES CON SENSOR TCS3472 (I2C) CON ARDUINO NANO

Imagen
  #include <Wire.h>     // libreria para bus I2C #include <Adafruit_GFX.h>   // libreria para pantallas graficas #include <Adafruit_SSD1306.h>   // libreria para controlador SSD1306 #include "Adafruit_TCS34725.h" Adafruit_TCS34725 tcs = Adafruit_TCS34725 ( TCS34725_INTEGRATIONTIME_300MS, TCS34725_GAIN_1X ) ; //614   #define ANCHO 128     // reemplaza ocurrencia de ANCHO por 128 #define ALTO 32       // reemplaza ocurrencia de ALTO por 64 #define OLED_RESET 4      // necesario por la libreria pero no usado Adafruit_SSD1306 oled ( ANCHO, ALTO, & Wire , OLED_RESET ) ;  // crea objeto #include <Servo.h> Servo servotop; Servo servorampa;   // Para guardar las frecuencias de los fotodiodos int test = 80 ; //Posicion en grados del servo superior para hacer test de color (110, 80) int recoge = 145 ; //Posicion en grados del servo superior para recoger pelota (160, 1...
Leer más

PROTOTIPO - CONTROL DE ACCESO CON RFID + TECLADO CON ALMACENAMIENTO EN MICRO SD

Imagen
  CODIGO PARA PLACA ARDUINO NANO #include <SPI.h>       // incluye libreria bus SPI #include <MFRC522.h>       // incluye libreria especifica para MFRC522 #include <SD.h> // for the SD card #include <RTClib.h> // for the RTC #include <LiquidCrystal_I2C.h>   //Downlaod the library: http://www.electronoobs.com/eng_arduino_liq_crystal.php LiquidCrystal_I2C lcd ( 0x 27 , 16 , 2 ) ;   #define RST_PIN   9      // constante para referenciar pin de reset #define SS_PIN   10      // constante para referenciar pin de slave select MFRC522 mfrc522 ( SS_PIN, RST_PIN ) ; // crea objeto mfrc522 enviando pines de slave select y reset #define CS_SD 4 #define buzzer 3 #define ledrojo A0 #define ledverde A2 #define ledazul A3 #define rele 5 #define boton 6 File myFile; RTC_DS3231 rtc;     // crea objeto del tipo RTC_DS3231 const PROGMEM byte Usuario...
Leer más

CONTROL DE ACCESO CON RFID YMODULO RTC DS3231

Imagen
  CONTROL DE ACCESO CON RFID YMODULO RTC DS3231 LIBRERIAS https://github.com/adafruit/RTClib https://github.com/miguelbalboa/rfid CODIGO PROGRAMACION INICIAL MODULO RTC DS3231 Este código se debe cargar dos (2) veces a la placa Arduino, la primera carga se hace con el código exactamente como se encuentra a continuación, luego se elimina o inhabilita la siguiente linea: rtc . adjust ( DateTime ( __DATE__, __TIME__ )) ; #include <Wire.h>   // incluye libreria para interfaz I2C #include <RTClib.h>   // incluye libreria para el manejo del modulo RTC RTC_DS3231 rtc;     // crea objeto del tipo RTC_DS3231 void setup () {   Serial . begin ( 9600 ) ;    // inicializa comunicacion serie a 9600 bps   if ( ! rtc . begin ()) {       // si falla la inicializacion del modulo   Serial . println ( "Modulo RTC no encontrado !" ) ;  // muestra mensaje de error   while ( 1 ) ;       ...
Leer más