#define close 0
#define open 1

/* rotate turret to find tub */
void armscan()
{
	int i = 0;
	init_expbd_servos(1);
	for(i=1300;analog(16)<50;i = i+2)
		{
            servo1=i;
		if(i>2500)
			{
				i =1300;
			}
		}

}
/* find the wafer boat */
int findtub()
{
int left=100;
int right=100;


while(1)
	{
	go_forward(left,right);
	sleep(.01);
	if(analog(6)<15)
	{
/*	printf("\nLeft:%d",analog(6));*/
	left=0;
	right=100;
	}
	if(analog(6)>70)
	{
/*	printf("\nRight:%d",analog(6));*/
	left=100;
	right=0;
	}
	if(analog(6)>15&&analog(6)<70)
	{
/*	printf("\nStraight:%d",analog(6));*/
	left=40;
	right=40;
	}

	if(analog(17) > 100)
		{
		go_forward(0,0);
		sleep(.7);
		beep();
		while(analog(17)>90)
			{
			go_forward(45,45);
			}
		stop();

		beep();
		return(0);
		}
	}
}
/* once we have a wafer, find the tub to place it in */
int findtubtwo()
{
int left=-45;
int right=-45;

while(1)
	{
	left=right=-35;
	go_backward(left,right);

	if(analog(6)<15)
	{
	left=0;
	right=-45;
	}
	if(analog(6)>70)
	{
	left=-45;
	right=0;
	}
	if(analog(6)>15&&analog(6)<70)
	{
	left=-45;
	right=-45;
	}

	if(analog(5) > 100)
		{
		return 0;	
		}
	}
}

/* beep to acknowledge success */
void beep_func(){
	int counter = 0;
	for( counter = 0; counter < 100; counter++){
	if(counter %3 == 0){
	beep();
	}

	}

}
/* self explanitory */
void go_forward(int l, int r)
{
motor(0,0-r);
motor(1,0-l);
}

void go_backward(int x,int y)
{
motor(0,0-x);
motor(1,0-y);
}

void stop(){
off(0);
off(1);

}
/* open up the pneumatic claw*/
int open_claw(int status)
{
if(status==close)
	{
	motor(2,-75);
	sleep(1.3);
	off(2);
	}
return open;
}
/* close claw */
int close_claw(int status)
{
if(status==open)
	{
	motor(2,75);
	sleep(1.3);
	off(2);
	}
return close;
}
/* start the motor to pressurize the claw tank */
void pressurize()
{
motor(3,75);

}
/* use the servo to lift the arm up */
void lift_arm()
{
int i =0;
init_expbd_servos(1);

for(i=3300;i > 2700;i = i-1)
	{
	sleep(.0005);
      servo5=i;
	}




}

void drop_arm(){
int i = 0;
init_expbd_servos(1);

for(i=2600;i < 4000;i = i+1)
	{
	sleep(.0005);
            servo5 =i;

	}


}

void stop_pressurize(){

off(3);

}
/* obsolete function */
void turn_platform()
{
int i = 0;
init_expbd_servos(1);

for(i=2700;i < 3650;i = i+10){

            servo1=i;}

}



/* robot commands */
void main(){
int i;
int clawstatus=open;

init_expbd_servos(1);
while(1)
{
servo1 = 1500;
lift_arm(); /* start with lifted arm */

pressurize();
findtub();
printf("\nanalog 16: %d", analog(16));
armscan();


drop_arm();
clawstatus=close_claw(clawstatus);
sleep(.8);
lift_arm();

stop_pressurize();

init_expbd_servos(1);
for(i=2000;i > 5;i = i-5)
	{
		sleep(.01);
            servo1=i;
	}
sleep(2.0);
printf("\n%d",analog(16));
if (analog(16)>19 && analog(16)<31)
	{
	clawstatus= open_claw(clawstatus);
	go_backward(-40,-40);
	sleep(.8);
	go_backward(0,0);

	}
if (analog(16)<10)
	{
	beep();
	findtubtwo();
	stop();
	turn_platform();
	drop_arm();
	clawstatus= open_claw(clawstatus);
	lift_arm();	
	}
else
	{
	drop_arm();
	clawstatus= open_claw(clawstatus);
	lift_arm();

	go_backward(-40,-40);
	sleep(.8);
	go_backward(0,0);
	}
}
}