روبوت تتبع الخط

const int S[5] = {A0, A1, A2, A3, A4};
const int weight[5] = {-2, -1, 0, 1, 2};

#define IN1 4
#define IN2 5
#define IN3 6
#define IN4 7
#define ENA 9
#define ENB 10

float Kp = 28.0, Ki = 0.0, Kd = 14.0;
float lastError = 0, integral = 0;
int baseSpeed = 150;

float readError() {
  long sum = 0, total = 0;
  for (int i = 0; i < 5; i++) {
    int v = analogRead(S[i]) > 500 ? 1 : 0; // 1 = black line
    sum += (long)v * weight[i];
    total += v;
  }
  if (total == 0) return lastError; // line lost -> keep last direction
  return (float)sum / total;
}

void setMotors(int left, int right) {
  left  = constrain(left,  -255, 255);
  right = constrain(right, -255, 255);
  digitalWrite(IN1, left  >= 0); digitalWrite(IN2, left  < 0);
  digitalWrite(IN3, right >= 0); digitalWrite(IN4, right < 0);
  analogWrite(ENA, abs(left));
  analogWrite(ENB, abs(right));
}

void setup() {
  for (int i = 0; i < 5; i++) pinMode(S[i], INPUT);
  pinMode(IN1,OUTPUT); pinMode(IN2,OUTPUT);
  pinMode(IN3,OUTPUT); pinMode(IN4,OUTPUT);
  pinMode(ENA,OUTPUT); pinMode(ENB,OUTPUT);
}

void loop() {
  float error = readError();
  integral += error;
  float derivative = error - lastError;
  float correction = Kp * error + Ki * integral + Kd * derivative;
  lastError = error;

  setMotors(baseSpeed + correction, baseSpeed - correction);
  delay(5);
}