//Team 92
//URobot-- Gather and sort tribbles into their respective bins.

/*
claw - servo 0
arm - servo 1
turret - servo 3

*/

#use "botball.ic"

#define turret 3
#define claw 0
#define arm 1
#define light 2
int angle; //track create's angle
int initial;  //create's initial angle

int main()
{
    int cal = 0; //calibrate robot
    int navPid; //allow alpha to run
    
    display_clear(); //clears display
    printf("Urobot Bot\n"); //prints bot name
    printf("Lockheed Martin Team 92\n"); //team name
    printf("Press A to Calibrate\n"); //tells user to press a to light calibrate
    printf("B to Bypass\n\n"); //or b to bypass calibration
    
    //Choose A Button for Light Cablibrated Run or B Button to run
    while(cal == 0)
      {

	//if a button is pressed, run light calibration
        if(a_button() == 1) 
        {
            cal = 1;
            beep();
            beep();
            wait_for_light(light);
        } //end if
        
	//if b button is pressed, bypass calibration
        if(b_button() == 1) 
        {
            cal = -1;
            beep();
        } //end if
    } //end while
    
    //start shut down process
    shut_down_in(119.5);
    navPid = start_process(alpha()); //main navigation process
} //end main


//main navigation process
void alpha()
{    
    enable_servos();
    create_connect();
    
    //Need to start claw inside box
    tribbleTree(70);
    
    create_mrp(50,420L); //Moves out of start box to corner
    
    create_spin_block(300,82); //Turn corner
    
    //First tribble tree top
    clawOpen(); //Opens claw
    topTribble(85); //Positions arm to get tribble
    create_mrp(260,426L); //Move forward to grab tribble
    clawClose(); //Grab tribbles
    plant(); //Drops tribbles into plant compartment
    
    //Second tribble tree top
    //Don't get first bottom to not knock over second tree
    create_mrp(-260,175L); //Move backward to grab next tree
    msleep(800L);
    topTribble(65); //Position claw to get top tribble
    create_spin_block(200,-18); //Spin for better positioning
    msleep(800L);
    create_mrp(260,60L); //Move forward to be able to grab tribbles
    msleep(800L);
    clawClose(); //Grab tribble
    crew(); //Arm drops tribble into crew slot
    
    //Second tribble tree bottom
    tribbleTree(65); //Position arm to get bottom
    msleep(800L);
    create_mrp(260,150L); //Move forward to be able to grab tribbles
    clawClose(); //Grab tribbles
    plant(); //Tribbles into plant area
    
    //First tree bottom
    create_spin_block(200,28); //Spin back to earlier position
    create_mrp(-260,80L); //Move back to position for next tribbles
    tribbleTree(90); //Positions arm to be able to grab
    msleep(400L);
    create_mrp(260,220L); //Move forward to grab tribbles
    clawClose(); //Grab tribbles
    crew(); //Tribbles into crew
    
    //Third tree top
    create_spin_block(200,-20); //Spin robot for better positioning
    //topTribble(69); //Position arm for top tribble
    topTribble(65);
    msleep(800L);
    create_mrp(260,90L); //Move forward to grab tribbles
    clawClose(); //Grab tribbles
    crew(); //Tribbles into crew area
    
    //Third tree bottom
    //tribbleTree(75); //Position arm for bottom tribbles
    tribbleTree(70);
    msleep(800L);
    create_mrp(260,200L); //Move forward to grab
    clawClose(); //Grab tribbles
    plant(); //Tribbles into plant box
    
    //Fourth tree top
    create_mrp(-260,300L); //Move backwards to get next tree
    msleep(500L);
    create_spin_block(200,-34); //Rotate robot to face fourth tree
    topTribble(90); //Move arm into position
    msleep(1000L);
    create_mrp(260,135L); //Move robot forward to get tribble
    clawClose(); //Grab tribbles
    plant(); //Tribbles into crew box
    
    //Fourth tree bottom
    create_mrp(-260,85L); //Move back to position
    tribbleTree(85); //Position arm to grab
    msleep(900L);
    create_mrp(275,230L); //Move robot forward to grab
    clawClose(); //Grab tribbles
    create_mrp(-350,100L);
    msleep(900L);
    crew(); //Tribbles into crew box
    
    set_servo_position(arm,60);
    set_servo_position(turret,85);
    clawOpen();
    
    //Go to dump crew in shelter
    create_mrp(500,175L); //Start toward shelter
    msleep(250L);
    create_spin_block(200,30); //Align with shelter
    msleep(250L);
    create_mrp(500,800L); //More toward shelter
    
    create_sensor_update();
    angle = create_angle();
    initial = create_angle();

    //pipe-bump loop
    while(1)
      {
	//right touch sensor is hit, turn robot left
        if(gc_rbump != 0)
          {
            create_mrp(200,-75L);
            create_spin_block(300,3);
            angle += create_angle();
            msleep(100L);
        } //end if

	//left touch sensor is hit, turn robot right
        if(gc_lbump != 0)
          {
            create_mrp(200,-75L);
            create_spin_block(300,-3);
            angle += create_angle();
            msleep(100L);
        } //end if

	//both touch sensor are hit...
        if((gc_rbump != 0) && (gc_lbump != 0))
          {
            //align the robots angle to its initial angle
            if(angle != initial)
              {
                create_spin_block(400,angle-initial);
                msleep(150L);
            } //end nested if

            else
              {
                break;
            } //end nested else
            
        } //end if
        
	//neither sensor is hit
        else
          {
            create_drive_straight(400);
        }
        create_sensor_update();
    } //end while
    
    set_servo_position(arm,250); //Raises the arm out of the way
    msleep(500L);
    
    create_spin_block(400,155); //Turn around to dump tribbles in shelter
    
    //Dump tribbles
    msleep(500L);
    mrp(3,700,135L); //Raise gate to get tribbles out

    //Move back and forth to jolt tribbles out
    create_mrp(1000,250L); //Move forward to jolt tribbles out
    msleep(250L);
    create_mrp(-1000,200L); //Move back to jolt tribbles out
    create_mrp(1000,200L); //Move forward to jolt tribbles out
    msleep(250L);
    create_mrp(-1000,200L); //Move back to jolt tribbles out 
    create_mrp(1000,200L); //Move forward to jolt tribbles out
    msleep(250L);
    create_mrp(-1000,200L); //Move back to jolt tribbles out 
    create_mrp(1000,600L); //Drive out of shelter
    msleep(500L);
    
    //Go to dump plants in solarium
    set_servo_position(turret,255); //moves the arm out of the way
    set_servo_position(arm,55); //moves the claw out of the way
    create_mrp(500,450L); //Drive out of shelter area
    msleep(400L);
    create_spin_block(400,-50); //Turn to align for driving toward bridge/solarium on opposite pvc
    create_mrp(500,1100L); //Drive towards the wall
    create_spin_block(400,65); //turn towards solarium
    
    create_sensor_update();

    //pipe-bump loop
    while(1)
      {
	
	//if right bump sensor triggers, turn left
        if(gc_rbump != 0)
          {
            create_mrp(200,-75L);
            create_spin_block(300,2);
            msleep(100L);
        } //end if

	//if left bump sensor triggers, turn right
        if(gc_lbump != 0)
          {
            create_mrp(200,-75L);
            create_spin_block(300,-2);
            msleep(100L);
        } //end if

	//if both bump sensors trigger (front), break from the loop and continue with code
        if((gc_rbump != 0) && (gc_lbump != 0))
          {
            break;
        } //end if
        
	//while no sensors are triggered, drive forward
        else
          {
            create_drive_straight(350);
        } //end if
        create_sensor_update();
    } //end while
    
    //turn robot around
    create_spin_block(400,-80);
    
    create_disconnect();
    disable_servos();
} //end "Alpha" - main drive function


