//MOTOR PORTS
#define LEFT_MOTOR 3
#define RIGHT_MOTOR 1
#define fan 2
//FLOATING ANALOG - ET
#define look_right 17
#define look_forward 18
#define look_left 16
//DIGITAL PORTS
#define switch 14
//ANALOG PORTS, IR & PHOTODIODES
#define IR 3
#define IR_verify 4
#define RIGHT_EYE 5
#define LEFT_EYE 6
//CODE FOR FLAME DETECTOR
#use hb_uv15.icb
/*
* Test of UVTron interrupt routine
* for handy board.
*/
void flame_detector()
{
/* set sampling rate of 250ms */
uv_stime=250;
while(1)
{
if (uv_count) {
printf("FIRE!\n");
beep();
} else {
printf(" \n");
}
// msleep(250L);
sleep(0.1);
}
}
//CODE USED IN NAVIGATION
#define forward() {motor(LEFT_MOTOR, 60); motor(RIGHT_MOTOR, 60);}
#define backward() {motor(LEFT_MOTOR, -60); motor(RIGHT_MOTOR, -60);}
#define right() {motor(LEFT_MOTOR, 60); motor(RIGHT_MOTOR, -30);}
#define left() {motor(LEFT_MOTOR, -30); motor(RIGHT_MOTOR, 60);}
#define sharp_left() {motor(LEFT_MOTOR, -60); motor(RIGHT_MOTOR, 60);}
//NAVIGATION
// three state approach; from Lab 4
void navigate_right_wall()
{
sleep(2.); //give user time to setup
while (1)
{
//state 3 - if between values 99 - 65, go forward along wall
if ((analog(look_right) < 75) && (analog(look_right) >= 60))
{
printf("State 3; lr=%d f=%d\n", analog(look_right), analog(look_forward));
forward();
msleep(50L);
ao();
}
//state 1 - if ETa reports values too close to wall
else if (analog(look_right) >= 75)
{
printf("State 1; lr=%d f=%d\n", analog(look_right), analog(look_forward));
left();
msleep(50L);
ao();
}
//state 2 - if ETa reports values too far from wall
else if (analog(look_right) < 60)
{
printf("State 2; ETa=%d ETb=%d\n", analog(look_right), analog(look_forward));
right();
msleep(50L);
ao();
}
if (digital(switch) == 1)
{
backward();
msleep(100L);
left();
msleep(100L);
ao();
}
//Do not run into the wall!!!!
if (analog(look_forward) >= 40)
{
printf("Steering clear! lr=%d f=%d\n", analog(look_right), analog(look_forward));
// beep();
sharp_left();
msleep(300L);
ao();
}
}
}
void seek_light()
{
while(1)
{
//while the right sensor gets more light...
if (analog(RIGHT_EYE) < analog(LEFT_EYE))
{
//turn right, towards light
motor(LEFT_MOTOR, 30); //turns wheel fwd
motor(RIGHT_MOTOR, 0);
msleep(50L);
}
//while the left sensor gets more light...
if (analog(RIGHT_EYE) > analog(LEFT_EYE))
{
//turn left, towards light
motor(RIGHT_MOTOR, 30); //turns wheel fwd
motor(LEFT_MOTOR, 0);
msleep(50L);
}
//while both sensors receive equal amounts of light...
if (analog(RIGHT_EYE) == analog(LEFT_EYE))
{
//go forward, towards light
motor(LEFT_MOTOR, 30);
motor(RIGHT_MOTOR, 30);
msleep(50L);
}
}
}
//CODE INVOLVING THE FAN
void fan_on()
{
motor(fan, 100);
}
void rotate_fan()
{
while (uv_count)
{
//start in middle, goes right, goes full left, back to center
servo0 = 2250;
sleep(1.);
servo0 = 2500;
sleep(1.);
servo0 = 2750;
sleep(1.);
servo0 = 3000;
sleep(1.);
servo0 = 2750;
sleep(1.);
servo0 = 2500;
sleep(1.);
servo0 = 2250;
sleep(1.);
servo0 = 2000;
sleep(1.);
servo0 = 1750;
sleep(1.);
servo0 = 1500;
sleep(1.);
servo0 = 1750;
sleep(1.);
servo0 = 2000;
sleep(1.);
}
}
void fan_off()
{
motor(fan, 0);
}
void put_out_flame(int pid)
{
int pid, pid_rotate;
forward();
sleep(1.);
//seek_light while not on white circle
pid = start_process (seek_light());
while (1)
{
if ((analog(IR) <= 50) && (analog(IR_verify) <= 50))
{
kill_process(pid);
break;
}
}
ao();
//turn on and rotate fan until flame is out...
while(1)
{
if (uv_count)
{
fan_on();
pid_rotate = start_process(rotate_fan());
while(1)
if (!uv_count)
{
kill_process(pid_rotate);
break;
}
}
else
{
fan_off();
break;
}
}
sleep(2.);
//check to make sure flame is really out- JUST IN CASE
//turn on and rotate fan until flame is out...
while(1)
{
if (uv_count)
{
fan_on();
pid_rotate = start_process(rotate_fan());
while(1)
if (!uv_count)
{
kill_process(pid_rotate);
break;
}
}
else
{
fan_off();
break;
}
}
kill_process(pid); //stop beeping from flame detector
}
void main()
{
int pid, pid_flame;
sleep(2.); //give user time to setup
init_expbd_servos(1);
//start flame detecting code; give audio cue if flame is present
pid_flame = start_process(flame_detector());
/* //SKIPPING OVER ONION ROOM
//start robot navigating right wall without looking at left or front sensor
//allows robot to "skip" over perpendicular corridor
pid = start_process(navigate_right_wall());
sleep(30.);
kill_process(pid);
//STATE 1: Go to top right room and check for flame
//start robot navigating right wall without looking at left or front sensor
//allows robot to "skip" over perpendicular corridor
pid = start_process(navigate_right_wall());
sleep(30.);
kill_process(pid);
//navigate_right_wall enables the robot to visits 3 of the 4 rooms...
pid = start_process(navigate_right_wall());
//stay here until we get into room A
while(1)
{
//now to tackle the onion roon...
if ((analog(look_forward) >= 20) && (analog(look_left) <= 25) && (analog(look_left) >= 15))
{
kill_process(pid);
sharp_left();
msleep(500L);
ao();
while (analog(IR) > 200)
forward();
ao();
beep();
break;
}
}
if (uv_count) //if candle is in room, put_out_flame
put_out_flame(pid_flame);
else //otherwise, backup, turn, and keep searching
{
//STATE 2 - all other rooms
while (analog(look_forward) > 16)
backward();
right_turn();
//not in room A, so check other 3 rooms
pid = start_process(navigate_right_wall());
//keep navigating until we find the candle
while(1)
{
if (uv_count) //if candle is in room, stop navigating and put_out_flame
{
kill_process(pid);
put_out_flame(pid_flame);
break;
}
}
}
*/
//CHECK OTHER 3 ROOMS
pid = start_process(navigate_right_wall());
//keep navigating until we find the candle
while(1)
{
if (uv_count) //if candle is in room, stop navigating and put_out_flame
{
kill_process(pid);
put_out_flame(pid_flame);
break;
}
}
printf("Fire's Out!\n");
set_beeper_pitch(1000.);
beeper_on();
sleep(1.);
beeper_off();
ao(); //shut all motors off
}