/* Battle Bot competition code.
Author: Arvind Harinder
Objective: wreck havock on the other team's side, and then take the cups and tribbles back to ours*/

#define armUp

void drive(int lpower,int rpower, long ldistance, long rdistance);
	long mseconds()
	{
	 return(long)(seconds() * 1000.); 
	}
		
int main()
{
	drive (700, 700, -2000L, -2000L);//jumps pipe
	drive (1000, 1000, 0L, 1050L);//turns
	drive (1000, 1000, -700L, -700L);//align w/ pipe
	drive (1000, 1000, 1500L, 1500L);//move forward 
	drive (1000, 1000, 0L, 1100L);//turns
	drive (1000, 1000, -700L, -700L);//alligns w/ pipe
	drive (1000, 1000, 5400L, 5400L);//heads for other team's peak 
	mrp(0, 700, 1000L);// messes up their turbine landing stations
	msleep(300L);
	mrp(0, 700, -1000L);// messes up their turbine landing stations
	msleep(300L);
	drive (1000, 1000, 500L, 0L);//messes up the landing station for the turbines
	drive (1000, 1000, 0L, 500L);//messes up the landing station for the turbines
	drive (1000, 1000, -2000L, -2000L);//backs up
	set_servo_position(1,210);// messes up opponent's tribbles and picks them up in the process
    	mav(1000,-1000); mav(1000,-1000);// moves back to middle of the board
   	 //return(long) seconds *1000.);
	long endtime = mseconds() + 8000L;  // Set end time
    	while (mseconds()<endtime) {
    	if (analog(5)>170) break;// using the IR sensor, the bot will see the black line
    	} 
	
	drive (1000, 1000, 2100L, 0L);// after seeing the black line it turns
	drive (800, 1000, 2100L, 2100L); //heads to the middle of the board
	drive (1000, 1000, 0L, 800L); //turns left
    	drive (1000, 1000, 3000L, 3000L); //heads for cups w/ orange and green balls 
	drive (1000, 1000, 1100L, 0L);//turns right 
	drive (1000, 1000, 0L, 1100L);//takes everything back to the peak
	
   	return 0;
}

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