#include <Servo.h>
#include <PID_v1.h> //Pid Kütüphanesi
#include <filters.h> //LowPassFilter kütüphanesi

//Filtre Tanımlamaları
const float cutoff_freq = 0.5; //Cutoff frequency in Hz
const float sampling_time = 0.01; //Sampling time in seconds.
IIR::ORDER order = IIR::ORDER::OD3; // Order (OD1 to OD4)
Filter f(cutoff_freq, sampling_time, order);

#define echoPinC 12 //Ultrasonik sensörü orta echo
#define trigPinC 13//Ultrasonik sensörü orta trig
#define Maksimum_uzaklik 100

// Motor pinlerini tanımlıyoruz.
#define MotorR1 8
#define MotorR2 7
#define MotorRE 9
#define MotorL1 5
#define MotorL2 4
#define MotorLE 11
#define ENCA 2
#define ENCB 3

Servo servo;
NewPing sonar(trigPinC, echoPinC, Maksimum_uzaklik);//ultrasonik sensör tanımlama
//kullanılacak eleman tanımı
unsigned int uzaklik;
unsigned int on_uzaklik;
unsigned int sol_uzaklik;
unsigned int sag_uzaklik;
unsigned int zaman;



long sure, uzaklik,uzaklikC; //süre ve uzaklık değişkenleri tanımlıyoruz.

int PWM1 = 95 ,PWM2 = 95;
unsigned int rpm1,rpm2; // rpm tutan veri
volatile byte pulses1,pulses2; // sinyal sayısı
unsigned long timeold1, timeold2;
// devir başına sinyal sayısı kodlayıcı diske göre
unsigned int pulsesperturn1 = 20;
unsigned int pulsesperturn2 = 20;


//PID tanımlamaları
double Setpoint, Input1,Input2, Output1,Output2;
//PID Başlangıç ayarları
double Kp=3.015, Ki=0.210, Kd=2;

PID myPID1(&Input1, &Output1, &Setpoint, Kp, Ki, Kd, DIRECT);
PID myPID2(&Input2, &Output2, &Setpoint, Kp, Ki, Kd, DIRECT);

void counter1()
{
//sayımı arttır
pulses1++;
}

void counter2()
{
//sayımı arttır
pulses2++;
}


void setup() {
// ultrasonik sensör Trig pininden ses dalgaları gönderdiği için OUTPUT (Çıkış),
// bu dalgaları Echo pini ile geri aldığı için INPUT (Giriş) olarak tanımlanır.
pinMode(echoPinC, INPUT);
pinMode(trigPinC, OUTPUT);

pinMode(MotorL1, OUTPUT);
pinMode(MotorL2, OUTPUT);
pinMode(MotorLE, OUTPUT); //Motorlarımızı çıkış olarak tanımlıyoruz.
pinMode(MotorR1, OUTPUT);
pinMode(MotorR2, OUTPUT);
pinMode(MotorRE, OUTPUT);
servo.attach(8);

pinMode(ENCA, INPUT);
pinMode(ENCB, INPUT);

//attachInterrupt(digitalPinToInterrupt(ENCA), readEncoder,RISING);
attachInterrupt(0, counter1, FALLING);
attachInterrupt(1, counter2, FALLING);
pulses1 = 0;
pulses2 = 0;
rpm1 = 0;
rpm2 = 0;
timeold1 = 0;
timeold2 = 0;

Input1 = 0;
Input2 = 0;
Setpoint = 100;
//PID sag otomotik modda başlatılıyor
myPID1.SetMode(AUTOMATIC);
myPID1.SetSampleTime(10);
myPID1.SetOutputLimits(100, 255);

//PID sol otomotik modda başlatılıyor
myPID2.SetMode(AUTOMATIC);
myPID2.SetSampleTime(10);
myPID2.SetOutputLimits(100, 255);

Serial.begin(9600);
}

