Notes

#include <QTRSensors.h>

#define NUM_SENSORS 8 // number of sensors used
#define NUM_SAMPLES_PER_SENSOR 4 // average 4 analog samples per sensor reading
#define EMITTER_PIN 2 // emitter is controlled by digital pin 2

QTRSensorsAnalog qtra((unsigned char[]) {0, 1, 2, 3, 4, 5, 6, 7},
NUM_SENSORS, NUM_SAMPLES_PER_SENSOR, EMITTER_PIN);
unsigned int sensorValues[NUM_SENSORS];

//TODO: Change pins
#define PWM_LEFT 9
#define PWM_RIGHT 11
#define LEFT_UP 2
#define LEFT_DOWN 3
#define RIGHT_UP 7
#define RIGHT_DOWN 8

#define SHARP 5

#define BASE_SPEED 70
#define KP 70
#define KD 40
#define KI 0

void setLeftDirection(bool);
void setRightDirection(bool);

float TIME_DIFF;
float total_left_distance_error;
float total_right_distance_error;
float last_right_distance_error;
float last_left_distance_error;

int leapCount = 0;
int haydi = 0;
int BITTI = 0;
int distance = 1;
int lastDistance = 1;

void setup() {
pinMode(PWM_LEFT, OUTPUT);
pinMode(PWM_RIGHT, OUTPUT);
pinMode(LEFT_UP, OUTPUT);
pinMode(LEFT_DOWN, OUTPUT);
pinMode(RIGHT_UP, OUTPUT);
pinMode(RIGHT_DOWN, OUTPUT);
pinMode(SHARP, INPUT);

digitalWrite(13, HIGH); // turn on Arduino's LED to indicate we are in calibration mode
for (int i = 0; i < 200; i++) // make the calibration take about 10 seconds
{
qtra.calibrate(); // reads all sensors 10 times at 2.5 ms per six sensors (i.e. ~25 ms per call)
}
digitalWrite(13, LOW); // turn off Arduino's LED to indicate we are through with calibration

// print the calibration minimum values measured when emitters were on
Serial.begin(9600);
for (int i = 0; i < NUM_SENSORS; i++)
{
Serial.print(qtra.calibratedMinimumOn[i]);
Serial.print(' ');
}
Serial.println();

// print the calibration maximum values measured when emitters were on
for (int i = 0; i < NUM_SENSORS; i++)
{
Serial.print(qtra.calibratedMaximumOn[i]);
Serial.print(' ');
}
Serial.println();
Serial.println();
delay(1000);

TIME_DIFF = millis()/1000;
}

