Problem with pixy and arduino line following 4wd robot

[coolpixle]

Hi everyone,

I'm trying to get my Arduino and Pixy2.1 to follow a black line using four motors (H-bridge setup). However, my robot doesn't seem to follow the line correctly, and I need help troubleshooting or improving my code.

Setup:

Hardware:
Arduino Mega
Pixy2.1 (Line Tracking Mode)
4 DC motors
Motor driver (L298N)
Software:
Reading the X-coordinate of the detected line and adjusting motor speeds accordingly

Current Behavior:

The Pixy detects the line, and I can see the X-coordinate changing in the Serial Monitor.
However, the robot doesn’t respond correctly to left and right turns.
I think there might be an issue with my motor control logic or threshold values.
I did use ChatGPT and the robot is just not turning its going straight but not turning when it needs to
Code Snippet:

#include <Pixy2.h>

Pixy2 pixy;

int motor1_pin1 = 51;
int motor1_pin2 = 49;
int motor2_pin1 = 47;
int motor2_pin2 = 45;
int motor3_pin1 = 39;
int motor3_pin2 = 37;
int motor4_pin1 = 35;
int motor4_pin2 = 33;

int ENA = 53;
int ENB = 43;
int ENA2 = 41;
int ENB2 = 31;

int forwardSpeed = 150;
int leftSpeed = 255;
int rightSpeed = 255;

// Define configurable thresholds for the line-following logic
int leftThreshold = 50;   // If the line is to the left of this position, turn right
int rightThreshold = 150; // If the line is to the right of this position, turn left
int centerThreshold = 100; // Line center position for moving forward

void setup() {
  // Initialize Serial for debugging
  Serial.begin(9600);

  // Initialize Pixy2
  pixy.init();
  pixy.setLamp(1, 1); // Turn on Pixy2's built-in LEDs

  // Set motor control pins as outputs
  pinMode(motor1_pin1, OUTPUT);
  pinMode(motor1_pin2, OUTPUT);
  pinMode(motor2_pin1, OUTPUT);
  pinMode(motor2_pin2, OUTPUT);
  pinMode(motor3_pin1, OUTPUT);
  pinMode(motor3_pin2, OUTPUT);
  pinMode(motor4_pin1, OUTPUT);
  pinMode(motor4_pin2, OUTPUT);
  pinMode(ENA, OUTPUT);
  pinMode(ENB, OUTPUT);
  pinMode(ENA2, OUTPUT);
  pinMode(ENB2, OUTPUT);

  // Set initial motor speeds (initially full speed for all directions)
  analogWrite(ENA, forwardSpeed);   
  analogWrite(ENB, forwardSpeed);   
  analogWrite(ENA2, forwardSpeed);  
  analogWrite(ENB2, forwardSpeed);  
}

void loop() {
  // Get line features from Pixy2
  pixy.line.getMainFeatures();
  if (pixy.line.numVectors) {
    int x = pixy.line.vectors[0].m_x0; // Starting X position of the line
    int x_end = pixy.line.vectors[0].m_x1; // Ending X position of the line

    // Print the line positions for debugging
    Serial.print("Line Start X Position: ");
    Serial.print(x);
    Serial.print(" | Line End X Position: ");
    Serial.println(x_end);

    // Adjusted Thresholds for Line Following
    if (x < leftThreshold) {
      Right();   // Line is to the far left, set speed for turning right
      Serial.println("right");
    }
    else if (x > rightThreshold) {
      Left();  // Line is to the far right, set speed for turning left
      Serial.println("left");
    }
    else {
      Forward(); // Line is near the center (Straight), set speed for moving forward
      Serial.println("forward");
    }
  } 
}

void Forward() {
  // Move forward with all 4 motors at the defined forward speed
  analogWrite(ENA, forwardSpeed);
  analogWrite(ENB, forwardSpeed);
  analogWrite(ENA2, forwardSpeed);
  analogWrite(ENB2, forwardSpeed);

  digitalWrite(motor1_pin1, HIGH);
  digitalWrite(motor1_pin2, LOW);
  digitalWrite(motor2_pin1, HIGH);
  digitalWrite(motor2_pin2, LOW);
  digitalWrite(motor3_pin1, HIGH);
  digitalWrite(motor3_pin2, LOW);
  digitalWrite(motor4_pin1, HIGH);
  digitalWrite(motor4_pin2, LOW);
}

void Right() {
  // Rotate left with specified leftSpeed
  analogWrite(ENA, leftSpeed);
  analogWrite(ENB, leftSpeed);
  analogWrite(ENA2, leftSpeed);
  analogWrite(ENB2, leftSpeed);

  digitalWrite(motor1_pin1, LOW);
  digitalWrite(motor1_pin2, LOW);  // Motor 1 stopped
  digitalWrite(motor2_pin1, LOW);
  digitalWrite(motor2_pin2, LOW);  // Motor 2 stopped
  digitalWrite(motor3_pin1, HIGH);
  digitalWrite(motor3_pin2, LOW);  // Motor 3 forward
  digitalWrite(motor4_pin1, LOW);
  digitalWrite(motor4_pin2, LOW);  // Motor 4 stopped
}

void Left() {
  // Rotate right with specified rightSpeed
  analogWrite(ENA, rightSpeed);
  analogWrite(ENB, rightSpeed);
  analogWrite(ENA2, rightSpeed);
  analogWrite(ENB2, rightSpeed);

  digitalWrite(motor1_pin1, LOW);
  digitalWrite(motor1_pin2, LOW);  // Motor 1 stopped

  digitalWrite(motor2_pin1, LOW);
  digitalWrite(motor2_pin2, LOW);  // Motor 2 stopped

  digitalWrite(motor3_pin1, LOW);
  digitalWrite(motor3_pin2, LOW);  // Motor 3 stopped

  digitalWrite(motor4_pin1, HIGH);
  digitalWrite(motor4_pin2, LOW);  // Motor 4 forward
}