void loop() {

if (millis() - timeold1 >= 1000) {
//Hesaplamalar sırasında kesintileri işlemeyin
detachInterrupt(0);
rpm1 = (60 * 1000 / pulsesperturn1 )/ (millis() - timeold1)* pulses1;
timeold1 = millis();
pulses1 = 0;
Serial.print("RPM = ");
Serial.println(rpm1,DEC);
Serial.print("PWM1 = ");
Serial.println(PWM1,DEC);
//Kesinti işlemeyi yeniden başlatın
attachInterrupt(0, counter1, FALLING);
}

if (millis() - timeold2 >= 1000) {
//Hesaplamalar sırasında kesintileri işlemeyin
detachInterrupt(1);
rpm2 = (60 * 1000 / pulsesperturn2 )/ (millis() - timeold2)* pulses2;
timeold2 = millis();
pulses2 = 0;
Serial.print("RPM2 = ");
Serial.println(rpm2,DEC);
Serial.print("PWM2 = ");
Serial.println(PWM2,DEC);
//Kesinti işlemeyi yeniden başlatın
attachInterrupt(1, counter2, FALLING);
}
Input1 = rpm1;
Input2 = rpm2;
myPID1.Compute();
myPID2.Compute();
Serial.print("PID1 Output : ");
Serial.println(Output1,DEC);

Serial.print("PID2 Output : ");
Serial.println(Output2,DEC);

Serial.print("Filitreli Output1 : ");
Serial.println(f.filterIn(Output1));

Serial.print("Filitreli Output2 : ");
Serial.println(f.filterIn(Output2));




PWM1=f.filterIn(Output1);
PWM2=f.filterIn(Output2);

uzaklikC = mesafeolc(trigPinC,echoPinC);
// sonsuz döngü
void loop()
{
delay(500);
servo.write(90);
sensor_olcum();
on_uzaklik = uzaklik;
if(on_uzaklik > 35 || on_uzaklik == 0)
{
ileri();
}
else
{
dur();
servo.write(180);
delay(500);
sensor_olcum();
sol_uzaklik = uzaklik;
servo.write(0);
delay(500);
sensor_olcum();
sag_uzaklik = uzaklik;
servo.write(90);
delay(300);
if(sag_uzaklik>sol_uzaklik)
{
sag();
delay(200);
ileri();
}
else if(sol_uzaklik >sag_uzaklik)
{
sol();
delay(200);
ileri();
}
else
{
geri();
}
}
}

long mesafeolc(int trigpin,int echopin){
digitalWrite(trigpin, LOW); //sensör pasif hale getirildi
delayMicroseconds(5);
digitalWrite(trigpin, HIGH); //Sensore ses dalgasının üretmesi için emir verildi
delayMicroseconds(10);
digitalWrite(trigpin, LOW); //Yeni dalgaların üretilmemesi için trig pini LOW konumuna getirildi
sure = pulseIn(echopin, HIGH); //ses dalgasının geri dönmesi için geçen sure ölçülüyor
uzaklik = sure / 29.1 / 2; //ölçülen süre uzaklığa çevriliyor cm
return uzaklik;
}

void ileri(){ // Robotun ileri yönde hareketi için fonksiyon tanımlıyoruz.
digitalWrite(MotorR1, HIGH); // Sağ motorun ileri hareketi aktif
digitalWrite(MotorR2, LOW); // Sağ motorun geri hareketi pasif
analogWrite(MotorRE, 150); // Sağ motorun hızı 150
digitalWrite(MotorL1, HIGH); // Sol motorun ileri hareketi aktif
digitalWrite(MotorL2, LOW); // Sol motorun geri hareketi pasif
analogWrite(MotorLE, 150); // Sol motorun hızı 150
}

void sag(){ // Robotun sağa dönme hareketi için fonksiyon tanımlıyoruz.
digitalWrite(MotorR1, LOW); // Sağ motorun ileri hareketi aktif
digitalWrite(MotorR2, LOW); // Sağ motorun geri hareketi pasif
analogWrite(MotorRE, 0); // Sağ motorun hızı 0 (Motor duruyor)

digitalWrite(MotorL1, HIGH); // Sol motorun ileri hareketi aktif
digitalWrite(MotorL2, LOW); // Sol motorun geri hareketi pasif
analogWrite(MotorLE, 150); // Sol motorun hızı 150

}

void sol(){ // Robotun sağa dönme hareketi için fonksiyon tanımlıyoruz.
digitalWrite(MotorR1, HIGH); // Sağ motorun ileri hareketi aktif
digitalWrite(MotorR2, LOW); // Sağ motorun geri hareketi pasif
analogWrite(MotorRE, 0); // Sağ motorun hızı 0 (Motor duruyor)

digitalWrite(MotorL1, LOW); // Sol motorun ileri hareketi aktif
digitalWrite(MotorL2, LOW); // Sol motorun geri hareketi pasif
analogWrite(MotorLE, 150); // Sol motorun hızı 150
}

void geri(){ // Robotun geri yönde hareketi için fonksiyon tanımlıyoruz.
digitalWrite(MotorR1, LOW); // Sağ motorun ileri hareketi pasif
digitalWrite(MotorR2, HIGH); // Sağ motorun geri hareketi aktif
analogWrite(MotorRE, 150); // Sağ motorun hızı 150

digitalWrite(MotorL1, LOW); // Sol motorun ileri hareketi pasif
digitalWrite(MotorL2, HIGH); // Sol motorun geri hareketi aktif
analogWrite(MotorLE, 150); // Sol motorun hızı 150
}