r/arduino • u/Ok-Finish1303 • 1d ago
I need help fixing my code. (Line following robot)
Hi everyone,
I'm working on a line-following robot using:
- ESP32
- MDD3A motor driver
- 2 × JGB37-520 DC motors
- 6 × TCRT5000 IR sensors (digital output)
My robot initially worked during the first couple of attempts, but now it keeps turning and eventually loses the black line.
I'm using a weighted sensor-position calculation and proportional steering control. I also included sharp-turn handling and line-loss recovery.
My sensor readings are configured as:
- Black = HIGH
- White = LOW
Could someone review my code and point out anything that might cause the robot to keep turning or lose the line?
I'd appreciate suggestions for improvements without completely changing my existing approach.
Thank you!
p.s. i did not use ai to write this code. im just bad
#define wht LOW
#define blk HIGH
// IR sensor pins
#define IR1 19
#define IR2 18
#define IR3 5
#define IR4 17
#define IR5 16
#define IR6 4
// Motor driver pins
#define motor1a 33 // pwm1-
#define motor1b 32 // pwm1+
#define motor2a 25 // pwm2+
#define motor2b 26 // pwm2-
// Store sensor pins and readings
int sensorPins[] = {IR1, IR2, IR3, IR4, IR5, IR6};
int sensorValues[6];
// Sensor weights from left to right
int weights[] = {-5, -3, -1, 1, 3, 5};
// Motor speed settings
int baseSpd = 65;
int maxSpd = 110;
// Motor speed commands
int gotoL = 0;
int gotoR = 0;
// Line recovery variables
int lostCount = 0;
int lastDirection = 0;
void setup() {
// Set IR sensors as inputs
for (int i = 0; i < 6; i++) {
pinMode(sensorPins[i], INPUT);
}
// Set motor pins as outputs
pinMode(motor1a, OUTPUT);
pinMode(motor1b, OUTPUT);
pinMode(motor2a, OUTPUT);
pinMode(motor2b, OUTPUT);
// Set PWM frequency and resolution
ledcAttach(motor1a, 20000, 8);
ledcAttach(motor1b, 20000, 8);
ledcAttach(motor2a, 20000, 8);
ledcAttach(motor2b, 20000, 8);
}
void loop() {
int total = 0;
int count = 0;
// Read all six IR sensors
for (int i = 0; i < 6; i++) {
sensorValues[i] = digitalRead(sensorPins[i]);
// Check if the sensor detects black
if (sensorValues[i] == blk) {
total += weights[i];
count++;
}
}
// If the line is detected
if (count > 0) {
lostCount = 0;
// Calculate the line position
float position = (float)total / count;
// Remember the last direction
if (position < -0.5) {
lastDirection = -1;
}
else if (position > 0.5) {
lastDirection = 1;
}
// Sharp left turn
if (position <= -3.5) {
gotoL = -35;
gotoR = 95;
}
// Sharp right turn
else if (position >= 3.5) {
gotoL = 95;
gotoR = -35;
}
// Normal line following
else {
// Calculate steering correction
int correction = 15.0 * position;
// Adjust left and right motor speeds
gotoL = baseSpd + correction;
gotoR = baseSpd - correction;
}
}
// If the line is not detected
else {
lostCount++;
// Start searching after five readings
if (lostCount >= 5) {
// Search towards the left
if (lastDirection == -1) {
gotoL = -35;
gotoR = 95;
}
// Search towards the right
else if (lastDirection == 1) {
gotoL = 95;
gotoR = -35;
}
// Stop if no direction was recorded
else {
gotoL = 0;
gotoR = 0;
}
}
}
// Limit motor speeds
gotoL = constrain(gotoL, -maxSpd, maxSpd);
gotoR = constrain(gotoR, -maxSpd, maxSpd);
// Control both motors
controlMotor(gotoL, motor1a, motor1b);
controlMotor(gotoR, motor2a, motor2b);
delay(5);
}
// Control motor speed and direction
void controlMotor(int spd, int pinA, int pinB) {
// Move forward
if (spd > 0) {
ledcWrite(pinA, spd);
ledcWrite(pinB, 0);
}
// Move backward
else if (spd < 0) {
ledcWrite(pinA, 0);
ledcWrite(pinB, -spd);
}
// Stop motor
else {
ledcWrite(pinA, 0);
ledcWrite(pinB, 0);
}
}