دنبال کردن خط با UGV — ساده‌ترین پروژه خودکار با حسگر IR (فارسی)

کد PID خط‌کشی ۵ حسگر

// ۵ حسگر TCRT5000 روی A0–A4
int sensorPins[5] = {A0,A1,A2,A3,A4};
int sensorValues[5];
int sensorMin[5], sensorMax[5];

float Kp=0.3, Ki=0.0005, Kd=5.0;
float lastError=0, integral=0;
int BASE_SPEED=180;

void calibrate(){
  for(int i=0;i<200;i++){
    for(int j=0;j<5;j++){
      int val = analogRead(sensorPins[j]);
      sensorMin[j] = min(sensorMin[j], val);
      sensorMax[j] = max(sensorMax[j], val);
    }
    delay(10);
  }
}

float getLinePosition(){
  float weightedSum=0, sum=0;
  for(int i=0;i<5;i++){
    sensorValues[i] = map(analogRead(sensorPins[i]), sensorMin[i], sensorMax[i], 0, 1000);
    weightedSum += sensorValues[i] * (i * 250);  // وزن‌دهی
    sum += sensorValues[i];
  }
  return (sum > 100) ? weightedSum/sum : 500;  // 0–1000، مرکز=500
}

void loop(){
  float position = getLinePosition();
  float error = position - 500;

  integral += error;
  integral = constrain(integral, -5000, 5000);
  float correction = Kp*error + Ki*integral + Kd*(error-lastError);
  lastError = error;

  int leftSpeed  = constrain(BASE_SPEED + correction, 0, 255);
  int rightSpeed = constrain(BASE_SPEED - correction, 0, 255);
  driveDiff(leftSpeed, rightSpeed);
}

کالیبراسیون حسگرها

1. روی UGV بگذار و دستی روی خط بچرخانش در حین calibrate().
2. sensorMin و sensorMax ذخیره شوند در EEPROM برای دفعات بعد.
3. PID را شروع کن: فقط P (مثلاً P=0.3)، اگر نوسان داشت D را بالا ببر.

• Pololu QTR Sensor راهنما
• آپارات: ربات دنبال خط