//Drops the plants into the plant compartment
void plant()
{
    int pos = get_servo_position(arm);

    //while the arm is not at 255 position, keep moving the servo
    while(pos != 255)
      {
        set_servo_position(arm,255);
        pos = get_servo_position(arm);
    } //end while
    
    msleep(1000L);
    pos = get_servo_position(turret);
    
    //while the turret is not at 34 position, keep moving the servo
    while(pos != 34)
      {
        set_servo_position(turret,34);
        pos = get_servo_position(turret);
    } //end while
    
    msleep(1500L); //Wait until arm stops moving
    dump(); //Opens claw slowly
} // end plant function


//Drops the crew into the crew compartment
void crew()
{
    int pos = get_servo_position(arm);

    //while the arm is not at 250 position, keep moving the servo
    while(pos != 250)
      {
        set_servo_position(arm,250);
        pos = get_servo_position(arm);
    } //end while

    msleep(1000L);
    pos = get_servo_position(turret);

    //while the turret is not at 90 position, keep moving the servo
    while(pos != 90)
      {
        set_servo_position(turret,90);
        pos = get_servo_position(turret);
    } //end while
    
    msleep(1500L); //Wait until arm stops moving
    dump(); //Opens claw slowly
} //end crew fuction


//Fully opens the claw for maximum grabing area
void clawOpen()
{    
    int pos = get_servo_position(claw);

    //while the claw is not at 140 position, keep moving the servo
    while(pos != 140)
      {
        set_servo_position(claw,140);
        pos = get_servo_position(claw);
    } //end while
} //end clawOpen function


