/*
Primary Coding: Roger, Chris H, Chris E
Bot Name: Heisman

Heisman's main objectives are to clear satellites from our base, cross the bridge to enemy territory,
obstruct the opponents' robots, and finish the match in the opponents' shelter.
Heisman gains a total of 54 points.
*/

/*
**Remember Gameboy is reversed!
Left Motor - 0
Right Motor - 2
Arm Servo - 3
Light Sensor - Analog 2
*/

#use "botball.ic"

//define constants
#define light 2
#define rmotor 2
#define lmotor 0
#define arm 3
#define rpower 950
#define lpower 1000


int main()
{
    
    int cal = 0; //calibration choice variable
    int navPid; //navigation process identification
    
    display_clear(); //clears display
    printf("Heisman 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
    
    enable_servos();
    armUp(); //presets the arm up for start of match
    disable_servos();
    
    //start shut down process
    shut_down_in(119.0);
    
    navPid = start_process(alpha()); //main navigation function
} //end main


//Main navigation function
void alpha()
{
    enable_servos();
    
    drive(lpower,rpower,2700L,2700L); //drive toward first cup
    drive(lpower,rpower,0L,390L); //turn to get first cup
    
    //grab cup
    armDown(); 
    armUp();
    
    drive(lpower,rpower,4000L,4000L); //drive forward
    drive(lpower,rpower,0L,250L); //twitch left
    
    disable_servos();
    beep();
    ao(); //turns everything off when done
    
} //end alpha


//move the robot based on the parameters
void drive(int left,int right,long ldis,long rdis)
{
    mrp(rmotor,right,rdis * (rdis/ldis));
    mrp(lmotor,left,ldis);
    bmd(rmotor);
    bmd(lmotor);
} //end drive


//raise the arm
void armUp()
{  
    servo(arm,215); //sets the arm position up
    
} //end armUp


//lower the arm
void armDown()
{   
    servo(arm,55); //sets the arm down
    
} //end armDown


//move the servo until it reaches the desired position
void servo(int port,int position)
{
    int initial = get_servo_position(port);
    
    //loop until the current position is equal to the desired position
    while(initial != position)
      {
        set_servo_position(port,position);
        get_servo_position(port);
    } //end while
} //end servo