/*
This program is for Challenge 2 of Roboticon 2005
Programmed by The Neuf
Help by Ferbinator and rest of gang
Also note that this was done rather quckly as we were in a time crunch
*/
int w_light_1,w_light_2,b_light_1,b_light_2,r_light,wait_time,loop_x,max_right_turn,max_left_turn,max_turn_time;
sub right_turn() {//this subroutine is used when we detect a red LED and need to turn
 //do the stuff to turn right
 //e.g., turn the motors different ways for x seconds
SetSensorType(SENSOR_1,SENSOR_TYPE_LIGHT);
Off(OUT_A+OUT_B);
SetPower(OUT_A+OUT_B,OUT_FULL);
OnFwd(OUT_B);
Wait(30);
Off(OUT_A+OUT_B);
//while(SENSOR_1 < 10 && SENSOR_1 > 30) {
while (SENSOR_1 >= w_light_1 && SENSOR_1 <= w_light_2) {
OnFwd(OUT_B);
Wait(10);
}
Off(OUT_A+OUT_B);
//Off(OUT_A + OUT_B);//turn off the motors
}
void go_left(int x) {//this subroutine is used to turn the robot left

OnFwd(OUT_A);
OnRev(OUT_B);
Wait(x);//time to keep the motors on, in x seconds
Off(OUT_A + OUT_B);//turn off the motors
}
void go_right(int x) {//this subroutine is used to turn the robot right
OnFwd(OUT_B);
OnRev(OUT_A);
Wait(x);//time to keep the motors on, in x seconds
Off(OUT_A + OUT_B);//turn off the motors
}
task main()
{
SetPower(OUT_A + OUT_B, 2);
w_light_1 = 30;//value for the min. value of the light sensor for the white area
w_light_2 = 45;//'' max. value
b_light_1 = 0;//min. light sensor value for the black line
b_light_2 = 29;//max. value for the black line
r_light = 46;//red line value
wait_time = 1000;//how long we should wait before we turn the light sensor back on if we detect that its a red LED light
loop_x = 0;//this variable is used by some loops in the program
max_right_turn = 10;//max number of loops we should do to see if we are going back on the black line (turning right)
max_left_turn = 5;//max number of loops we should do to see if we are going back on the black line (turning left)
max_turn_time = 1;//time motors turn on for to get back on the black line
while(true) //loops forever
{
SetSensorType(SENSOR_1,SENSOR_TYPE_LIGHT);//turns on the sensor and sets the value to SENSOR_1
if (SENSOR_1 >= b_light_1 && SENSOR_1 <= b_light_2) {//on the black line
OnFwd(OUT_A+OUT_B);//go forward
SetPower(OUT_A+OUT_B,2);
Wait(20);
}//end the black line check
else { //otherwise its not the black line so we check for others
if (SENSOR_1 >= r_light) {//if its the red LED...
Off(OUT_A + OUT_B);
PlaySound (SOUND_LOW_BEEP); PlaySound (SOUND_CLICK);
//turn the light off then check again
loop_x = 0;
SetPower(OUT_A+OUT_B,OUT_FULL);
while (loop_x <= 5) {
OnFwd(OUT_A);
OnRev(OUT_B);
Wait(6);
Off(OUT_A+OUT_B);
SetSensorType(SENSOR_1,SENSOR_TYPE_NONE);//turn off the light sensor
Wait(30);
SetSensorType(SENSOR_1,SENSOR_TYPE_LIGHT);
Wait(10);
OnFwd(OUT_B);
OnRev(OUT_A);
Wait(6);
Off(OUT_A+OUT_B);
SetSensorType(SENSOR_1,SENSOR_TYPE_NONE);//turn off the light sensor
Wait(30);
SetSensorType(SENSOR_1,SENSOR_TYPE_LIGHT);
Wait(10);
Off(OUT_A+OUT_B);
loop_x += 1;
}
loop_x = 0;
if (SENSOR_1 >= r_light) {//if its the red LED...
//LED is still on, so we are done
PlaySound (SOUND_DOUBLE_BEEP);
PlaySound (SOUND_DOUBLE_BEEP);
PlaySound (SOUND_DOUBLE_BEEP);
PlaySound (SOUND_DOUBLE_BEEP);
PlaySound (SOUND_DOUBLE_BEEP);
PlaySound (SOUND_DOUBLE_BEEP);
return;
} else { //else the LED is off so turn
right_turn();
}//end the check to see if we are done the maze

} else {//not black line and not the red light so we must be off the line

//lets do the things to get back on
//our robot is more likely to turn right so we will try turning left first
loop_x = 0;
while(loop_x <= max_left_turn || (SENSOR_1 <= w_light_1 && SENSOR_1 >= w_light_2)) {//turn left until either we get on the black line or until the 'fail safe is reached'
SetPower(OUT_A+OUT_B,OUT_FULL);
loop_x += 1;
go_right(max_turn_time);
//SetUserDisplay(loop_x,0);
}//end the left while loop
go_right(5);
if (SENSOR_1 >= w_light_1 && SENSOR_1 <= w_light_2) {//if we are still in the white, turn right, but we have to compensate for the left turn
loop_x = 0;
while(loop_x <= max_right_turn || (SENSOR_1 <= w_light_1 && SENSOR_1 >= w_light_2)) {//turn left until either we get on the black line or until the 'fail safe is reached'
SetPower(OUT_A+OUT_B,OUT_FULL);
loop_x +=1;
go_left(max_turn_time);
}//end the right while loop
go_left(5);
} else {//if we are still in the white zone
OnFwd(OUT_A+OUT_B);//move the robot forward
Wait(2);//waits
SetPower(OUT_A+OUT_B,2);
}//end the we found the black light thing
}//end the other light check
}//end the else for the black line




}//end the while loop
}//main task()