// Team 129 House Bot
// This bot drives out of the starting box and
// collects all the houses, put umbrellas in 
// them and brings them back to the starting box.

// Left Motor = motor port 0
// Right Motor = motor port 2
// Umbrella Arm Motor = motor port 1
// Starting Light = analog port 2
// Right Touch Sensor = digital port 8
// Left Touch Sensor = digital port 9

#use "botball.ic"

// Forward Function:
// pre-condition: distance is a flot of the distance we want to drive across the board in inches.
// post-condition: robot has driven distance inches across the board.
void forward(float distance) {                             
    mrp(0,800,(long)(distance*(129.275)));                        
    mrp(2,820,(long)(distance*(129.275)));                        
    bmd(0);                                                    
    bmd(2);                                                    
    msleep(100L);    
}                                                              

//Rotate function:
//pre-condition: turns is a float number representing the number of rotations we want the robot
//to make. Can take fractions of a turn.
//post-condition: robot has turned itself through turns rotations
void rotate(float turns) {                                                              
    float half_width, radius, ticks_per_robot_rev, pi;         
    float ticks_per_wheel_rev, robot_rev_circum, wheel_circum; 
    long ticks_to_turn;                                        
    sleep(.5);                                                 
    half_width=100.0; radius=34.4;pi=3.1416;                    
    ticks_per_wheel_rev=1100.0;                                 
    wheel_circum=pi*radius*2.0;                             
    robot_rev_circum=half_width*pi*2.0;                        
    ticks_per_robot_rev=ticks_per_wheel_rev*robot_rev_circum/wheel_circum;
    ticks_to_turn=(long)(turns*ticks_per_robot_rev);           
    mrp(0,500,ticks_to_turn);                                  
    mrp(2,500,-ticks_to_turn);                                 
    bmd(0);
    bmd(2);
    msleep(100L);    
}

//Main method: runs the main functions of the robot.
void main()
{
    //Begin light sensor calibration.
    display_clear(); 
    sleep(0.5);
    printf("Team 129 House Bot\n");
    sleep(1.0);
    printf("A to calibrate\nB to bypass\n");
    while (i==0) 
      {
        if (a_button()==1) {
            i = 1;
            beep();
            wait_for_light(2);
        } 
        if (b_button()==1) {
            i = 1;
            beep();
        }
        
    }
    //End light sensor calibration.
    // Shut down process
    //Turns the robot off after 89 seconds, within the game time limits.
    shut_down_in(89.0);
    
    // Run the navigation process: our robot moves around the board.
    start_process(nav());
    
    // Done
    printf("Done\n");
    ao();
}

//Navigation function: moves around the board.
void nav()
{
    forward(17.0); //Move forward beyond the starting box.
    
    rotate(0.23); //Left turn out of the starting box.
    
    forward(16.0);  //Move forward to the first house.
    rotate(.23);   //Left turn to face out to board.
    //Wait for robot to line up with PVC piping, to ensure a straight line.
    while((digital(8) == 0) || (digital(9) == 0))
      {
        motor(0,-50);
        motor(2,-50);
    }
    printf("%d %d\n", digital(8),digital(9));
    forward(16.75); //Move forward across the board, collecting the houses
    rotate(-.03); //A small adjustment to the right to get house into left side
    forward(15.25); //Continue collecting houses.
    mrp(1,100,-100L); //Lowers the arm onto the houses, securing them
    bmd(1); 
    rotate(-.05); //Zigzag around dumps
    forward(-2.5);
    rotate(.07);
    sleep(30); //Wait for water bot to finish
    forward(-33.5); //Moves backward with houses in tow.
    //Wait for touch on side
    while((digital(8) == 0) || (digital(9) == 0))
      {
        motor(0,-50);
        motor(2,-50);
    }
    printf("%d %d\n", digital(8),digital(9));
    forward(3.5); //Get away from the side
    rotate(.23); //Left turn towards the box   
    forward(17.5); //Go towards the box
    rotate(-.23); //Right turn facing the box
    forward(10.0); //Get in the box
    return();    
}
