Revisión de Código de Controlador de Motores
vez algun fallo en el programa ?? const int NUM_SENSORS = 8;
int sensorPins[NUM_SENSORS] = {A0, A1, A2, A3, A4, A5, A6, A7};
// Pines de control para el driver TB6612FNG
// Motor A
const int AIN1 = 3;
const int AIN2 = 4;
const int PWMA = 5;
// Motor B
const int BIN1 = 6;
const int BIN2 = 7;
const int PWMB = 9;
const int baseSpeed = 150; // Velocidad base (0 - 255)
const int maxSpeed = 255; // Velocidad máxima
// Inicializa el driver deteniendo ambos motores
void setupMotors() {
pinMode(AIN1, OUTPUT);
pinMode(AIN2, OUTPUT);
pinMode(PWMA, OUTPUT);
pinMode(BIN1, OUTPUT);
pinMode(BIN2, OUTPUT);
pinMode(PWMB, OUTPUT);
digitalWrite(AIN1, LOW);
digitalWrite(AIN2, LOW);
analogWrite(PWMA, 0);
digitalWrite(BIN1, LOW);
digitalWrite(BIN2, LOW);
analogWrite(PWMB, 0);
}
// Control del Motor A
void setMotorA(int speed) {
if (speed > 0) {
digitalWrite(AIN1, HIGH);
digitalWrite(AIN2, LOW);
} else if (speed 0) {
digitalWrite(BIN1, HIGH);
digitalWrite(BIN2, LOW);
} else if (speed < 0) {
digitalWrite(BIN1, LOW);
digitalWrite(BIN2, HIGH);
speed = -speed;
} else {
digitalWrite(BIN1, LOW);
digitalWrite(BIN2, LOW);
}
analogWrite(PWMB, constrain(speed, 0, maxSpeed));
}
// Función que lee los 8 sensores y calcula una posición ponderada
int readLinePosition() {
long sumValues = 0;
long weightedSum = 0;
for (int i = 0; i < NUM_SENSORS; i++) {
int sensorValue = analogRead(sensorPins[i]);
sumValues += sensorValue;
// Se asume que cada sensor representa una posición escalada (0, 1000, 2000, ... 7000)
weightedSum += (long)sensorValue * i * 1000;
}
if (sumValues == 0) {
return -1; // Línea no detectada
}
int position = weightedSum / sumValues;
return position;
}
void setup() {
Serial.begin(115200);
// Configurar pines de sensores
for (int i = 0; i < NUM_SENSORS; i++) {
pinMode(sensorPins[i], INPUT);
}
setupMotors();
Serial.println("Inicio de seguidor de línea");
}
void loop() {
int pos = readLinePosition();
if (pos == -1) {
// Si no se detecta la línea, detenerse.
setMotorA(0);
setMotorB(0);
Serial.println("Línea no detectada");
delay(100);
return;
}
// El punto central ideal de la línea es a la mitad: ((NUM_SENSORS-1)*1000)/2
int center = ((NUM_SENSORS - 1) * 1000) / 2; // en este caso, 3500
int error = pos - center;
// Control proporcional simple
float Kp = 0.2;
int correction = Kp * error;
int motorSpeedA = baseSpeed + correction;
int motorSpeedB = baseSpeed - correction;
// Asegurar la velocidad dentro de los límites
motorSpeedA = constrain(motorSpeedA, 0, maxSpeed);
motorSpeedB = constrain(motorSpeedB, 0, maxSpeed);
setMotorA(motorSpeedA);
setMotorB(motorSpeedB);
Serial.print("Posicion: ");
Serial.print(pos);
Serial.print(" | Error: ");
Serial.print(error);
Serial.print(" | SpeedA: ");
Serial.print(motorSpeedA);
Serial.print(" | SpeedB: ");
Serial.println(motorSpeedB);
delay(10);
}
Thought for 21 sec
El código parece funcionar correctamente sin errores evidentes. Algunos puntos a considerar o mejorar podrían ser:
-
Rango de valores de sensores:
Se asume que cada sensor aporta un valor proporcional a su posición (0, 1000, 2000, …). Asegúrate de que esto se corresponda con la salida real de tus sensores, ya que los valores deanalogReadvarían entre 0 y 1023 en la mayoría de las placas Arduino. -
Manejo de valores negativos en la función de control de motores:
El código toma el valor absoluto si el error es negativo y establece las señales de control correspondientes para invertir la dirección, lo cual es correcto. Solo verifica que la lógica de inversión (cambiar AIN1 y AIN2, y lo mismo para BIN1 y BIN2) se ajuste al comportamiento esperado del driver TB6612FNG. -
Constantes y reputación de nombres:
- Es buena práctica declarar constantes como
const float Kpo incluso definir otros parámetros de control (si planeas experimentar con ellos). - Puedes agregar comentarios adicionales para clarificar los cálculos de la posición y el error, especialmente si otros van a revisar el código.
- Es buena práctica declarar constantes como
En resumen, no se observa ningún fallo lógico o sintáctico en el programa. ¡Puedes probarlo en tu hardware y ajustar los parámetros según sea necesario!
Additional code review
Hardware setup confirmation
PID control optimization
No schematic yet
Talk to Flux to get started.
