the line-following car prefers the right
When all 8 sensors are black, it always goes to the right.
10 Replies
fails in that part
This is the code
// Definiciones de pines
#define s0 A0
#define s1 A1
#define s2 A2
#define s3 A3
#define s4 A4
#define s5 A5
#define s6 A6
#define s7 A7
#define LED_ON 11
#define LED 13
#define BOTON 12
#define GO 2
#define RDY 4
#define motorAInput1 7 // Motor Derecho
#define motorAInput2 8 // Motor Derecho
#define motorBInput1 5 // Motor Izquierdo
#define motorBInput2 4 // Motor Izquierdo
#define motorAPWM 9 // PWM Motor Derecho
#define motorBPWM 3 // PWM Motor Izquierdo
// Variables de sensores y calibración
int sensores[8];
int lectura_fondo[8];
int lectura_linea[8];
int umbral[8];
// Configuración de la línea (0 = Negra, 1 = Blanca)
int linea = 0;
// Parámetros de control
float Kp = 0.32;
float Kd = 2.5;
float Ki = 0.003;
int vel = 110;
int veladelante = 200;
int velatras = 200;
int sensibilidad = 100;
// Variables para el cálculo PID
double ERROR_POSICION = 0;
double ERROR_ULTIMO = 0;
double ERROR_I = 0;
// Prototipos de funciones
void fondos();
void lineas();
void promedio();
void lectura();
bool frenos();
void PID();
void driveMotors(int motorSpeedB, int motorSpeedA);
void setup() {
Serial.begin(115200);
TCCR1B = TCCR1B & B11111000 | B00000010;
TCCR2B = TCCR2B & B11111000 | B00000010;
pinMode(motorAInput1, OUTPUT);
pinMode(motorAInput2, OUTPUT);
pinMode(motorBInput1, OUTPUT);
pinMode(motorBInput2, OUTPUT);
pinMode(motorAPWM, OUTPUT);
pinMode(motorBPWM, OUTPUT);
pinMode(LED, OUTPUT);
pinMode(LED_ON, OUTPUT);
pinMode(BOTON, INPUT);
pinMode(GO, INPUT);
pinMode(RDY, INPUT);
digitalWrite(LED_ON, HIGH);
digitalWrite(LED, HIGH);
delay(500);
// Definiciones de pines
#define s0 A0
#define s1 A1
#define s2 A2
#define s3 A3
#define s4 A4
#define s5 A5
#define s6 A6
#define s7 A7
#define LED_ON 11
#define LED 13
#define BOTON 12
#define GO 2
#define RDY 4
#define motorAInput1 7 // Motor Derecho
#define motorAInput2 8 // Motor Derecho
#define motorBInput1 5 // Motor Izquierdo
#define motorBInput2 4 // Motor Izquierdo
#define motorAPWM 9 // PWM Motor Derecho
#define motorBPWM 3 // PWM Motor Izquierdo
// Variables de sensores y calibración
int sensores[8];
int lectura_fondo[8];
int lectura_linea[8];
int umbral[8];
// Configuración de la línea (0 = Negra, 1 = Blanca)
int linea = 0;
// Parámetros de control
float Kp = 0.32;
float Kd = 2.5;
float Ki = 0.003;
int vel = 110;
int veladelante = 200;
int velatras = 200;
int sensibilidad = 100;
// Variables para el cálculo PID
double ERROR_POSICION = 0;
double ERROR_ULTIMO = 0;
double ERROR_I = 0;
// Prototipos de funciones
void fondos();
void lineas();
void promedio();
void lectura();
bool frenos();
void PID();
void driveMotors(int motorSpeedB, int motorSpeedA);
void setup() {
Serial.begin(115200);
TCCR1B = TCCR1B & B11111000 | B00000010;
TCCR2B = TCCR2B & B11111000 | B00000010;
pinMode(motorAInput1, OUTPUT);
pinMode(motorAInput2, OUTPUT);
pinMode(motorBInput1, OUTPUT);
pinMode(motorBInput2, OUTPUT);
pinMode(motorAPWM, OUTPUT);
pinMode(motorBPWM, OUTPUT);
pinMode(LED, OUTPUT);
pinMode(LED_ON, OUTPUT);
pinMode(BOTON, INPUT);
pinMode(GO, INPUT);
pinMode(RDY, INPUT);
digitalWrite(LED_ON, HIGH);
digitalWrite(LED, HIGH);
delay(500);
// Proceso de calibración
while (true) { if (digitalRead(BOTON)) break; }
for(int i=0; i<50; i++){ fondos(); digitalWrite(LED, LOW); delay(20); digitalWrite(LED, HIGH); delay(20); }
while (true) { if (!digitalRead(BOTON)) break; }
for(int i=0; i<50; i++){ lineas(); digitalWrite(LED, LOW); delay(20); digitalWrite(LED, HIGH); delay(20); }
promedio();
while (true) { if (digitalRead(BOTON)) break; }
digitalWrite(LED, HIGH);
delay(250);
}
void loop() {
lectura();
if (!frenos()) {
PID();
}
}
void fondos(){
lectura_fondo[0] = analogRead(s0);
lectura_fondo[1] = analogRead(s1);
lectura_fondo[2] = analogRead(s2);
lectura_fondo[3] = analogRead(s3);
lectura_fondo[4] = analogRead(s4);
lectura_fondo[5] = analogRead(s5);
lectura_fondo[6] = analogRead(s6);
lectura_fondo[7] = analogRead(s7);
}
void lineas(){
lectura_linea[0] = analogRead(s0);
lectura_linea[1] = analogRead(s1);
lectura_linea[2] = analogRead(s2);
lectura_linea[3] = analogRead(s3);
lectura_linea[4] = analogRead(s4);
lectura_linea[5] = analogRead(s5);
lectura_linea[6] = analogRead(s6);
lectura_linea[7] = analogRead(s7);
}
void promedio(){
for(int i=0; i<8; i++){
umbral[i] = (lectura_fondo[i] + lectura_linea[i]) / 2;
}
}
void lectura(void){
sensores[0] = analogRead(s0);
sensores[1] = analogRead(s1);
sensores[2] = analogRead(s2);
sensores[3] = analogRead(s3);
sensores[4] = analogRead(s4);
sensores[5] = analogRead(s5);
sensores[6] = analogRead(s6);
sensores[7] = analogRead(s7);
for(int i = 0; i < 8; i++) {
if (linea == 0) {
sensores[i] = map(sensores[i], lectura_fondo[i], lectura_linea[i], 0, 255);
} else {
sensores[i] = map(sensores[i], lectura_fondo[i], lectura_linea[i], 255, 0);
}
sensores[i] = constrain(sensores[i], 0, 255);
}
}
// Proceso de calibración
while (true) { if (digitalRead(BOTON)) break; }
for(int i=0; i<50; i++){ fondos(); digitalWrite(LED, LOW); delay(20); digitalWrite(LED, HIGH); delay(20); }
while (true) { if (!digitalRead(BOTON)) break; }
for(int i=0; i<50; i++){ lineas(); digitalWrite(LED, LOW); delay(20); digitalWrite(LED, HIGH); delay(20); }
promedio();
while (true) { if (digitalRead(BOTON)) break; }
digitalWrite(LED, HIGH);
delay(250);
}
void loop() {
lectura();
if (!frenos()) {
PID();
}
}
void fondos(){
lectura_fondo[0] = analogRead(s0);
lectura_fondo[1] = analogRead(s1);
lectura_fondo[2] = analogRead(s2);
lectura_fondo[3] = analogRead(s3);
lectura_fondo[4] = analogRead(s4);
lectura_fondo[5] = analogRead(s5);
lectura_fondo[6] = analogRead(s6);
lectura_fondo[7] = analogRead(s7);
}
void lineas(){
lectura_linea[0] = analogRead(s0);
lectura_linea[1] = analogRead(s1);
lectura_linea[2] = analogRead(s2);
lectura_linea[3] = analogRead(s3);
lectura_linea[4] = analogRead(s4);
lectura_linea[5] = analogRead(s5);
lectura_linea[6] = analogRead(s6);
lectura_linea[7] = analogRead(s7);
}
void promedio(){
for(int i=0; i<8; i++){
umbral[i] = (lectura_fondo[i] + lectura_linea[i]) / 2;
}
}
void lectura(void){
sensores[0] = analogRead(s0);
sensores[1] = analogRead(s1);
sensores[2] = analogRead(s2);
sensores[3] = analogRead(s3);
sensores[4] = analogRead(s4);
sensores[5] = analogRead(s5);
sensores[6] = analogRead(s6);
sensores[7] = analogRead(s7);
for(int i = 0; i < 8; i++) {
if (linea == 0) {
sensores[i] = map(sensores[i], lectura_fondo[i], lectura_linea[i], 0, 255);
} else {
sensores[i] = map(sensores[i], lectura_fondo[i], lectura_linea[i], 255, 0);
}
sensores[i] = constrain(sensores[i], 0, 255);
}
}
void PID(){
ERROR_POSICION = sensores[0] * (-4) + sensores[1] * (-2) + sensores[2] * (-1) + sensores[3] * (-0.5) +
sensores[4] * (0.5) + sensores[5] * (1) + sensores[6] * (2) + sensores[7] * (4);
float P = Kp * ERROR_POSICION;
float D = Kd * (ERROR_POSICION - ERROR_ULTIMO);
float I = Ki * ERROR_I;
ERROR_ULTIMO = ERROR_POSICION;
double CX = P + D + I;
CX = constrain(CX, -255, 255);
int motorSpeedA = vel + CX; // Motor Derecho
int motorSpeedB = vel - CX; // Motor Izquierdo
motorSpeedA = constrain(motorSpeedA, -255, 255);
motorSpeedB = constrain(motorSpeedB, -255, 255);
driveMotors(motorSpeedB, motorSpeedA);
}
bool frenos(){
// Salida leve de pista
if ((sensores[0] > sensibilidad || sensores[1] > sensibilidad) && sensores[3] < sensibilidad && sensores[4] < sensibilidad) {
driveMotors(veladelante, -velatras);
return true;
}
if ((sensores[7] > sensibilidad || sensores[6] > sensibilidad) && sensores[3] < sensibilidad && sensores[4] < sensibilidad) {
driveMotors(-velatras, veladelante);
return true;
}
// Pérdida total de línea para curvas cerradas
int umbral_perdida = 50;
if (sensores[2] < umbral_perdida && sensores[3] < umbral_perdida && sensores[4] < umbral_perdida && sensores[5] < umbral_perdida) {
if (ERROR_ULTIMO > 0) {
driveMotors(veladelante, -velatras); // Gira a la derecha
} else {
driveMotors(-velatras, veladelante); // Gira a la izquierda
}
return true;
}
return false;
}
void driveMotors(int motorSpeedB, int motorSpeedA) {
// Motor izquierdo (B)
digitalWrite(motorBInput1, motorSpeedB < 0);
digitalWrite(motorBInput2, motorSpeedB >= 0);
analogWrite(motorBPWM, abs(motorSpeedB));
// Motor derecho (A)
digitalWrite(motorAInput1, motorSpeedA < 0);
digitalWrite(motorAInput2, motorSpeedA >= 0);
analogWrite(motorAPWM, abs(motorSpeedA));
}
void PID(){
ERROR_POSICION = sensores[0] * (-4) + sensores[1] * (-2) + sensores[2] * (-1) + sensores[3] * (-0.5) +
sensores[4] * (0.5) + sensores[5] * (1) + sensores[6] * (2) + sensores[7] * (4);
float P = Kp * ERROR_POSICION;
float D = Kd * (ERROR_POSICION - ERROR_ULTIMO);
float I = Ki * ERROR_I;
ERROR_ULTIMO = ERROR_POSICION;
double CX = P + D + I;
CX = constrain(CX, -255, 255);
int motorSpeedA = vel + CX; // Motor Derecho
int motorSpeedB = vel - CX; // Motor Izquierdo
motorSpeedA = constrain(motorSpeedA, -255, 255);
motorSpeedB = constrain(motorSpeedB, -255, 255);
driveMotors(motorSpeedB, motorSpeedA);
}
bool frenos(){
// Salida leve de pista
if ((sensores[0] > sensibilidad || sensores[1] > sensibilidad) && sensores[3] < sensibilidad && sensores[4] < sensibilidad) {
driveMotors(veladelante, -velatras);
return true;
}
if ((sensores[7] > sensibilidad || sensores[6] > sensibilidad) && sensores[3] < sensibilidad && sensores[4] < sensibilidad) {
driveMotors(-velatras, veladelante);
return true;
}
// Pérdida total de línea para curvas cerradas
int umbral_perdida = 50;
if (sensores[2] < umbral_perdida && sensores[3] < umbral_perdida && sensores[4] < umbral_perdida && sensores[5] < umbral_perdida) {
if (ERROR_ULTIMO > 0) {
driveMotors(veladelante, -velatras); // Gira a la derecha
} else {
driveMotors(-velatras, veladelante); // Gira a la izquierda
}
return true;
}
return false;
}
void driveMotors(int motorSpeedB, int motorSpeedA) {
// Motor izquierdo (B)
digitalWrite(motorBInput1, motorSpeedB < 0);
digitalWrite(motorBInput2, motorSpeedB >= 0);
analogWrite(motorBPWM, abs(motorSpeedB));
// Motor derecho (A)
digitalWrite(motorAInput1, motorSpeedA < 0);
digitalWrite(motorAInput2, motorSpeedA >= 0);
analogWrite(motorAPWM, abs(motorSpeedA));
}
Where are sensors on car. Just front?
Also what direction is it going. How do all sensors detect black at the same time ?
@GxdzYT
qtr-8a
yes
I cant see where on the track, all the sensors would be reporting a line/black
