Notes

#include<SoftwareSerial.h>

const int trig = 6;
const int echo = 7;

int sure = 0; // mesafe hesabından kullanılacak olan değişkenler tanımladı
int mesafe = 0;
int count = 1;


int S1, S2, S3, S4, S5; // sol ve sağ sensörün durum değişkenleri tanımlandı



float Kp=25,Ki=0.6,Kd=2;
float error=0, P=0, I=0, D=0, PID_value=0;
float previous_I=0;
float previous_error=0;
int sensor[5]={0, 0, 0, 0, 0};
int initial_motor_speed=70;

int line;
int outline;

void read_sensor_values(void);
void calculate_pid(void);
void motor_control(void);

void setup()
{
pinMode(9,OUTPUT); //PWM Pin 1
pinMode(10,OUTPUT); //PWM Pin 2
pinMode(4,OUTPUT); //Left Motor Pin 1
pinMode(5,OUTPUT); //Left Motor Pin 20
pinMode(2,OUTPUT); //Right Motor Pin 1
pinMode(3,OUTPUT); //Right Motor Pin 2
Serial.begin(9600); //Enable Serial Communications
}








void arama()
{

line = 0;
outline = 1;

while ((sensor[0]==outline)&&(sensor[1]==outline)&&(sensor[2]==outline)&&(sensor[3]==outline)&&(sensor[4]==outline))
{


S1 = digitalRead(sensor[0]); // sol ve sağ sensör okunup değişkenlere kaydedildi.||
S2 = digitalRead(sensor[1]);
S3 = digitalRead(sensor[2]);
S4 = digitalRead(sensor[3]);
S5 = digitalRead(sensor[4]);

digitalWrite(trig , HIGH);
delayMicroseconds(1000);
digitalWrite(trig , LOW);

sure = pulseIn(echo , HIGH);
mesafe = (sure / 2) / 28.5;

if (mesafe < 10 ) // mesafe 30cm den küçük ise robotu geri al ve döndür
{
delay(150);
digitalWrite(2 , LOW);
digitalWrite(3 , HIGH);
digitalWrite(4 , LOW);
digitalWrite(5 , LOW);
delay(150);

}
else // mesafe 30cm den büyük ise düz git
{
delay(150);
digitalWrite(2 , HIGH);
digitalWrite(3 , LOW);
digitalWrite(4 , HIGH);
digitalWrite(5 , LOW);
delay(150);
}

}

}












void loop()
{

arama();



read_sensor_values();
calculate_pid();
Serial.println("calculate");
Serial.println(PID_value);
Serial.println("void içi");
Serial.println(PID_value);

motor_control();
Serial.println("motor kontrol");
Serial.println(PID_value);
}


void read_sensor_values()
{
line = 0;
outline = 1;
sensor[0]=digitalRead(A0);
sensor[1]=digitalRead(A1);
sensor[2]=digitalRead(A2);
sensor[3]=digitalRead(A3);
sensor[4]=digitalRead(A4);

if((sensor[0]==outline)&&(sensor[1]==outline)&&(sensor[2]==outline)&&(sensor[3]==outline)&&(sensor[4]==line))
error=4;
else if((sensor[0]==line)&&(sensor[1]==outline)&&(sensor[2]==outline)&&(sensor[4]==outline)&&(sensor[4]==outline))
error=-4;

if((sensor[0]==outline)&&(sensor[1]==outline)&&(sensor[2]==outline)&&(sensor[3]==line)&&(sensor[4]==line))
error=3;
else if((sensor[0]==line)&&(sensor[1]==line)&&(sensor[2]==outline)&&(sensor[3]==outline)&&(sensor[4]==outline))
error=-3;

if((sensor[0]==outline)&&(sensor[1]==outline)&&(sensor[2]==outline)&&(sensor[3]==line)&&(sensor[4]==outline))
error=2;
else if((sensor[0]==outline)&&(sensor[1]==line)&&(sensor[2]==outline)&&(sensor[3]==outline)&&(sensor[4]==outline))
error=-2;

if((sensor[0]==outline)&&(sensor[1]==outline)&&(sensor[2]==line)&&(sensor[3]==line)&&(sensor[4]==outline))
error=1;
else if((sensor[0]==outline)&&(sensor[1]==line)&&(sensor[2]==line)&&(sensor[3]==outline)&&(sensor[4]==outline))
error=-1;

if((sensor[1]==outline)&&(sensor[2]==line)&&(sensor[3]==outline))
error=0;
}


void calculate_pid()
{
P = error;
I = I + previous_I;
D = error-previous_error;

PID_value = (Kp*P) + (Ki*I) + (Kd*D);

previous_I=I;
previous_error=error;


}


void motor_control()
{
int left_motor_speed;
int right_motor_speed;

// Calculo da velocidade de cada motor:
left_motor_speed = initial_motor_speed+PID_value;
right_motor_speed = initial_motor_speed-PID_value;

if(left_motor_speed < 0)
left_motor_speed = 0;
if(right_motor_speed < 0)
right_motor_speed = 0;

analogWrite(10,left_motor_speed); //Left Motor Speed
analogWrite(9,right_motor_speed); //Right Motor Speed


digitalWrite(2,HIGH);
digitalWrite(4,HIGH);
//following lines of code are to make the bot move forward
/*The pin numbers and high, low values might be different
depending on your connections */


}