Scribd
Upload
80 views

Maze Code

This code defines constants and variables for controlling the motors and reading the sensor values of a robot. It contains functions for moving the robot straight, turning left or right, and following the left wall. The main loop reads the sensor values and calls the appropriate functions to navigate based on whether the center or far sensors detect a wall. It stores the path taken in a character array and contains functions for replaying the path.

Uploaded by

MahmudIbrahim
Copyright
© © All Rights Reserved
Available Formats
Download as DOCX, PDF, TXT or read online on Scribd
80 views

Maze Code

This code defines constants and variables for controlling the motors and reading the sensor values of a robot. It contains functions for moving the robot straight, turning left or right, and following the left wall. The main loop reads the sensor values and calls the appropriate functions to navigate based on whether the center or far sensors detect a wall. It stores the path taken in a character array and contains functions for replaying the path.

Uploaded by

MahmudIbrahim
Copyright
© © All Rights Reserved
Available Formats
Download as DOCX, PDF, TXT or read online on Scribd
You are on page 1/ 9

#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 200

#define leftMotor1 7
#define leftMotor2 6

#define rightMotor1 5
#define rightMotor2 8

#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);
//Serial.begin(115200);
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);

// serial printing below for debugging purposes

// Serial.print("leftCenterReading: ");
// Serial.println(leftCenterReading);
// Serial.print("leftNearReading: ");
// Serial.println(leftNearReading);
// Serial.print("leftFarReading: ");
// Serial.println(leftFarReading);

// Serial.print("rightCenterReading: ");
// Serial.println(rightCenterReading);
// Serial.print("rightNearReading: ");
// Serial.println(rightNearReading);
// Serial.print("rightFarReading: ");
// Serial.println(rightFarReading);
// delay(200);


}


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();
}

You might also like