#define

#define
#define
#define
#define

NEAR 23//cm, the threshold to near a abstacle

timef1 6000
timet1 2700
timef2 2000
timef3 6000

//searching location-predefined ball

int searchFlag=0; //change to 1 if finding the ball
int nearFlag=0;//change to 1 if coming close to a ball
int lenTime=0; //control the time inside the loop
task search_around()
{
Wait(2000);
while(true)
{
NumOut(0, LCD_LINE4, SensorUS(IN_4));
// go to the ball
while(true)
{
OnFwd(OUT_AC, 20);
Wait(timef2);
if(nearFlag == 1)
{
Off(OUT_AC);
break;
}
lenTime = lenTime + 1;
if(lenTime > 500)
{
break;
}
}
//grab the ball and check
OnFwd(OUT_AC,25);
Wait(timef3);
Off(OUT_AC);
OnRev(OUT_B, 25);
Wait(4000);
Off(OUT_B);
if(SENSOR_3 == 5)
{
//send it back and we are done
TextOut(20, LCD_LINE3, "Lockdown successed!");
OnRev(OUT_AC, 20);
Wait(timef3);
Off(OUT_AC);
repeat(lenTime)
{
OnRev(OUT_AC, 20);
Wait(timef2);
Off(OUT_AC);
}
OnRev(OUT_A, 30);
OnFwd(OUT_C, 30);
Wait(timet1);
Off(OUT_AC);

OnFwd(OUT_AC, 20);
Wait(timef1);
Off(OUT_AC);
break;
}
else
{
//failed
TextOut(20, LCD_LINE3, "Lockdown failed!");
break;
}
}
}
task check_dist()
{
while(true)
{
if(SensorUS(IN_4) <= NEAR)
{
TextOut(20, LCD_LINE3, "near");
ClearSensor(IN_4);
nearFlag = 1;
}
else
{
nearFlag=0;
}
}
}
task main()
{
//The interfaces of motors
//A:left B:middle C:right
//The interfaces of sensors
//IN_1:TOUCH IN_2: Color IN_3:

IN_4:Ultrasonic

//First step: go straight and turn around

OnFwd(OUT_AC, 20);
Wait(timef1);
Off(OUT_AC);
OnRev(OUT_A, 25);
OnFwd(OUT_C, 25);
Wait(timet1);
Off(OUT_AC);
//second step: go straight to get the ball and bring it back
Precedes(search_around, check_dist); //searching while forwarding
SetSensorColorFull(IN_3);
SetSensorUltrasonic(IN_4);
}