//Lockheed Martin Explorer Post 1010 Create Code
//Author: Eliot Rudnick-Cohen
//File Name: 09-0193_P3_Software_robot1.c
/*
Purpose: to grab botguy and take the green and fossil fuel containers and any water balls
we come across to the peak and also to place the wind turbine we start with in a foundation up at the peak
*/
//Team 09-193
//Claw info:  Servo port 1, values from 850(open) to 
//1500(closed with botguy, 1900 completely closed)
//1950 closed wind claw

/*
The following values define the ports used for the claws and the values which are their up, down,
open and closed positions
*/
int clawP = 1;
int opened = 850;
int closed = 1700;
int mOne = 0;
int mTwo = 3;
int topHat = 11;
int white;
int up=1300;	//Up position for wind servo
int down = 0; //Down position for wind servo
int WindClaw=2;//Port of wind claw

void getBotguy ();
void claw (int pos);
void create_mrp(int speed, long dist);
int line (int value);
void Wind_turbine(int pos);

int main () {

	create_connect();
	enable_servos();
	claw(closed);//positions claw so it does not collide with the CBC robot
	Wind_turbine(up);//moves turbine into robot
	//Light Start Routine
	//the following code starts the robot in response to the light at the start of the match
	printf("Ready...\nLight Start? (A=yes, B=no)\n");
	while (!a_button() && !b_button());
	if (a_button()) {
		printf("Calibrate\n");
		msleep(1000L);
		wait_for_light(8);
	} else if (b_button()) {
		printf("Go in 2!\n");
		msleep(2000L);
	}
	//end Light Start Routine
	shut_down_in(119);
	white = analog(topHat);//assigns value to line sensor(part of currently non-functional line sensing code)
	
	//Drive out of starting box and get botguy
	create_drive_straight(400);
	msleep(550);
	
	create_drive_straight(400);
	msleep(500);
	//Opens the grabber motor
	mrp(3,-600,300);
	//by opening the grabber motor we can ensure our obtaining of the pom containing cups!
	
	claw(opened);//Opens claw
	create_spin_CW(300);//Turns to face botguy
	msleep(730);
	getBotguy();//Gets botguy(see function getBotguy())
	create_mrp(-600,750l);//drives the create into our inclined section
	
	create_spin_CW(200);//turns to position itself so that it can drive straight into the peak
	msleep(725);
	create_stop();
	
	
	//beeps in order to indicate the phase of the code the robot is in
	beep();
	//drives forward
	create_drive(300,500);
	
	
	//the following code is our non-function line tracking code, it is included here for historical purposes
	
	//the following code currently fails epically, but may eventually be perfected to the point of being of use
	/*
	create_spin_CW(-100);//starts turning
	int a=0;
	while(a==0)//while the robot is not on the line it will spin till it reaches it
	{
		a=line(analog(topHat));//sets a to know if the suface is white or black
		}
	create_stop();
	//create_spin_CW(-200);
		msleep(150);
	//if (line == 1)	
		//on line
	//else
		//off line
	*/
	//end non-functional line tracking code
	
	
	create_mrp(-600,1200l);//drives into the peak
	create_spin_block(100,30);//turns to be able to score objects
	msleep(1000);
	//drives the robot into a position to score a wind turbine
	create_mrp(-200, 270);
	
	
	Wind_turbine(down);//puts arm down in order to score wind turbine in a foundation
	claw(opened);//drops botguy
	//end of the code
	//begin twitching in order to make sure botguy is touching the ground(and not suspended by the claw)
	create_spin_block(400,2);
	msleep(100);
	create_spin_block(-400,2);
	msleep(100);
	create_spin_block(400,2);
	msleep(100);
	create_spin_block(-400,2);
	msleep(100);
	create_spin_block(400,2);
	msleep(100);
	create_spin_block(-400,2);
	msleep(100);
	
	msleep(200);
	Wind_turbine(down);
	//the following messages indicate that the robot has finished running, this makes it easier to debug the code
	printf("i can haz cheeseburger?\n");
	printf("You can not haz cheeseburger. GTFO Grammar.\nWhat the flux?!");
	
	disable_servos();
	create_disconnect();
	return 0;
}//end robot's code

//Grabs botguy
void getBotguy () {
	
	create_mrp(-300,150l);//drives up to botguy
	create_stop();
	claw(closed);//grabs botguy
	msleep(200);
	create_spin_CW(300);//turns to get cups of fossil fuels and green energy
	msleep(850);
}//end getBotguy()

void claw (int pos) {
	
	set_servo_position(clawP,pos);
	msleep(1000);
}//end claw



// Moves the create drive base the specified number of
//  mm at the given speed in mm/sec.
void create_mrp(int speed, long dist) {
    create_drive_straight(speed);
    if (speed < 0) dist = -dist;
    // Time = Distance/Speed!!!
    msleep(dist * 1000L / (long)speed);
    create_stop();
    msleep(50L);
}
//line function(doesn't actually work, of historical significance only)
int line (int value) {
	
	if (value > white+100)
		return 1;
	else
		return 0;
}          	
//moves the position of the winbd turbine in order to score it and also to hold it up
void Wind_turbine(int pos)
{
	set_servo_position(WindClaw, pos);
	msleep(200);
}