In my final year of university, one of the projects I had to do was to make a robot that can navigate through a maze to find and put out simulated fires. We had a set microcontroller to use (Freescale's HCS12 Microcontroller) and then we were free to use whatever components we wanted to complete the task.
Not the neatest looking robot, but it did its job.
Then came the motors and controlling the motors. I used a Solarbotics Compact L298 Motor driver as it was small, and was able to control 2 motors. Since I was using LEGO for the body, I decided to use Lego RCX motors. To connect the LEGO motors to the motor driver (since LEGO motors used LEGO connections to the LEGO Microcontroller normally, I took one of the wires that connect to the motor, cut it in half, and stripped the ends. This gave me 2 wires which I could then use to connect to the motor driver for forward and reverse of the motor. The motor driver had 5 pins connected to the microcontroller which were the enable, and the forward and reverse for each motor. The motor's had a separate power supply from the microcontroller so the microcontroller wouldn't be affected if the motors drew too much power.
Now that the robot can move, I had to deal with navigating a maze and how to sense if there is a wall or not. I ended up using digital distance sensors. The one is used was the Sharp GP2Y0D810Z0F IR 2 - 10cm Range sensor. The reason I chose this was that using a digital sensor greatly simplified coding as all I had to do in the code was to see if the sensor produced a high or low value. The sensors attached to an input pin for the microcontroller and then the 5V source of the microcontroller and the ground.
The robot used 3 of these sensors. It had one on the front to detect if there was a wall in front of it and it used 2 facing the left side. The sensors were on the left side because the method I chose of navigating the maze was to use the left-hand wall following rule. The robot would continually follow the maze on the left hand side. The robot would continually turning a slight left when it cannot detect a wall on the left and would go straight once it did. So when there is an opening on the left, the robot would turn left into the opening until it found the wall again. The reason there are 2 sensors facing left was to make sure that the robot was not turning left and running into the wall and getting stuck. The two left facing sensors where offset a small distance so when the robot is running straight, only one sensor would trigger, but if the robot was running at an angle towards the wall, both sensors would trigger and the robot would make a slight adjustment to go back straight.
Next was detecting fires. I originally ordered a premade digital fire sensor a month before the project's due date, but it ended up arriving a week after it was due. Fortunately, I realized the fire sensor was most likely an infrared detector as fires produce a large amount of infrared light so I made my own following a guide on making a light/dark line following circuit that used IR sensors.
The circuit uses a LM324N comparator opamp. The detector in the circuit was an IR receiver which would pass through the opamp. If there was a fire or IR source, then the opamp would produce a high value which could then be read by the microcontroller. If there wasn't it would produce a low value. The circuit was mounted on the top of the robot.
The IR receiver was wrapped with heat shrink to focus where it detected IR because without it, the sensor would detect IR from over head lights or sunlight and would trigger when its not supposed to.
The last part of the robot was the mechanism for putting out the fire. We were not allowed to use liquid to put out the fire so I went with a small computer fan. I could have had the fan running the entire time to put out fires, but to save battery, I had it only turn on the fan when fire was detected. The fan ran off of the motor's batteries so I used a relay to allow the microcontroller to control the fan.
The relay was attached to an output pin of the microcontroller. To turn on the fan, the microcontroller would output a high value on that pin, which would send 5V to the relay. When the relay receives 5V, the relay would complete the circuit for the fan side allowing it to receive the 6V from the power supply, turning it on.
Pseudocode:
Initialize variables
Set motor to turn left to find wall
If (leftbumper_GetVal()==1) //If left sensor is
activated ie. It is against the wall
{
If
(leftinnerbumper_GetVal()==1) //If inner left sensor is activate ie. Too close
to wall
{
Turn right
}
Else
{
Go forward
}
}
If (leftbumper_GetVal()==0) //If left sensor isnt
activated ie. Not against the wall
{
Go left to find wall again
}
If (frontbumper_GetVal()==1) //If front bumper is
activated
{
If
(firesensor_GetVal()==1) //If the fire sensor is activated ie. If it detects a
fire
{
Stop,
turn on fan, delay, turn off fan
Turn 90 degrees, go forward a
bit, resume going to left to find wall
}
Else
{
Turn 90 degrees, go forward a
bit, resume going to left ot find wall
}
}
Repeat
The code for the robot is designed to be simple.
The program runs on an endless loop performing one task then the next. The
robot first checks if it is against the left wall. If it is, then it checks if
it is too close to the wall through the inner side sensor, if it is it turns
right slightly. If it isnt too close to the wall, it goes forward along the
wall. If it isnt against the wall, then it turns left to find the wall. Then it
checks if the front bumper is activated meaning that the robot has run into the
wall. If it has, then it checks the fire sensor. If the fire sensor is
activated then the robot stops and turns the fan on for a set amount of time,
then turns it off, turns to the right, goes forward a bit, then resumes turning
left to find the wall. If no fire is detected then the robot just turns to the
right and goes forward. The actual program, shown below, has
redundancies programmed in in case the robot gets stuck in one part of the
program.
Code:
int test;
long count;
void Delay(long val)
{
long i = 0;
for (i = 0; i < val; i++) {}
}
//I1 = RIGHT back -> Turn right
//I2 = RIGHT fwd -> Turn left
//I3 = LEFT back -> Turn left
//I4 = LEFT fwd -> Turn right
//GO FORWARD
void Fwd()
{
I1_SetVal();
I2_ClrVal();
I3_SetVal();
I4_ClrVal();
}
//GO BACKWARD
void Back()
{
I1_ClrVal();
I2_SetVal();
I3_ClrVal();
I4_SetVal();
}
//TURN LEFT
void Left()
{
I1_ClrVal();
I2_ClrVal();
I3_SetVal();
I4_ClrVal();
}
//TURN RIGHT
void Right()
{
I1_SetVal();
I2_ClrVal();
I3_ClrVal();
I4_ClrVal();
}
//STOP
void Brake()
{
I1_SetVal();
I2_SetVal();
I3_SetVal();
I4_SetVal();
}
//PIVOT LEFT
void Pleft()
{
I1_ClrVal();
I2_SetVal();
I3_SetVal();
I4_ClrVal();
}
//PIVOT RIGHT
void Pright()
{
I1_SetVal();
I2_ClrVal();
I3_ClrVal();
I4_SetVal();
}
void main(void)
{
/* Write your local variable definition here */
/*** Processor Expert internal initialization. DON'T REMOVE THIS CODE!!! ***/
PE_low_level_init();
/*** End of Processor Expert internal initialization. ***/
/* Write your code here */
test = 0; //Arbitrary variable for continous loop purposes
Enable_SetVal(); //Enable motors
//START
Left(); //Turn LEFT to find wall
while (test < 1){ //Infinite Loop
while(IRS_GetVal() == 0) //While left sensor is triggered/following wall
{
if (IRS2_GetVal() == 0) //If the inner left sensor is triggered ie. robot veering too close to wall
{
Right(); // Slight right to turn away from wall
Delay(500);
}
else
{
Fwd(); //Go forward
}
if(IRF_GetVal() == 0) //Front bumper triggers while left one is still on
{
if (Fire_GetVal() !=0) //If the fire sensor is triggered while the front bumper is triggered
{
Back(); //Back short period of time to stop robot momentum in front of fire
Delay(5000);
Brake(); //Brake to stop robot
Fan_SetVal(); //Turn on fan
Delay(100000);
Fan_ClrVal(); //Stop fan
Pright(); //Turn right
Delay(20000); //90 degrees
}
else
{
Back();
Delay(500);
Pright(); //Turn right
Delay(20000); //90 degrees
//Fwd(); //Keep going
//Delay(30000);
}
}
else //If front bumper not triggered (i.e. no wall)
{
Fwd(); //Keep going
}
}
while(IRS_GetVal() != 0){ //Left bumper is not triggered/following wall
{
if(IRF_GetVal() == 0) //Front bumper triggers while left one is off
{
if (Fire_GetVal() !=0) //If the fire sensor is triggered while the front bumper is triggered
{
Back(); //Back short period of time to stop robot momentum in front of fire
Delay(5000);
Brake(); //Brake to stop robot
Fan_SetVal(); //Turn on fan
Delay(100000);
Fan_ClrVal(); //Stop fan
Pright(); //Turn right
Delay(20000); //90 degrees
}
else
{
Back();
Delay(500);
Pright(); //Turn right
Delay(20000); //90 degrees
//Fwd(); //Keep going
//Delay(30000);
}
}
else{
Left(); //Keep turning left until it finds the wall
/*for (count = 0; count < 400000; count++) {
if (IRS_GetVal() ==0)
{break;}
if (count == 399000){
Right();
Delay(10000);
}
*/
if(IRF_GetVal() == 0) //Front bumper triggers while left one is still on
{
if (Fire_GetVal() !=0) //If the fire sensor is triggered while the front bumper is triggered
{
Back(); //Back short period of time to stop robot momentum in front of fire
Delay(5000);
Brake(); //Brake to stop robot
Fan_SetVal(); //Turn on fan
Delay(100000);
Fan_ClrVal(); //Stop fan
Pright(); //Turn right
Delay(20000); //90 degrees
}
else
{
Back();
Delay(500);
Pright(); //Turn right
Delay(20000); //90 degrees
}
}
}
}
}
}
No comments:
Post a Comment