void loop() {

float avarage_distance = 3500;
//int beyaz = checkifwhite();

unsigned int distance = qtra.readLine(sensorValues,1,1);
float left_distance_error = (avarage_distance - distance) / avarage_distance;
float right_distance_error = (distance - avarage_distance) / avarage_distance;
lastDistance = distance;
distance = getDistance();
//Serial.println(front_distance);
TIME_DIFF -= millis();
TIME_DIFF /= -1000;
TIME_DIFF += 0.01;
float left_error_derivative = (last_left_distance_error - left_distance_error) / TIME_DIFF;
float right_error_derivative = (last_right_distance_error - right_distance_error) / TIME_DIFF;


setLeftDirection(true);
setRightDirection(true);

int left_speed = BASE_SPEED + KP * left_distance_error - KD * left_error_derivative + KI * total_left_distance_error;
int right_speed = BASE_SPEED + KP * right_distance_error - KD * right_error_derivative + KI * total_right_distance_error;



///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////SAĞA KAYMA////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
timehascome();
if (haydi == 1){
leapCount++;
//Serial.print(leapCount);
if (leapCount <= 40){
int left_speed = BASE_SPEED + KP * left_distance_error - KD * left_error_derivative + KI * total_left_distance_error;
int right_speed = BASE_SPEED + KP * right_distance_error - KD * right_error_derivative + KI * total_right_distance_error;
}
else if (leapCount > 40) //AYARLANACAK
{
haydi=0;
//leapCount = 0;
left_speed = BASE_SPEED - 1; //AYARLANACAK2
right_speed = BASE_SPEED - 15;
/* Serial.print(", ");
Serial.print(left_speed);
Serial.print(", ");
Serial.println(right_speed);*/

delay(1000); //AYARLANACAK3
//BITTI = 1;

left_speed = BASE_SPEED; //AYARLANACAK2
right_speed = BASE_SPEED;
delay(500);


Serial.println(space());

int ert = space();
while (ert == 1){ //SPACE 1 VERİRKEN(YANİ BOŞLUKTAYKEN), BASE_SPEED-10 VERMEYE DEVAM EDECEK.
Serial.println("BOSLUKTAYIM");
left_speed = BASE_SPEED-10; //YOL DEĞİŞTİRİRKEN HIZLARI BASE_SPEED - 10 YAPIYORUM, BURASI OPSİYONEL
right_speed = BASE_SPEED-10;
ert = space();
}

}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////SAĞA KAYMA SON//////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////


/* int ert = space();
if (ert == 1){
//Serial.println("BOSLUKTAYIM");
left_speed = BASE_SPEED;
right_speed = BASE_SPEED;
}*/
/*Serial.print(", ");
Serial.print(left_speed);
Serial.print(", ");
Serial.println(right_speed);*/


if (left_speed > 200){
left_speed = 200;
}
else if (left_speed < 0){
left_speed = 0;
}
if (right_speed > 200){
right_speed = 200;
}
else if (right_speed < 0){
right_speed = 0;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
if(distance == 0){
left_speed = 0;
right_speed = 0;
}

if (getDistance != distance){
delay(1000);
}

analogWrite(PWM_LEFT, left_speed);
analogWrite(PWM_RIGHT, right_speed);

last_left_distance_error = left_distance_error;
last_right_distance_error = right_distance_error;

total_left_distance_error +=left_distance_error;
total_right_distance_error +=right_distance_error;
TIME_DIFF = millis();
for (int ah=0; ah<7;ah++){
Serial.print(sensorValues[ah]);
Serial.print(", ");

}
Serial.println("");
}

int timehascome()
{
BITTI = 1;
int total=0;
/*for (int i = 0; i < NUM_SENSORS; i++)
{
total += sensorValues[i];
}
if (total>5000){*/
//Serial.println(sensorValues[0]);
if (sensorValues[0] < 300 && sensorValues[1] < 300 && sensorValues[2] < 300 && sensorValues[3] < 300 && sensorValues[4] < 300 && sensorValues[5] < 300 && sensorValues[6] < 300 && sensorValues[7] < 300){

Serial.println("timehascome");
haydi=1;
return 1;
}
}

int space()
{
int total=0;
/*for (int i = 0; i < NUM_SENSORS; i++)
{
total += sensorValues[i];
}*/
if (sensorValues[0] > 700 && sensorValues[1] > 700 && sensorValues[2] > 700 && sensorValues[3] > 700 && sensorValues[4] > 700 && sensorValues[5] > 700 && sensorValues[6] > 700 && sensorValues[6] > 700){

Serial.println("Space");
return 1;
}
else
{
return 0;;
}
}

int checkifwhite()
{
long total;
int average;
total = 0;
average = 0;
qtra.read(sensorValues);
for (int i = 0; i < NUM_SENSORS; i++)
{
total += sensorValues[i];
}
average = total / NUM_SENSORS;
if (average < 900)//this is white surface
{
return 0;
}
else//black surface
{
return 1;
}
}

void setLeftDirection(bool is_forward){
if (is_forward){
digitalWrite(LEFT_UP, HIGH);
digitalWrite(LEFT_DOWN, LOW);
} else{
digitalWrite(LEFT_UP, LOW);
digitalWrite(LEFT_DOWN, HIGH);
}
}

void setRightDirection(bool is_forward){
if (is_forward){
digitalWrite(RIGHT_UP, HIGH);
digitalWrite(RIGHT_DOWN, LOW);
} else{
digitalWrite(RIGHT_UP, LOW);
digitalWrite(RIGHT_DOWN, HIGH);
}
}

int getDistance(){
if (digitalRead(SHARP) == 0){
return 0;
} else if (digitalRead(SHARP) == 1){
return 1;
}
}