//Midterm

#define RIGHT_SPEED 40 //OUTPUT A

#define LEFT_SPEED 44 //OUTPUT C
#define REV_SPEED 40 //OUTPUT AC REV
#define THRESHOLD 55
#define BUMP IN_1
#define RIGHTEYE IN_3
#define LEFTEYE IN_4
#define SONIC IN_2
int bound;
int linep;
int circlep;
int turn;
int a;
int b;
int c;
int d;
int e;
int circle_count;

task circle()
{
circlep=1;
a = SensorUS (IN_2);
repeat(50) //take sonic readings for 2.5 seconds and store average in
"bound"
{
Wait(50);
b = SensorUS (IN_2);
a=((a+b)/2);
}
bound = a;
while(true)
{
circlep=1;
b = SensorUS (IN_2);
while((SensorUS (IN_2) == bound)&& circle_count <5)
{
OnFwdSync(OUT_AC, 45, 0);
}

while((SensorUS (IN_2) < bound) && circle_count < 5)//steer away

{
OnFwd(OUT_C, 50);
OnFwd(OUT_A, 55);
}
while((SensorUS (IN_2) > bound) && circle_count < 5) //steer into
{
OnFwd(OUT_C, 50);
OnFwd(OUT_A, 30);
}
}
}
task line()
{
OnFwd(OUT_A, RIGHT_SPEED);
OnFwd(OUT_C, LEFT_SPEED);
while(true)
{
linep = 1;
if ((Sensor(IN_4) < THRESHOLD) && (Sensor(IN_3) > THRESHOLD)) //Correct
Left
{
OnRev(OUT_C, LEFT_SPEED);
turn = 0; //left turn
Wait(100);
until(Sensor(IN_4) > THRESHOLD);
OnFwd(OUT_A, RIGHT_SPEED);
OnFwd(OUT_C, LEFT_SPEED);
}
if ((Sensor(IN_3) < THRESHOLD) && (Sensor(IN_4) > THRESHOLD)) //Correct
Right
{
OnRev(OUT_A, RIGHT_SPEED);
turn = 1; //right turn
Wait(100);
until(Sensor(IN_3) > THRESHOLD);
OnFwd(OUT_A, RIGHT_SPEED);
OnFwd(OUT_C, LEFT_SPEED);
}

}
}
task look_for_green()
{
while(true)
{
while ((Sensor(IN_4) < 60) && (Sensor(IN_4) > 50) && (Sensor(IN_3) <
60) && (Sensor(IN_3) > 50))
{
stop line;
linep=0;
OnFwd(OUT_A, RIGHT_SPEED);
OnFwd(OUT_C, LEFT_SPEED);
Wait(1000);
circle_count=0;
Off(OUT_AC);
e += 1;
if (e==2)
{
stop line;
stop circle;
PlayToneEx(294,400,3,FALSE); Wait(750);
PlayToneEx(370,400,3,FALSE); Wait(750);
PlayToneEx(294,400,3,FALSE); Wait(750);
PlayToneEx(880,400,3,FALSE); Wait(750);
PlayToneEx(1175,1600,3,FALSE); Wait(2500);
StopAllTasks();
}
start line;
stop look_for_green;
}
}
}

task count()
{
while(true)
{
if(Sensor(IN_3)<THRESHOLD) c=1;
if((Sensor(IN_3)>THRESHOLD) && c==1)
{
circle_count += 1;
c=0;
}
if (circle_count == 5)
{
Off(OUT_AC);
d += 1; //counts first time navigating can
a = 0;
b = 0;
start look_for_green;
Wait(50);
stop circle;
Wait(50);
circlep=0;
start line;
stop circle; //redundant
stop count;
}
}
}
task turnaround()
{
while(true)
{
if(SENSOR_1==1)
{
if (e==1)
{
d=0;
}
stop line;

linep = 0;
OnRev(OUT_AC, REV_SPEED);
Wait(300);
Off(OUT_AC);
Wait(250);
OnRev(OUT_C, LEFT_SPEED);
OnFwd(OUT_A, RIGHT_SPEED);
Wait(650);
Off(OUT_AC);
Wait(250);
if ((circle_count < 5) && (d == 0))
{
start circle;
}
if ((circle_count < 5) && (d == 0))
{
start count;
}
}
}
}
task display_values()
{
while(true)
{
TextOut(1, LCD_LINE1, "a " );
NumOut(25, LCD_LINE1, a);
TextOut(50, LCD_LINE1, "b " );
NumOut(75, LCD_LINE1, b);
TextOut(1, LCD_LINE2, "bound " );
NumOut(40, LCD_LINE2, bound);
TextOut(1, LCD_LINE3, "circle_count " );
NumOut(80,LCD_LINE3, circle_count);
TextOut(1, LCD_LINE4, "d " );
NumOut(25, LCD_LINE4, d);
TextOut(50, LCD_LINE4, "e " );
NumOut(75, LCD_LINE4, e);
TextOut(1, LCD_LINE5, "line prg " );
NumOut(60, LCD_LINE5, linep);
TextOut(1, LCD_LINE6, "circle prg " );
NumOut(70, LCD_LINE6, circlep);

if(circle_count==5)
{
stop circle;
}
}
}

task main()
{
SetSensorLight(RIGHTEYE);
SetSensorLight(LEFTEYE);
SetSensorTouch(BUMP);
SetSensorLowspeed(SONIC);
a = 0; //for US sensor averaging
b = 0; //for US sensor averaging
c = 0;
d = 0;
e = 0;
linep = 0;
circle_count = 0;
start line;
circlep=0;
start turnaround;
start display_values;
}