What is wrong with my line follower bot's code? (Except the PID constants and speed, that I can change later on) The bot's wheels are not working properly and moving in random directions. Components- Arduino Nano, L298N motor driver, N20 motors, IR array, IC7805

[u/Unique-Performance13]

#define IR0 A0
#define IR1 A1
#define IR2 A2
#define IR3 A3
#define IR4 A4
#define IR5 A5
#define IR6 2
#define IR7 4


// Motor pins (L298N)
const int ENA = 9;   // left motor PWM
const int IN1 = 7;
const int IN2 = 8;


const int ENB = 10;  // right motor PWM
const int IN3 = 11;
const int IN4 = 12;


struct Paths 
{
  bool L;
  bool S;
  bool R;
};


int ir[8];
int weights[8] = {-3,-2,-1,0,0,1,2,3};


float Kp = 30;
float Kd = 8;


int last_error = 0;
int baseSpeed = 80;


void setup() 
{
  Serial.begin(115200);
  Serial.println();
  pinMode(IR0, INPUT);
  pinMode(IR1, INPUT);
  pinMode(IR2, INPUT);
  pinMode(IR3, INPUT);
  pinMode(IR4, INPUT);
  pinMode(IR5, INPUT);
  pinMode(IR6, INPUT);
  pinMode(IR7, INPUT);


  pinMode(ENA, OUTPUT);
  pinMode(ENB, OUTPUT);
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
}


char path[100];
int i = 0;
char shorten(char a, char b, char c)
{
  if (a == 'L' && b == 'B' && c == 'L')
    return 'S';
  if (a == 'R' && b == 'B' && c == 'R')
    return 'S';
  if (a == 'S' && b == 'B' && c == 'S')
    return 'B';
  if (a == 'L' && b == 'B' && c == 'R')
    return 'B';
  if (a == 'R' && b == 'B' && c == 'L')
    return 'B';
  if (a == 'L' && b == 'B' && c == 'S')
    return 'R';
  if (a == 'S' && b == 'B' && c == 'L')
    return 'R';
  if (a == 'R' && b == 'B' && c == 'S')
    return 'L';
  if (a == 'S' && b == 'B' && c == 'R')
    return 'L';
  return 'B';
}


void storePath(char move)
{
  path[i] = move;
  i++;
  while (i >= 3 && path[i-2] == 'B')
  {
    path[i-3] = shorten(path[i-3], path[i-2], path[i-1]);
    i = i - 2;
  }
}


void loop() 
{
  if (finish())
  {
    setMotor(0,0);
    while(1);
  }
  readSensors();
  for (int j=0; j<8; j++)
  {
    Serial.print(ir[j]);
    Serial.print(" ");
  }


  if (lineLost()) 
  {
    searchLine();
  }
  else if (isNode()) 
  {
    Paths p = detectPaths();
    char move = decideMove(p);
    storePath(move);
    executeTurn(move);
    while (isNode()) 
    {
        readSensors();
        setMotor(baseSpeed, baseSpeed);
  }
  }
  else 
  {
    followLinePID();
  }
} 
bool finish()
{
  if (allHigh())
  {
    setMotor(baseSpeed,baseSpeed);
    delay(400);
    readSensors();
    if (allHigh())
    {
      return true;
    }
  }
  return false;
}
bool allHigh()
{
  int count = 0;
  for (int k=0; k<8; k++)
  {
    count += ir[k];
  }
  return count == 8;
}
void readSensors() {
  ir[0] = !digitalRead(IR0);
  ir[1] = !digitalRead(IR1);
  ir[2] = !digitalRead(IR2);
  ir[3] = !digitalRead(IR3);
  ir[4] = !digitalRead(IR4);
  ir[5] = !digitalRead(IR5);
  ir[6] = !digitalRead(IR6);
  ir[7] = !digitalRead(IR7);
}


int getPosition() {
  int numerator = 0;
  int denominator = 0;
  for (int i = 0; i < 8; i++) {
    if (ir[i]) 
    {
      numerator += weights[i];
      denominator++;
    }
  }
  if (denominator == 0) return 0;
  return numerator / denominator;
}


void followLinePID() {
  int position = getPosition();
  int correction = computePID(position);
  int leftSpeed  = baseSpeed + correction;
  int rightSpeed = baseSpeed - correction;
  setMotor(leftSpeed, rightSpeed);
}


int computePID(int position) {
  int error = position;
  int derivative = error - last_error;
  int correction = (Kp * error) + (Kd * derivative);
  last_error = error;
  return correction;
}


Paths detectPaths() {
  Paths p;
  p.L = ir[0] || ir[1] || ir[2];
  p.S = ir[3] || ir[4];
  p.R = ir[5] || ir[6] || ir[7];
  return p;
}


bool isNode() 
{
  bool centerActive = ir[3] || ir[4];
  bool leftActive   = ir[0] || ir[1] || ir[2];
  bool rightActive  = ir[5] || ir[6] || ir[7];
  return centerActive && (leftActive || rightActive);
}


char decideMove(Paths p) 
{
  if (p.L) return 'L';
  if (p.S) return 'S';
  if (p.R) return 'R';
  return 'B';
}


void executeTurn(char move) {
  if (move == 'L') {
    turnLeft90();
  } 
  else if (move == 'R') {
    turnRight90();
  } 
  else if (move == 'B') {
    uTurn();
  } 
  else {
    setMotor(baseSpeed, baseSpeed);
    delay(50);
  }
}


bool lineLost() {
  for (int i = 0; i < 8; i++) {
    if (ir[i]) return false;
  }
  return true;
}


void searchLine() 
{
  setMotor(120, -120);
}


void turnLeft90() 
{
  setMotor(-150, 150);
  delay(200);
  setMotor(0, 0);
}


void turnRight90() {
  setMotor(150, -150);
  delay(200);
  setMotor(0, 0);
}


void uTurn() {
  setMotor(150, -150);
  delay(400);
  setMotor(0, 0);
}


void setMotor(int leftSpeed, int rightSpeed) 
{
  leftSpeed = constrain(leftSpeed, -255, 255);
  rightSpeed = constrain(rightSpeed, -255, 255);


  if (leftSpeed >= 0) 
  {
    digitalWrite(IN1, HIGH);
    digitalWrite(IN2, LOW);
    analogWrite(ENA, leftSpeed);
  } else 
  {
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, HIGH);
    analogWrite(ENA, -leftSpeed);
  }


  if (rightSpeed >= 0) 
  {
    digitalWrite(IN3, HIGH);
    digitalWrite(IN4, LOW);
    analogWrite(ENB, rightSpeed);
  } else 
  {
    digitalWrite(IN3, LOW);
    digitalWrite(IN4, HIGH);
    analogWrite(ENB, -rightSpeed);
  }
}