Çizgi Labirenti Çözerek En Kısa Yoldan Bitiş Noktasını Bulan Robot Yapımı

Önceki projelerimizde iki duvarlı labirent platformunda robotun labirentin çıkışını bulmasını yapmıştık. Projeye buradan bakabilirsiniz. Bu projede ise çizgi labirenti çözen robot yapımını anlatacağım. Projenin özetle mantığı şu şekilde; robot başlangıç noktasında başlayarak çizgiyi takip edip bitiş noktasına varacak. Yalnız ilk denemede yanlış yollara sapacağı için varış zamanı daha geç olacaktır. Fakat her yanlış yola saptığında bunları bir algoritma ile saptayacak ve ikinci denemede bu yollara girmeyecek. Kısacası ilk denemede robot gittiği yolu kaydedip en kısa yolu tespit ederek ikinci denemede bu tespit ettiği yoldan giderek en kısa yolu bulmuş olacak.

Ürünü Satın Al

Malzemeler

1. Arduino

2. QTR-8A Kızılötesi sensör

3. 2 Adet DC Motor ve Robot Şasesi veya hazır robot kit alabilirsiniz.

4. L298N veya l293b motor sürücü

5. 7.4v Batarya

Genel Tasarımı : 

Devre Şeması:

Arduino Kodu:

#define leftCenterSensor 3
#define leftNearSensor 4
#define leftFarSensor 5
#define rightCenterSensor 2
#define rightNearSensor 1
#define rightFarSensor 0

int leftCenterReading;
int leftNearReading;
int leftFarReading;
int rightCenterReading;
int rightNearReading;
int rightFarReading;

int leftNudge;
int replaystage;
int rightNudge;

#define leapTime 400
#define leftMotor1 9
#define leftMotor2 8
#define rightMotor1 6
#define rightMotor2 7
#define led 13

char path[30] = {};
int pathLength;
int readLength;

void setup(){

pinMode(leftCenterSensor, INPUT);
pinMode(leftNearSensor, INPUT);
pinMode(leftFarSensor, INPUT);
pinMode(rightCenterSensor, INPUT);
pinMode(rightNearSensor, INPUT);
pinMode(rightFarSensor, INPUT);

pinMode(leftMotor1, OUTPUT);
pinMode(leftMotor2, OUTPUT);
pinMode(rightMotor1, OUTPUT);
pinMode(rightMotor2, OUTPUT);


pinMode(led, OUTPUT);
digitalWrite(led, HIGH);
delay(1000);
}

void loop(){

readSensors();

if(leftFarReading<200 && rightFarReading<200 &&
(leftCenterReading>200 || rightCenterReading>200) ){
straight();
}
else{
leftHandWall();
}

}

void readSensors(){

leftCenterReading = analogRead(leftCenterSensor);
leftNearReading = analogRead(leftNearSensor);
leftFarReading = analogRead(leftFarSensor);
rightCenterReading = analogRead(rightCenterSensor);
rightNearReading = analogRead(rightNearSensor);
rightFarReading = analogRead(rightFarSensor);

}

void leftHandWall(){

if( leftFarReading>200 && rightFarReading>200){
digitalWrite(leftMotor1, HIGH);
digitalWrite(leftMotor2, LOW);
digitalWrite(rightMotor1, HIGH);
digitalWrite(rightMotor2, LOW);

delay(leapTime);
readSensors();

if(leftFarReading>200 || rightFarReading>200){
done();
}
if(leftFarReading<200 && rightFarReading<200){
turnLeft();
}

}

if(leftFarReading>200){ // if you can turn left then turn left
digitalWrite(leftMotor1, HIGH);
digitalWrite(leftMotor2, LOW);
digitalWrite(rightMotor1, HIGH);
digitalWrite(rightMotor2, LOW);

delay(leapTime);
readSensors();

if(leftFarReading<200 && rightFarReading<200){
turnLeft();
}
else{
done();
}
}

if(rightFarReading>200){
digitalWrite(leftMotor1, HIGH);
digitalWrite(leftMotor2, LOW);
digitalWrite(rightMotor1, HIGH);
digitalWrite(rightMotor2, LOW);

delay(30);
readSensors();

if(leftFarReading>200){
delay(leapTime-30);
readSensors();

if(rightFarReading>200 && leftFarReading>200){
done();
}
else{
turnLeft();
return;
}
}
delay(leapTime-30);
readSensors();
if(leftFarReading<200 && leftCenterReading<200 &&
rightCenterReading<200 && rightFarReading<200){
turnRight();
return;
}
path[pathLength]='S';
// Serial.println("s");
pathLength++;
//Serial.print("Path length: ");
//Serial.println(pathLength);
if(path[pathLength-2]=='B'){
//Serial.println("shortening path");
shortPath();
}
straight();
}
readSensors();
if(leftFarReading<200 && leftCenterReading<200 && rightCenterReading<200
&& rightFarReading<200 && leftNearReading<200 && rightNearReading<200){
turnAround();
}

}
void done(){
digitalWrite(leftMotor1, LOW);
digitalWrite(leftMotor2, LOW);
digitalWrite(rightMotor1, LOW);
digitalWrite(rightMotor2, LOW);

replaystage=1;
path[pathLength]='D';
pathLength++;
while(analogRead(leftFarSensor)>200){
digitalWrite(led, LOW);
delay(150);
digitalWrite(led, HIGH);
delay(150);
}
delay(500);
replay();
}

