Çizgi İzleyen Robot Uygulaması

Görseldeki Sparkfun TB6612fng motor sürücü kartı pin yapısı verilmiştir. Görseldeki çizgi izleyen robot devresinde Sparkfun TB6612fng (kırmızı renkli PCB) motor sürücü kartı kullanılmıştır. Pololu TB6612fng entegresini kullanan yeşil renkli PCB kartın boyutları ve pin yerleşim düzeni farklıdır. Çizgi izleyen robot programı L298N motor sürücüsüyle de kullanılabilir.

VCC: 5 V besleme.
GND: Topraklama pini.
VM: Motor besleme girişi 2,5 V-13,5 V.
STBY: “LOW”=standby. Sürücüyü aktif yapmak için lojik 1 yapılır. 5 V hattına bağlanılarak da
kullanılabilir.
AO1, AO2: Birinci motor (sağ) çıkışları. 1,2 A sürekli, 3,2 A anlık.
BO1, BO2: İkinci motor (sol) çıkışları. 1,2 A sürekli, 3,2 A anlık.
AIN1, AIN2: Birinci motor (sağ) kontrol girişleri. 200 kΩ dâhilî pull-down.
BIN1, BIN2: İkinci motor (sol) kontrol girişleri. 200 kΩ dâhilî pull-down.
PWMA: Birinci motor (sağ) PWM girişi.
PWMB: İkinci motor (sol) PWM girişi

#include <QTRSensors.h> //Qtr v4.0 

#define PWMA 3 // A sağ motor.
#define AIN2 4
#define AIN1 5 
#define STBY 6
#define BIN1 7 // B sol motor.
#define BIN2 8
#define PWMB 9

#define sensorSayisi 8
#define sensorOrnekSayisi 4
#define emiterPini 11
#define LED 13

int maxHiz = 70; // Motor pwm ayarı 0 - 255.

int hata = 0, turev = 0;
float KP = 0.03, KD = 0.5; // Oran (KP) ve türev (KD) sabitleri. (Her araca göre ayar yapılmalıdır.)

unsigned int pozisyon = 3500;

int fark = 0; // Motorlara uygulanan fark.
int sonHata; // Orantılı son değer. (Hatanın türevini hesaplamak için kullanılır.)
int hedef = 3500; // Sensörden gelen 0 - 7000 arası değerin orta noktası.

QTRSensors qtr; // qtr isimli nesne oluşturuldu.
unsigned int sensor[sensorSayisi];

void setup() {
  qtr.setTypeAnalog();  //QTR-8A ayarla. (QTR-8RC için qtr.setTypeRC() fonksiyonu kullanılır.)
  qtr.setSensorPins((const uint8_t[]) {
    A0, A1, A2, A3, A4, A5, A6, A7
  }, 8);
  pinMode(AIN1, OUTPUT);
  pinMode(AIN2, OUTPUT);
  pinMode(BIN1, OUTPUT);
  pinMode(BIN2, OUTPUT);
  pinMode(LED, OUTPUT);
  pinMode(STBY, OUTPUT);

  delay(1000); //Araca enerji verince 1sn bekle
  kalibrasyon(1); // 0 elle, 1 otomatik kafa sallama.
}

void loop() {
  sensorOku();
  pd();
}

void sensorOku() {
  pozisyon = qtr.readLineWhite(sensor); // Beyaz çizginin pozisyonunu oku. (0 - 7000)
  hata = pozisyon - hedef; // Pozisyondan 3500 (hedef) çıkar. Hatayı bul.
  qtr.read(sensor); // Sekiz sensörün ham değelerini oku.
}

void pd() {
  turev = hata - sonHata; // Hatadan bir önceki hatayı çıkar.
  sonHata = hata; // Şimdiki hatayı kaydet.

  fark = ( hata * KP) + ( turev * KD ); // Motorlara uygulanacak farkı hesapla.

  constrain(fark, -maxHiz, maxHiz); // fark en fazla maxHiz olsun.

  if ( fark < 0 ) // fark negatif ise
    motor(maxHiz, maxHiz + fark); // Sağ motorun hızını düşür.
  else // fark negatif değilse
    motor(maxHiz - fark, maxHiz); // Sol motorun hızını düşür.
}

void motor(int solMotorPWM, int sagMotorPWM) {
  digitalWrite(STBY, HIGH);

  if ( solMotorPWM >= 0 )  { // İleri.
    digitalWrite(BIN1, HIGH);
    digitalWrite(BIN2, LOW);
  }
  else  { // Negatifse geri döndür.
    digitalWrite(BIN1, LOW);
    digitalWrite(BIN2, HIGH);
    solMotorPWM *= -1;
  }
  analogWrite(PWMB, solMotorPWM);

  if ( sagMotorPWM >= 0 )  { // İleri.
    digitalWrite(AIN1, HIGH);
    digitalWrite(AIN2, LOW);
  }
  else  { // Negatifse geri döndür.
    digitalWrite(AIN1, LOW);
    digitalWrite(AIN2, HIGH);
    sagMotorPWM *= -1;
  }
  analogWrite(PWMA, sagMotorPWM);
}

