Line follower

#define leftMotorPin 3;
#define rightMotorPin 5;
#define leftIRPin A0;
#define centerIRPin A1;
#define rightIRPin A2;

#define setpoint 512; // analogRead returns 0-1023

// PID
#define Kp 0.5;
#define Ki 0.0;
#define Kd 0.0;

double input, output;
double previous_leftError = 0;
double previous_centerError = 0;
double previous_rightError = 0;
double integral = 0;

#define sampleTime 100; // in milliseconds
#define maxOutput 255;

unsigned long previousMillis = 0;

void setup() {
  pinMode(leftMotorPin, OUTPUT);
  pinMode(rightMotorPin, OUTPUT);

  pinMode(leftIRPin, INPUT);
  pinMode(centerIRPin, INPUT);
  pinMode(rightIRPin, INPUT);
}

void loop() {
  unsigned long currentMillis = millis();
  if (currentMillis - previousMillis >= sampleTime) {
    int leftIR = analogRead(leftIRPin);
    int centerIR = analogRead(centerIRPin);
    int rightIR = analogRead(rightIRPin);

    int leftError = leftIR - setpoint;
    int centerError = centerIR - setpoint;
    int rightError = rightIR - setpoint;

    double dt = (currentMillis - previousMillis) / 1000.0; // Convert milliseconds to seconds
    double leftDerivative = (leftError - previous_leftError) / dt;
    double centerDerivative = (centerError - previous_centerError) / dt;
    double rightDerivative = (rightError - previous_rightError) / dt;

    output = Kp * (leftError + centerError + rightError) + Ki * integral + Kd * (leftDerivative + centerDerivative + rightDerivative);

    // Limit the output to the maximum value
    output = constrain(output, -maxOutput, maxOutput);

    integral += (leftError + centerError + rightError);

    previous_leftError = leftError;
    previous_centerError = centerError;
    previous_rightError = rightError;

    previousMillis = currentMillis;

    int leftSpeed = constrain(255 - output, -255, 255);
    int rightSpeed = constrain(255 + output, -255, 255);

    // Update motor speeds
    analogWrite(leftMotorPin, leftSpeed);
    analogWrite(rightMotorPin, rightSpeed);
 }
}