#define LIFT servo0 /*used to change axis of movement*/
#define LEFT 0 /*left X motor on motor 0*/
#define RIGHT 2 /*right X motor on motor 1*/
#define YMOTOR 1 /*Y motor on motor 2*/
#define FAN 3 /*motor port for fan*/
#define MOTSPEED 100 /*motor speed*/
#define X_LIFT_VALUE 4100 /*value for LIFT to move on x axis*/
#define Y_LIFT_VALUE 0 /*value for LIFT to move on y axis*/
#define FRANGE GP2D02_11 /*analog port for the first front range finder*/
#define LRANGE GP2D02_15 /*analog port for the left range finder*/
#define RRANGE GP2D02_10 /*analog port for the right range finder*/
#define RRANGE2 GP2D02_14 /*the second right range finder*/
#define BRANGE GP2D02_13 /*analog port for the back range finder*/
#define LINESENSE analog(4)
#define SENSTRAIGHT 2150 /*straight value for IR sensor servo*/
#define IRSENSOR analog(3) /*analog port for IR sensor*/
#define SENSERVO servo1 /*servo port for IR sensor servo*/
#define LINEFOUND 10
#define OFF 0
int lposition,lreading=200,turn;
/*
BEX.C - an interactive learning experience
Design team - Marc Friedenberg, Peter Kay, Dan McAdams
*/
void main()
{
init_expbd_servos(1);
GP2D02_enable(1);
SENSERVO=4100;
while (!start_button())
{
/*if (analog(2)>=10)
{
printf("I hear you!\n");
sleep(4.);
go_room_4();
}*/
if (stop_button())
{
test();
}
}
while (start_button())
{
if (stop_button())
{
self_check();
break;
}
}
go_room_4();
}
void motor_kill()
{
motor(YMOTOR,MOTSPEED*-1+30);
sleep (.15);
motor(YMOTOR,OFF);
}
void back_motor_kill()
{
motor(YMOTOR,MOTSPEED-30);
sleep(.15);
motor(YMOTOR,OFF);
}
void go_forward()
{
LIFT=Y_LIFT_VALUE;
sleep(1.);
motor(YMOTOR,MOTSPEED);
}
void go_slow_forward()
{
LIFT=Y_LIFT_VALUE;
sleep(1.);
motor(YMOTOR,50);
}
void go_back()
{
LIFT=Y_LIFT_VALUE;
sleep(1.);
motor(YMOTOR,MOTSPEED*-1);
}
void go_left()
{
LIFT=X_LIFT_VALUE;
sleep(1.);
motor (RIGHT,75);
motor (LEFT,-75);
sleep(.42);
motor(RIGHT,OFF);
motor(LEFT,OFF);
right_align();
}
void go_right()
{
LIFT=X_LIFT_VALUE;
sleep(1.);
motor (RIGHT,-75);
motor (LEFT,-5);
sleep(.4);
motor(RIGHT,OFF);
motor(LEFT,OFF);
}
void celebrate()
{
beep();
beep();
beep(); /*The celebration routine*/
beep();
beep();
}
void blow_out_candle()
{
int angle,lreading;
motor(FAN,100);
for (angle=0;angle<=4100;angle+=65)
{
SENSERVO=angle;
if (IRSENSOR<=lreading)
{
lreading=IRSENSOR;
}
sleep(.05);
}
check_success();
motor(FAN,OFF);
celebrate(); /*beep 5 times*/
}
void center_me()
{
LIFT=X_LIFT_VALUE;
if (LRANGE<165)
{
motor(LEFT,-50);
motor(RIGHT,-50);
while (LRANGE<165) {}
motor (LEFT,50);
motor(RIGHT,50);
sleep(.3);
motor(LEFT,OFF);
motor(RIGHT,OFF);
}
}
int check_success()
{
int angle,check_reading=1000;
for (angle=4100;angle>=0;angle-=65)
{
SENSERVO=angle;
sleep(.05);
if (IRSENSOR<=check_reading)
{
check_reading=IRSENSOR;
}
}
if (check_reading>=100)
{
check_success();
}
}
int candle_in_room()
{
int angle,reading;
lreading=200;
for (angle=4100;angle>=0;angle-=75) /*decrement the sensor/fan servo from 4100 to 0*/
{
SENSERVO=angle; /*turn the servo to the specified angle*/
sleep(.1); /*wait .1 seconds to allow the servo to turn*/
if (IRSENSOR<=lreading) /*if the current reading of the sensor is lower than lowest recorded value..*/
{
lreading=IRSENSOR; /*set the lowest reading to the current reading*/
lposition=angle; /*set the lowest reading position to the current position*/
}
}
printf("Value %d at %d\n",lreading,lposition); /*print the lowest reading and position*/
turn=abs(lposition-SENSTRAIGHT)/300;
if (lreading<=20)
{
return 1;
}
else
{
return 0;
}
}
void scan_for_lowest()
{
int angle,reading;
printf("LPOS %d,TURN %d\n",lposition,turn);
if (turn!=0)
turn+=3;
motor(YMOTOR,OFF);
encoder0_counts=0;
LIFT=X_LIFT_VALUE;
sleep(2.);
if (lposition<SENSTRAIGHT) /*candle on right*/
{
printf("Candle on right\n");
motor(LEFT,55);
motor(RIGHT,-55);
while (encoder0_counts<=turn) {}
motor(LEFT,OFF);
motor(RIGHT,OFF);
}
if (lposition>SENSTRAIGHT) /*candle on left*/
{
printf("Candle on left\n");
motor(LEFT,-55);
motor(RIGHT,55);
while (encoder0_counts<=turn) {}
motor(LEFT,OFF);
motor(RIGHT,OFF);
}
go_forward();
go_until_paper();
blow_out_candle();
}
void go_until_line()
{
while (1)
{
if (LINESENSE<=LINEFOUND)
{
beep();
motor_kill();
}
}
}
void go_until_paper()
{
while (1)
{
if (LINESENSE<=LINEFOUND)
{
sleep(.15);
if (LINESENSE<=LINEFOUND)
{
motor_kill();
break;
}
}
}
}
void right_align()
{
}
void self_check()
{
int mot;
for (mot=0;mot<=3;mot++) /*test the motors*/
{
motor(mot,50);
}
sleep(.05);
for (mot=0;mot<=3;mot++)
{
motor(mot,0); /*turn off the motors*/
}
LIFT=2000;
sleep(.5); /*test the lift servo*/
LIFT=2500;
SENSERVO=2000;
sleep(.5); /*test the IR/fan servo*/
SENSERVO=2500;
if ((LINESENSE==0)||(LINESENSE==255))
{
celebrate();
printf("Bad LINESENSE! -4\n");
sleep(5.);
self_check();
}
if ((IRSENSOR==0)||(IRSENSOR==255))
{
celebrate();
printf("Bad IRSENSOR! -6\n");
sleep(5.);
self_check();
}
if ((FRANGE==0)||(FRANGE==255)||(FRANGE==1))
{
celebrate();
printf("Bad FRANGE -11!\n");
sleep(5.);
self_check();
}
if ((BRANGE==0)||(BRANGE==255)||(BRANGE==1))
{
celebrate();
printf("Bad BRANGE! -12\n");
sleep(5.);
self_check();
}
if ((LRANGE==0)||(LRANGE==255)||(LRANGE==1))
{
celebrate();
printf("Bad LRANGE! -15\n");
sleep(5.);
self_check();
}
if ((RRANGE==0)||(RRANGE==255)||(RRANGE==1))
{
celebrate();
printf("Bad RRANGE! -10\n");
sleep(5.);
self_check();
}
printf("Test passed!\n"); /*give the user a message*/
while (!start_button()) {}
while (start_button()) {}
printf("Wish me luck!\n");
}
void go_room_4()
{
go_forward();
while (FRANGE<=150);
motor_kill();
sleep(1.);
go_left();
sleep (1.);
go_forward();
sleep(1.3);
while (LRANGE>=110) {}
sleep(.35); /*go a little further after seeing the door to room 4*/
motor_kill();
go_left();
sleep(1.);
go_forward();
sleep(.8);
motor_kill();
if (candle_in_room())
{
celebrate();
scan_for_lowest();
}
else
{
go_back();
while (BRANGE<=165) {}
back_motor_kill();
sleep(1.);
go_right();
celebrate();
go_room_3();
}
}
void align_right()
{
LIFT=X_LIFT_VALUE;
if (RRANGE<RRANGE2)
{
printf("I'm tilted left %d %d\n",RRANGE,RRANGE2);
sleep(2.);
motor(RIGHT,-65);
motor(LEFT,65);
while (RRANGE<RRANGE2) {}
motor(RIGHT,OFF);
motor(LEFT,OFF);
}
if (RRANGE>RRANGE2)
{
printf("I'm tilted right %d %d\n",RRANGE,RRANGE2);
sleep(2.);
motor(RIGHT,65);
motor(LEFT,-65);
while (RRANGE>RRANGE2) {}
motor(RIGHT,OFF);
motor(LEFT,OFF);
}
}
void go_room_3()
{
go_forward();
while (FRANGE<120) {}
motor_kill();
sleep(1.);
go_left();
sleep(1.);
go_forward();
while (RRANGE>=100) {}
sleep(.35);
motor_kill();
sleep(1.);
go_right();
sleep(1.);
go_forward();
while (FRANGE<=140) {}
motor_kill();
sleep(1.);
go_right();
go_slow_forward();
go_until_line();
if (candle_in_room())
{
celebrate();
scan_for_lowest();
}
else
{
go_back();
while (BRANGE<=165) {}
back_motor_kill();
sleep(1.);
go_right();
celebrate();
go_room_1();
}
}
void go_room_1()
{
go_forward();
while (RRANGE>=100);
sleep(.3);
motor_kill();
sleep(1.);
go_right();
sleep (1.);
go_forward();
while (FRANGE<=105) {}
motor_kill();
go_left();
sleep(1.);
go_slow_forward();
go_until_line();
sleep(1.);
if (candle_in_room())
{
celebrate();
scan_for_lowest();
}
else
{
go_back();
while (BRANGE<=165) {}
back_motor_kill();
sleep(1.);
go_right();
celebrate();
go_room_2();
}
}
void go_room_2()
{
go_forward();
while(RRANGE>=150) {}
sleep(.3);
motor_kill();
sleep(1.);
go_right();
sleep(1.);
go_slow_forward();
go_until_line();
scan_for_lowest();
}
int abs(int num)
{
if (num>0)
return num;
else
return num*-1;
}
void test()
{
go_forward();
go_until_line();
while (1)
{
printf("%d\n",LINESENSE);
sleep(.2);
}
if (candle_in_room())
{
celebrate();
scan_for_lowest();
}
else
printf("No candle!\n");
}