//Slowly opens the claw for dumping
void dump()
{
    int i = 0;
    int pos = get_servo_position(claw);

    //while the claw is not at 140 position, keep moving the servo
    while(pos != 140)
      {
        set_servo_position(claw,pos += 5);
        msleep(75L); //pause to slow claw opening
        pos = get_servo_position(claw);
    } //end while
} //end dump function


//Closes the claw
void clawClose()
{
    int pos = get_servo_position(claw);

    //while the arm is not at 45 position, keep moving the servo
    while(pos != 45)
      {
        set_servo_position(claw,45);
        pos = get_servo_position(claw);
    } //end while

    msleep(750L);
} //end clawClose function


//Raises the claw slightly off the ground to grab the top tribble
void topTribble(int angle) {
    
    int pos = get_servo_position(turret); //position of servo
    
    create_passive();
    
    //keep moving servo until position is the matches "angle" parameter
    while(pos != angle)
    {
        set_servo_position(turret,angle);
        pos = get_servo_position(turret);
        msleep(50L);
    } //end while
    
    
    msleep(1500L);
    pos = get_servo_position(arm);
    
    //loop until position is 22
    while(pos != 22)
    {
        set_servo_position(arm,22);
        pos = get_servo_position(arm);
        msleep(50L);
    } //end while
    
    create_safe();
} //end topTribble function


//Drops the claw all the way down to grab the base of the tree
void tribbleTree(int angle) {
    
    int pos = get_servo_position(turret); //the current position of the servo
    
    create_passive();
    
    //keep moving servo until position is the matches "angle" parameter
    while(pos != angle)
    {
        set_servo_position(turret,angle);
        pos = get_servo_position(turret);
        msleep(50L);
    } //end while
    
    msleep(1500L);
    pos = get_servo_position(arm);
    
    //loop until position is zero
    while(pos != 0)
    {
        set_servo_position(arm,0);
        pos = get_servo_position(arm);
        msleep(50L);
    } //end while
    
    create_safe();
} //end tribbleTree


//Create drives forward given speed and distance
void create_mrp(int speed,long dist)
{
    create_drive_straight(speed);
    
    if(speed < 0)
    {
        dist = -dist;
    } //end if
    
    msleep(dist * 1000L / (long)speed);
    create_stop();
    msleep(50L);
} //end create_mrp