// Team 191 -- Justin
// CBC for create
// Create aims to collect cups the cups, get/intsall turbines and get botguy

void drive(int t) //drives create straight for t milliseconds
{
	create_drive_direct(-450, -500);
	msleep(t);
	create_stop();
}
void turbine() //collects a turbine
{
	set_servo_position(3,750); //lowers turbine gathering arm
	drive(700);
	set_servo_position(3,1250); //picks up turbine
	create_stop();
	msleep(200);
}
void turn_right() //turns the create 90 degrees right
{
	create_drive_direct(-470, 500);
	msleep(650);
	create_stop();	
}
void turn_left() //turns the create 90 degrees left
{
	create_drive_direct(470, -500);
	msleep(650);
	create_stop();
}
int main()
{
	int mode =0; //determines seeding or competition code
	printf("A for seeding B for competition/n");
	while(a_button()!=1&&b_button()!=1)
	{
		beep();
		msleep(100);
	}
	if(a_button())
	{
		printf("Seeding Mode");
		mode =1;
	}
	if(b_button())
	{
		printf("Competition Mode");
		mode =2;
	}
	if(mode==1) //run seeding code
	{
		// code seeding
	}
	if(mode==2) //run competition code
	{
		create_connect();
		create_full();
		enable_servos(); //enables servos to pick up turbines
		turbine();
		drive(2800);
		turn_right();
		drive(4000);
		create_disconnect(); 
		disable_servos();
	}
}