/* Beattle Bot competition code.
Author: Arvind Harinder
File Name: 09-0193_P3_Software_robot2.c
Objective: 
grab opponent's coupler with windmill
grab opponent's cups with orange and green tribbles
push everything plus anything left in the middle of the board(botguy) up to our peak
*/ 

#define armUp
#ifndef visionchannel
#define visionchannel 3
#endif

void drive(int lpower,int rpower, long ldistance, long rdistance);
	
	
		
int main()

{
	
	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);
	}
	msleep(300L);//light callibration code with an else statement in case we need to bypass light starts
	enable_servos();	
	set_servo_position(1,1500);//opens claw so that the arm is in position to grab the coupler
	
	set_servo_position(2, 600);//opens clamp so that it will be in position to grab the turbine
	msleep(200L);drive (400, 400, 0L, -80L);//twitch to compensate for the akward angle of the starting position
	drive (740,  740, -1000L, -1000L);//jumps pipe
	drive (1300,  1300, -1000L, -1000L);//extra push incase anything got caught
	drive (1000, 1000, 0L, 1050L);//turns
	drive (1000, 1000, -900L, -900L);//align w/ pipe
	drive (1300, 1300, 1620L, 1620L);//move forward 
	drive (1000, 1000, 0L, 1100L);//turns
	drive (800, 800, -1500L, -1500L);//alligns w/ pipe
	drive (1000, 1000, 50L, 0L);// twitch in case it alligns with the edge of the pipe and throws off the bot
	drive (750, 750, 1880L, 1880L);//heads for wind turbines 
	msleep(800L);
	set_servo_position(1, 1100);//slowly closes in on the turbine
	msleep(200L);
	set_servo_position(1, 1000);
	msleep(200L);
	set_servo_position(1, 900);
	msleep(200L);
	set_servo_position(1, 700);
	msleep(400l);
	set_servo_position(1, 400);//gets the turbing
	msleep(400l);
	drive (1000, 1000, -190L, -190L);//backs up to avoid the pipe
	drive (1000, 1000, 560L, -560L);//turns right
	msleep(800L);
	set_servo_position(1, 600);//holds onto the coupler
	msleep(400l);
	set_servo_position(2, 1400);//holds onto the turbine
	msleep(200L);
	set_servo_position(1, 1600);//opens so that it can get the tribbles
	msleep(400l);
	drive (1300, 1300, 900L, 980L);//heads for the cup of tribbles
	set_servo_position(1, 550);// grabs it
	msleep(400l);
	set_servo_position(1, 300);//grabs cupler and tribbles hard enough to lif them up and cary them over the bump on the board
	msleep(400l);
	drive (1300, 800, 100L, 100L);//heads for other team's tribbles
	drive (800, 1300, 100L, 100L);// moves in a twitchy/zig zaz motion to get over the bump where the boards connect
	drive (1300, 800, 100L, 100L);
	drive (800, 1300, 100L, 100L);
	drive (1300, 800, 100L, 100L);
	drive (800, 1300, 100L, 100L);
	drive (1300, 800, 100L, 100L);
	drive (800, 1300, 100L, 100L);
	drive (1300, 800, 100L, 100L);
	drive (800, 1300, 100L, 100L);
	drive (1300, 800, 100L, 100L);
	drive (800, 1300, 100L, 100L);
	drive (1300, 800, 100L, 100L);
	drive (800, 1300, 100L, 100L);
	drive (1300, 800, 100L, 100L);
	drive (800, 1300, 100L, 100L);
	drive (1300, 800, 100L, 100L);
	drive (800, 1300, 100L, 100L);
	drive (1300, 800, 100L, 100L);
	drive (800, 1300, 100L, 100L);
	drive (1300, 800, 100L, 100L);
	drive (800, 1300, 100L, 100L);
	drive (1300, 800, 100L, 100L);
	drive (800, 1300, 100L, 100L);
	drive (1300, 800, 100L, 100L);
	drive (800, 1300, 100L, 100L);
	drive (1300, 800, 100L, 100L);
	drive (800, 1300, 100L, 100L);
	drive (1300, 1300, -100L, -100L);//makes sure to avoid the other team's starting box pipe
	drive (740,  740, 0L, 200L);//compensates for the bump where the boards meet
	drive (600, 600, 600, -600);//turns right, facing our peak
	set_servo_position(1, 500);//holds onto the coupler
	msleep(400l);
	drive (1000, 1000, -1500, -1500);//alligns
	drive (600, 630, 3500, 3500);//heads to the peak
	drive (1000, 1000, 0L, 1200L);//turns away from the pipe at the valley 
	drive (600, 600, 1000, 1000);//moves to avoid the pipe
	drive (1000, 1000, 1200L, 0L);//turns to face the peak
	drive (1000, 1000, 4000L, 4000L);//goes into our peak
	disable_servos();

	
	
	disable_servos();
	
	
	
}



void drive(int lpower,int rpower, long ldistance, long rdistance)
{
	mrp(1,lpower,ldistance); 
	mrp(2,rpower,rdistance); 
	bmd(1);
	bmd(2);


	
}