#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <PID_v1.h>
// IR sensor pins
const int irPins[16] = {A0, A1, A2, A3, A4, A5, A6, A7, B0, B1, B3, B4, B5, B6, B7, B8};

// Motor driver pins
const int IN1 = PA8;
const int IN2 = PA9;
const int IN3 = PA10;
const int IN4 = PA11;
const int ENA = PA12;
const int ENB = PA15;

// PID control variables
double setpoint = 0, input = 0, output = 0;
double Kp = 1, Ki = 0, Kd = 0;
PID myPID(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);

// LCD display
LiquidCrystal_I2C lcd(0x27, 16, 2);
void setup() {
  Serial.begin(9600);

  // Initialize LCD
  lcd.begin();
  lcd.backlight();

  // Set motor pins as output
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  pinMode(ENA, OUTPUT);
  pinMode(ENB, OUTPUT);

  // Initialize PID
  myPID.SetMode(AUTOMATIC);
  myPID.SetOutputLimits(-255, 255); // Output range for PWM signal
}
void loop() {
  int error = 0;
  int sensorsValue = 0;
  for (int i = 0; i < 16; i++) {
    sensorsValue = analogRead(irPins[i]);
    if (sensorsValue > 500) { // Assume 500 is the threshold for line detection
      error += (i - 7); // Compute the error based on sensor position
    }
  }

  input = error;
  setpoint = 0; // Desired position (center)
  myPID.Compute();

  // Display PID values on LCD
  lcd.setCursor(0, 0);
  lcd.print("PID: ");
  lcd.print(output);
  lcd.setCursor(0, 1);
  lcd.print("Err: ");
  lcd.print(error);

  // Motor control based on PID output
  if (output > 0) {
    analogWrite(ENA, output);
    analogWrite(ENB, output);
    digitalWrite(IN1, HIGH);
    digitalWrite(IN2, LOW);
    digitalWrite(IN3, LOW);
    digitalWrite(IN4, HIGH);
  } else {
    analogWrite(ENA, -output);
    analogWrite(ENB, -output);
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, HIGH);
    digitalWrite(IN3, HIGH);
    digitalWrite(IN4, LOW);
 }
}