/*
 * CBC Robot
 * Florence "FLOOR-E" Scorp
 * 2009
 * Revision Date: 4/11
 * 
 * Code by: Terry Sifrit
 * Purpose: Retrieve poms and take them to the peak.
 * 
 * Revision History:
 * 4/1: Captures three or four poms per run.
 * 4/4: Made adjustments to turn radii.
 * 4/6: Successfully captured six poms - best yet!
 * 4/8: Mechanical adjustments.  Changed code accordingly.
 * 4/10: Added light start.
 * 4/11: Now captures an average of six poms.
 */

// Motors:
// Left motor
#define leftmotor 0
//Right motor
#define rightmotor 1

//The use of bmd2 essentially cuts down on potential errors when employing multiple bmd functions which could cause one motor to travel farther than the other. Essentially, if one motor stops, then the other will cut off even if it would otherwise have further to travel, preventing any twitches from occuring afterwards.
void bmd2(){
	while (!get_motor_done(leftmotor) && !get_motor_done(rightmotor)) msleep(5L);
	off(leftmotor);
	off(rightmotor);
}

//The forward function essentially simplifies the use of mrp. It runs both motors at nearly the same speed (the difference compensating for imperfections in the balance of our robot) and ends with the bmd 2 function (see above) to keep them as accurate as possible. The first input is the total distance traveled, and the second input is a number to be multipled by either 770 or 800 (depending on the motor) for the speed.
void forward(int differential,int d)
{
	mrp(leftmotor, ( d * 800), differential);
	mrp(rightmotor, ( d * 770), differential);
	bmd2();
}

//The turn function works similarly to the forward function, with the simple difference being that one motor moves in reverse to turn the robot in place. Again, the first input to the function is the distance traveled by each wheel, and the second number is a number to be multipled for 500 for speed.
void turn(int differential,int d)
{
	
	mrp(leftmotor,(d * 500), differential);
	mrp(rightmotor, (d * 500), -differential);
	bmd2();
	
}

//The bulk of the code. Lines 35 through 38 are for either light start or bypassing and immediately starting (for testing purposes), followed by a failsafe shutoff one second before the match ends (provided our robot starts at the proper time).
int main()
{
	int i=0, l, r; long time;
	disable_servos();
	cbc_display_clear();
	printf("Ready...\nLightstart? (A=yes, B=no)\n");
	while (!a_button() && !b_button());
	if (a_button()) { // If a is pressed, the wait for light function is called on.
		msleep(1000L);
		wait_for_light(9);
		} else if (b_button()) { // The b button bypasses the light start, useful for testing purposes.
		printf("Go in 2!\n");
		msleep(2000L);
	}
	shut_down_in(89.);
	enable_servos();
	set_servo_position(2,0); // Opens our front claw further, so the roomba can escape more easily.
	set_servo_position(1,1800); //Keeps our robot's rear appendage at its starting position when the servos activate as to not get in the way.
	mav(leftmotor, -500); // Lines 52-54, until our rear bump sensors are simultaneously pressed, the robot will move backwards towards the pvc to line itself up.
	mav(rightmotor, -500);
	while (!digital (0) || !digital (1));
	mav(leftmotor,-250); //Lines 55-56, the robot backs up a little more to ensure alignment
	mav(rightmotor,-250);
	set_servo_position(2,300); // Lines 57-61, the front claw moves to a less clumsy position, the wheels stop moving, and the servos disable to save power.
	sleep(.5);
	off(leftmotor);
	off(rightmotor);
	disable_servos();
	mrp(leftmotor,500,200); //62-63, the robot moves forward slowly to get away from the wall it just aligned on.
	mrp(rightmotor,500,200);
	sleep(3.0);
	mrp(0,1000,1000); //The robot makes a wide right turn, preparing to back up to align on a second wall.
	bmd(0);
	mav(leftmotor, -500); //67-74, another loop to back up against the wall, with slight extra movement to make sure alignment is as close as possible.
	mav(rightmotor, -500);
	while (!digital (0) || !digital (1));
	mav(leftmotor,-250); 
	mav(rightmotor,-250);
	sleep(.5);
	off(leftmotor);
	off(rightmotor);
	forward(8000,1.25); // Robot moves out of the starting box, and prepares to go up the slope to the right (All obstacles will have been cleared by our other robot by this time).
	turn(540,1.2); // Right turn to face up the slope.
	forward(2000,1.2); // The robot begins to ascend the slope, not hitting any black lines, but getting near to align to them.
	mav(leftmotor,500); // Lines 78-81, the motors continue to move until the tophat sensor in port 8 reaches a high value, indicating it is crossing black electrical tape.
	mav(rightmotor,500);
	while(analog10 (8) < 400);
	ao();
	forward(-525,1.); //Movement in reverse...
	turn(560,1.25); //Clockwise turn, aiming the robot towards the black line running up and down the slope.
	mrp(leftmotor,500,-500); //Relatively slow movement backwards to put tophat sensors so they have not crossed the line yet.
	mrp(rightmotor,500,-500);
	bmd2();
	mav(leftmotor,250); //87-100, nested if statements which will cause the left and right motors to stop when the left and right tophat sensors reach a high value, respectively, indicating they are over black electrical tape. Once both are tripped, the loop ends.
	mav(rightmotor,250);
	l = r = 0;
	while (!l || !r) {
		if(analog10 (8) > 400){
			off(leftmotor);
			l = 1;
		}
		if(analog10 (10) >400){
			off(rightmotor);
			r = 1;
		}
	}
	ao();
	forward(-2000,1.25); //Movement backwards, towards the wall, preparing to collect poms lined up on the pvc. 
	mav(leftmotor, -500); //102-109, another backup loop identical to lines 67-74.
	mav(rightmotor, -500);
	while (!digital (0) || !digital (1));
	mav(leftmotor,-250);
	mav(rightmotor,-250);
	sleep(.5);
	off(leftmotor);
	off(rightmotor);
	enable_servos(); //Servos activate.
	set_servo_position(1,475); //The robots rear appendage falls down on the pvc, so that when the robot moves forward any poms on the pvc will be pulled along (not all ten, but as many as 8).
	set_servo_position(2,900); //Closes the front claw further, making navigation easier.
	sleep(4); // The roomba will be moving past at this point, so this robot will wait for it to pass to go up to the peak.
	forward(1500,1.25); //The robot moves forward, pulling any poms caught off the pvc pipe seperating the valley and the slope.
	mrp(leftmotor,500,1000); //115-117, the robot makes a wide turn towards the peak, keeping the poms from falling out of the rear collection arm.
	mrp(rightmotor,1000,2000); 
	bmd2();
	forward(1300,1.2); // Ascension to the peak.
	turn(500,1.2); //Point-turn to the right, heading towards the right side of the peak.
	forward(1000,1.2); //120-126, the robot moves towards the top right side, angles slightly to get the poms loose from the rear arm, backs up, and returns to the previous angle to prepare to go down the hill to collect more poms or simply guard our scoring objects.
	disable_servos();
	mrp(leftmotor,600,300);
	bmd(leftmotor);
	forward(-800,1.2);
	mrp(leftmotor,600,-300);
	bmd(leftmotor);

}