#include <QTRSensors.
h>
// Definir pines para el controlador de motores TB6612FNG
#define motorAInput1 7
#define motorAInput2 8
#define motorBInput1 5
#define motorBInput2 4
#define motorAPWM 9
#define motorBPWM 3
#define STBY 6 // Pin STBY del TB6612FNG
// Definir pines para el sensor QTR-8A
#define NUM_SENSORS 8
#define NUM_SAMPLES_PER_SENSOR 4
#define EMITTER_PIN 10
// Definir pines para los botones
#define CALIBRATION_BUTTON 12
#define START_BUTTON 2
// Constantes del control PID
#define KP 4.6// Constante proporcional
#define KD 19.4 // Constante derivativa
#define KI 0.009 // Constante integral
QTRSensors qtr;
unsigned int sensorValues[NUM_SENSORS];
int lastError = 0;
int integral = 0;
void setup() {
// Establecer pines de salida para el driver TB6612FNG
pinMode(motorAInput1, OUTPUT);
pinMode(motorAInput2, OUTPUT);
pinMode(motorBInput1, OUTPUT);
pinMode(motorBInput2, OUTPUT);
pinMode(motorAPWM, OUTPUT);
pinMode(motorBPWM, OUTPUT);
pinMode(STBY, OUTPUT);
// Establecer pines de entrada para los botones
pinMode(CALIBRATION_BUTTON, INPUT);
pinMode(START_BUTTON, INPUT);
digitalWrite(STBY, HIGH);
// Inicializa el sensor QTR-8A
[Link]();
[Link]((const uint8_t[]){A0, A1, A2, A3, A4, A5, A6, A7},
NUM_SENSORS);
[Link](EMITTER_PIN);
digitalWrite(LED_BUILTIN, HIGH); // enciende LED de Arduino para indicar el
inicio de la calibracion
[Link](9600);// inicio de comunicacion serial
// Esperar a que se presione el bot�n de calibraci�n
while (digitalRead(CALIBRATION_BUTTON) == LOW) {
delay(10);
}
// Calibrar el sensor
for (int i = 0; i < 250; i++) {
[Link]();
delay(10);
}
digitalWrite(LED_BUILTIN, LOW); //apaga el led Arduino para indicar termino de
calibracion
// Esperar a que se presione el bot�n de inicio
while (digitalRead(START_BUTTON) == LOW) {
delay(10);
}
}
void loop() {
// Leer valores del sensor
uint16_t position = [Link](sensorValues);
// Calcular el error (posici�n central ponderada)
int weightedSum = 0;
int sum = 0;
for (int i = 0; i < NUM_SENSORS; i++) {
weightedSum += (i * sensorValues[i]);
sum += sensorValues[i];
}
int error = position - (NUM_SENSORS - 1) * 1000 / 2;
// Calcular componentes de control PID
int proportional = KP * error;
int derivative = KD * (error - lastError);
integral += error;
int integralComponent = KI * integral;
// Calcular se�al de control
int controlSignal = proportional + derivative + integralComponent;
// Controlar motores usando la se�al de control
int motorSpeed = 255; //Ajuste este valor para controlar la velocidad de
avance//130
// Control de motores usando la se�al de control
int motorSpeedA = motorSpeed - controlSignal;
int motorSpeedB = motorSpeed + controlSignal;
// Limit the motor speed values to the range of 0-255
motorSpeedA = constrain(motorSpeedA, 0, 255);
motorSpeedB = constrain(motorSpeedB, 0, 255);
digitalWrite(motorBInput1, LOW);
digitalWrite(motorBInput2, HIGH);
digitalWrite(motorAInput1, LOW);//LOW para avanzar hacia adelante
digitalWrite(motorAInput2, HIGH);
analogWrite(motorAPWM, motorSpeedA);
analogWrite(motorBPWM, motorSpeedB);
// Update last error
lastError = error;
// Print sensor values and position to the serial monitor
for (int i = 0; i < NUM_SENSORS; i++) {
[Link](sensorValues[i]);
[Link]('\t');
}
[Link](controlSignal);