void turnLeft(){

while(analogRead(rightCenterSensor)>200||analogRead(leftCenterSensor)>200){
digitalWrite(leftMotor1, LOW);
digitalWrite(leftMotor2, HIGH);
digitalWrite(rightMotor1, HIGH);
digitalWrite(rightMotor2, LOW);

delay(2);
digitalWrite(leftMotor1, LOW);
digitalWrite(leftMotor2, LOW);
digitalWrite(rightMotor1, LOW);
digitalWrite(rightMotor2, LOW);

delay(1);
}

while(analogRead(rightCenterSensor)<200){
digitalWrite(leftMotor1, LOW);
digitalWrite(leftMotor2, HIGH);
digitalWrite(rightMotor1, HIGH);
digitalWrite(rightMotor2, LOW);

delay(2);
digitalWrite(leftMotor1, LOW);
digitalWrite(leftMotor2, LOW);
digitalWrite(rightMotor1, LOW);
digitalWrite(rightMotor2, LOW);

delay(1);
}

if(replaystage==0){
path[pathLength]='L';
//Serial.println("l");
pathLength++;
//Serial.print("Path length: ");
//Serial.println(pathLength);
if(path[pathLength-2]=='B'){
//Serial.println("shortening path");
shortPath();
}
}
}

void turnRight(){

while(analogRead(rightCenterSensor)>200){
digitalWrite(leftMotor1, HIGH);
digitalWrite(leftMotor2, LOW);
digitalWrite(rightMotor1, LOW);
digitalWrite(rightMotor2, HIGH);

delay(2);
digitalWrite(leftMotor1, LOW);
digitalWrite(leftMotor2, LOW);
digitalWrite(rightMotor1, LOW);
digitalWrite(rightMotor2, LOW);

delay(1);
}
while(analogRead(rightCenterSensor)<200){
digitalWrite(leftMotor1, HIGH);
digitalWrite(leftMotor2, LOW);
digitalWrite(rightMotor1, LOW);
digitalWrite(rightMotor2, HIGH);

delay(2);
digitalWrite(leftMotor1, LOW);
digitalWrite(leftMotor2, LOW);
digitalWrite(rightMotor1, LOW);
digitalWrite(rightMotor2, LOW);

delay(1);
}
while(analogRead(leftCenterSensor)<200){
digitalWrite(leftMotor1, HIGH);
digitalWrite(leftMotor2, LOW);
digitalWrite(rightMotor1, LOW);
digitalWrite(rightMotor2, HIGH);

delay(2);
digitalWrite(leftMotor1, LOW);
digitalWrite(leftMotor2, LOW);
digitalWrite(rightMotor1, LOW);
digitalWrite(rightMotor2, LOW);

delay(1);
}

if(replaystage==0){
path[pathLength]='R';
Serial.println("r");
pathLength++;
Serial.print("Path length: ");
Serial.println(pathLength);
if(path[pathLength-2]=='B'){
Serial.println("shortening path");
shortPath();
}
}

}

void straight(){
if( analogRead(leftCenterSensor)<200){
digitalWrite(leftMotor1, HIGH);
digitalWrite(leftMotor2, LOW);
digitalWrite(rightMotor1, HIGH);
digitalWrite(rightMotor2, LOW);

delay(1);
digitalWrite(leftMotor1, HIGH);
digitalWrite(leftMotor2, LOW);
digitalWrite(rightMotor1, LOW);
digitalWrite(rightMotor2, LOW);

delay(5);
return;
}
if(analogRead(rightCenterSensor)<200){
digitalWrite(leftMotor1, HIGH);
digitalWrite(leftMotor2, LOW);
digitalWrite(rightMotor1, HIGH);
digitalWrite(rightMotor2, LOW);

delay(1);
digitalWrite(leftMotor1, LOW);
digitalWrite(leftMotor2, LOW);
digitalWrite(rightMotor1, HIGH);
digitalWrite(rightMotor2, LOW);

delay(5);
return;
}

digitalWrite(leftMotor1, HIGH);
digitalWrite(leftMotor2, LOW);
digitalWrite(rightMotor1, HIGH);
digitalWrite(rightMotor2, LOW);

delay(4);
digitalWrite(leftMotor1, LOW);
digitalWrite(leftMotor2, LOW);
digitalWrite(rightMotor1, LOW);
digitalWrite(rightMotor2, LOW);

delay(1);

}