void kalibrasyon(bool secim) { // 1 otomatik, 0 elle.
  if (secim) { // secim 1 ise otomatik kalibrasyon yap.
    byte hiz = 40; // Aracın kafasını sallama hızı.
    for (byte i = 0; i < 3; i++) {  // Sağa sola üç kez kafa salla.
      while (sensor[7] < 300) {
        motor(hiz, -hiz);
        qtr.calibrate();
        sensorOku();
      }
      while (sensor[7] > 700) {
        motor(hiz, -hiz);
        qtr.calibrate();
        sensorOku();
      }
      while (sensor[0] < 300) {
        motor(-hiz, hiz);
        qtr.calibrate();
        sensorOku();
      }
      while (sensor[0] > 700) {
        motor(-hiz, hiz);
        qtr.calibrate();
        sensorOku();
      }
      while (sensor[3] > 500)  { // Ortada dur.
        motor(hiz, -hiz);
        qtr.calibrate();
        sensorOku();
      }
    }
  } else { // secim 0 ise elle kalibrasyon yap.
    for ( byte i = 0; i < 70; i++)  { // Dahili LED yanıp söndüğü sürece (3 sn) elle kalibrasyon yap.
      digitalWrite(LED, HIGH); delay(20);
      qtr.calibrate();
      digitalWrite(LED, LOW); delay(20);
      qtr.calibrate();
    }
  }
  motor(0, 0);
  delay(2000); // Kalbirasyondan sonra 3 sn bekle.
}

#include <QTRSensors.h> //Qtr v4.0 
// L298N motor sürücü pin tanımlamaları.
#define ENA 9 //A sağ motor.
#define IN1 8
#define IN2 7
#define IN3 5 //B sol motor.
#define IN4 4
#define ENB 3

#define sensorSayisi 8
#define sensorOrnekSayisi 4
#define emiterPini 11
#define LED 13
#define STBY 9

byte maxHiz = 70; // Motor pwm ayarı 0 - 255.

int hata = 0, turev = 0;
float KP = 0.03, KD = 0.5; // Oran (KP) ve türev (KD) sabitleri. (Her araca göre ayar yapılmalıdır.)

int pozisyon = 3500;

int fark = 0; // Motorlara uygulanan fark.
int sonHata; // Orantılı son değer. (Hatanın türevini hesaplamak için kullanılır.)
int hedef = 3500; // Sensörden gelen 0 - 7000 arası değerin orta noktası.

QTRSensors qtr; // qtr isimli nesne oluşturuldu.
unsigned int sensor[sensorSayisi];

void setup() {
  qtr.setTypeAnalog();  //QTR-8A ayarla. (QTR-8RC için qtr.setTypeRC() fonksiyonu kullanılır.)
  qtr.setSensorPins((const uint8_t[]) {
    A0, A1, A2, A3, A4, A5, A6, A7
  }, 8);
  pinMode(LED, OUTPUT);
  pinMode(STBY, OUTPUT);

  delay(1000); //Araca enerji verince 1sn bekle
  for ( int i = 0; i < 70; i++)  { // Dahili LED yanıp söndüğü sürece (3 sn) elle kalibrasyon yap.
    digitalWrite(LED, HIGH); delay(20);
    qtr.calibrate();
    digitalWrite(LED, LOW); delay(20);
    qtr.calibrate();
  }
  delay(3000); // Kalbirasyondan sonra 3 sn bekle.
}

void loop() {
  pozisyon = qtr.readLineWhite(sensor); // Beyaz çizginin pozisyonunu oku. (0 - 7000)
  hata = pozisyon - hedef; // Pozisyondan 3500 (hedef) çıkar. Hatayı bul.
  turev = hata - sonHata; // Hatadan bir önceki hatayı çıkar.
  sonHata = hata; // Şimdiki hatayı kaydet.

  int fark = ( hata * KP) + ( turev * KD ); // Motorlara uygulanacak farkı hesapla.

  if ( fark > maxHiz ) fark = maxHiz; // fark en fazla maxHiz olsun.
  else if ( fark < -maxHiz ) fark = -maxHiz;

  if ( fark < 0 ) // fark negatif ise
    motor(maxHiz, maxHiz + fark); // Sağ motorun hızını düşür.
  else // fark negatif değilse
    motor(maxHiz - fark, maxHiz); // Sol motorun hızını düşür.
}

void sagMotor(int pwm) {
  if ( pwm >= 0 )  {
    digitalWrite(IN1, HIGH);
    digitalWrite(IN2, LOW);
  }
  else  {
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, HIGH);
    pwm *= -1;
  }
  analogWrite(ENA, pwm);
}

void solMotor(int pwm) {
  if ( pwm >= 0 )  {
    digitalWrite(IN3, HIGH);
    digitalWrite(IN4, LOW);
  }
  else  {
    digitalWrite(IN3, LOW);
    digitalWrite(IN4, HIGH);
    pwm *= -1;
  }
  analogWrite(ENB, pwm);
}

void motor(int sol, int sag) {
  digitalWrite(STBY, HIGH);
  solMotor(sol);
  sagMotor(sag);
}