PDA

View Full Version : Trouble Using Photosensors in Nav Code



MTA_CPE
03-25-2011, 09:01 AM
Ok, I am entering a firefighting contest at a local univesrsity, this is my first time programming or building a robot

I have written the code for therobot to navigate the maze,If your familiar with the maze used at trinity, this bot goes into the island room, then calls a function to be a left wall follower


#include <EncodersAB.h>
#include <Motors.h>
#include <SharpIR.h>


Motors drive = Motors(); //drive motors
SharpIR rightir = SharpIR(GP2D12,3);
SharpIR leftir = SharpIR(GP2D12,1);
SharpIR centerir = SharpIR(GP2D12,2);
volatile int flame=0;
int r=analogRead(5);
int l=analogRead(4);
int c = analogRead(0);
boolean wallFollow = false;
boolean follow = false;

void setup()
{

// This version separates the initial turn and then allows the bot to detect a room from the forward function
Serial.begin(38400);
delay(250);
int r=analogRead(5);
int l=analogRead(4);
int c = analogRead(0);


}

void loop()
{
int r=analogRead(5);
int c = analogRead(0);
int l=analogRead(4);






drive.set(125,125);









if((centerir.getData()<15)){//against a wall with an opening on one side

if(rightir.getData()>leftir.getData()){//Against a wall
drive.set(0,0);
delay(300);
do{drive.set(150,-150);//Turn to the right
}while(centerir.getData()<30);
drive.set(100,100);//Move forward to avoid the loop
delay(1500); //May have to increase or decrease this value
}

if(rightir.getData()<leftir.getData()){
drive.set(0,0);
delay(300);
do{drive.set(-150,150);//Turn to the left
}while(centerir.getData()<30);
drive.set(100,100);//Move Forward to avoid the loop
delay(1500); //May have to increase or decrease this value
}
}

if(((rightir.getData()>60)&&(leftir.getData()>40))&&(centerir.getData()>30)){// Opening on the right hand side /// this might not be required, possibly omit
drive.set(0,0);
delay(300);
drive.set(100,100); //Get Clearance for the turn
delay(1000);
drive.set(150,-150); // turn right
delay(450);
drive.set(100,100); //move forward to avoid the loop
delay(1000);
}


if(((leftir.getData()>60)&&(rightir.getData()<40))&&(centerir.getData()>30)){ //Opening on the left hand side not against a wall /// this might nor be required, possibly emit
drive.set(0,0);
delay(300);
drive.set(100,100);//Get Clearance for the turn
delay(1000);
drive.set(-150,150);//Turn Left
delay(450);
drive.set(100,100);//Move into the turn
delay(1000); //Was 1000 before, changing for test
}



if((leftir.getData()<30)&&(rightir.getData()<30)&&centerir.getData()>15){ //Used for going down hallways
do{drive.set(125,125);
if(rightir.getData()<18){ //Alligns the robot in case it gets too close to the wall
do{ drive.set(125,150); //150
}while(rightir.getData()<18);
}
if(leftir.getData()<18){ //Alligns the robot in case it gets too close to the wall
do{drive.set(150,125);//150
}while(leftir.getData()<18);
}

if(leftir.getData()>30&&rightir.getData()<20&&centerir.getData()>15){
drive.set(0,0);
delay(250);
drive.set(100,100);
delay(1500); //changed from 1500
drive.set(-150,150);
delay(450);
drive.set(100,100);
delay(1500);
if(rightir.getData()>30&&leftir.getData()<30){

}
}
if(rightir.getData()>30&&leftir.getData()<30&&centerir.getData()>15){
drive.set(0,0);
delay(250);
drive.set(100,100);
delay(1500);
drive.set(150,-150);
delay(450);
drive.set(100,100);
delay(1500);
if(rightir.getData()>30&&leftir.getData()<30){
roomSearchright();
}
}



}while(centerir.getData()>15); //runs this loop until the robot hits a wall or finds another opening ||(centerir.getData()>20)
}



delay(500);

}



void roomSearchright() //Used to look around a room entered from the right side, exit left, three rotations broken up to find the flame
{

boolean follow = false;
drive.set(0,0);
delay(250);

do{ drive.set(100,100);
}while(centerir.getData()<20);
drive.set(0,0);
delay(500);

drive.set(105,-105);//Rotate Around The Room Looking for fire
delay(466);
drive.set(0,0);
delay(250);

drive.set(105,-105);//Rotate Around The Room Looking for fire
delay(466);
drive.set(0,0);
delay(250);

drive.set(105,-105);//Rotate Around The Room Looking for fire
delay(466);
drive.set(0,0);
delay(200);



drive.set(0,0);
delay(500);


if(centerir.getData()>20){
do{drive.set(100,100);
}while(centerir.getData()>20);
}

do{ drive.set(150,-150);
delay(450);
}while(centerir.getData()<20);
follow = true;





if(follow == true){

wallFollowing();
}
}







void find_flame()
{





}

void wallFollowing(){

// IR sensor returns distance in centimeters
int val = leftir.getData();





if(centerir.getData()>20){

if(val < 14){
// if we are way too close, turn away fast
drive.set(225,20);
}else if(val < 19){
// if we are too close, drift away
drive.set(200,180);
}else if(val > 25){
// too far away, turn towards wall more
drive.set(20,200);
}else{
// default, drift towards wall
drive.set(180,225);
}

}
if(centerir.getData()<20){
do{
drive.set(150,-150);
}while(centerir.getData()<20);
}



//stops here before leaving the room, imediately after the turn, write wall follopwing code



wallFollowing();
}

This code works very consistently, I have also written flame detection code below, which i have tested and it works



#include <EncodersAB.h>
#include <Motors.h>
#include <SharpIR.h>


Motors drive = Motors(); //drive motors
SharpIR rightir = SharpIR(GP2D12,3);
SharpIR leftir = SharpIR(GP2D12,1);
SharpIR centerir = SharpIR(GP2D12,2);

int r=analogRead(5);
int l=analogRead(4);




void setup()
{


Serial.begin(38400);
delay(2000);
int r=analogRead(5);
int l=analogRead(4);

pinMode(12,OUTPUT);

}

void loop()
{
int r= analogRead(5);

int l= analogRead(4);


if(r>l){
drive.set(100,150);
}else if(l>r){
drive.set(150,100);
}


if(r<50&&l<50){
drive.set(0,0);
fan_time();



}





}

void fan_time()
{
drive.set(0,0);

digitalWrite(12,HIGH);
}Now the problem im having is that i cannot combine the two, I am using two photosensors to detect the flame (No UV tron) - Basically i need it to check for the flame while doing the roomsearchright function and the wall following function, I have tried and have had no luck so far, can anyone help me combine these two bits of code?