void turnAround(){
digitalWrite(leftMotor1, HIGH);
digitalWrite(leftMotor2, LOW);
digitalWrite(rightMotor1, HIGH);
digitalWrite(rightMotor2, LOW);

delay(150);
while(analogRead(leftCenterSensor)<200){
digitalWrite(leftMotor1, LOW);
digitalWrite(leftMotor2, HIGH);
digitalWrite(rightMotor1, HIGH);
digitalWrite(rightMotor2, LOW);

delay(2);
digitalWrite(leftMotor1, LOW);
digitalWrite(leftMotor2, LOW);
digitalWrite(rightMotor1, LOW);
digitalWrite(rightMotor2, LOW);

delay(1);
}
path[pathLength]='B';
pathLength++;
straight();
//Serial.println("b");
//Serial.print("Path length: ");
//Serial.println(pathLength);
}

void shortPath(){
int shortDone=0;
if(path[pathLength-3]=='L' && path[pathLength-1]=='R'){
pathLength-=3;
path[pathLength]='B';
//Serial.println("test1");
shortDone=1;
}

if(path[pathLength-3]=='L' && path[pathLength-1]=='S' && shortDone==0){
pathLength-=3;
path[pathLength]='R';
//Serial.println("test2");
shortDone=1;
}

if(path[pathLength-3]=='R' && path[pathLength-1]=='L' && shortDone==0){
pathLength-=3;
path[pathLength]='B';
//Serial.println("test3");
shortDone=1;
}


if(path[pathLength-3]=='S' && path[pathLength-1]=='L' && shortDone==0){
pathLength-=3;
path[pathLength]='R';
//Serial.println("test4");
shortDone=1;
}

if(path[pathLength-3]=='S' && path[pathLength-1]=='S' && shortDone==0){
pathLength-=3;
path[pathLength]='B';
//Serial.println("test5");
shortDone=1;
}
if(path[pathLength-3]=='L' && path[pathLength-1]=='L' && shortDone==0){
pathLength-=3;
path[pathLength]='S';
//Serial.println("test6");
shortDone=1;
}

path[pathLength+1]='D';
path[pathLength+2]='D';
pathLength++;
//Serial.print("Path length: ");
//Serial.println(pathLength);
//printPath();
}

void printPath(){
Serial.println("+++++++++++++++++");
int x;
while(x<=pathLength){
Serial.println(path[x]);
x++;
}
Serial.println("+++++++++++++++++");
}

void replay(){
readSensors();
if(leftFarReading<200 && rightFarReading<200){
straight();
}
else{
if(path[readLength]=='D'){
digitalWrite(leftMotor1, HIGH);
digitalWrite(leftMotor2, LOW);
digitalWrite(rightMotor1, HIGH);
digitalWrite(rightMotor2, LOW);

delay(100);
digitalWrite(leftMotor1, LOW);
digitalWrite(leftMotor2, LOW);
digitalWrite(rightMotor1, LOW);
digitalWrite(rightMotor2, LOW);

endMotion();
}
if(path[readLength]=='L'){
digitalWrite(leftMotor1, HIGH);
digitalWrite(leftMotor2, LOW);
digitalWrite(rightMotor1, HIGH);
digitalWrite(rightMotor2, LOW);

delay(leapTime);
turnLeft();
}
if(path[readLength]=='R'){
digitalWrite(leftMotor1, HIGH);
digitalWrite(leftMotor2, LOW);
digitalWrite(rightMotor1, HIGH);
digitalWrite(rightMotor2, LOW);

delay(leapTime);
turnRight();
}
if(path[readLength]=='S'){
digitalWrite(leftMotor1, HIGH);
digitalWrite(leftMotor2, LOW);
digitalWrite(rightMotor1, HIGH);
digitalWrite(rightMotor2, LOW);

delay(leapTime);
straight();
}

readLength++;
}

replay();

}

void endMotion(){

digitalWrite(led, LOW);
delay(500);
digitalWrite(led, HIGH);
delay(200);
digitalWrite(led, LOW);
delay(200);
digitalWrite(led, HIGH);
delay(500);
endMotion();
}