function_main.ic

/** 
 * @file   function_main.ic
 * @brief  Main loop
 * @author Andrew Krieger
 * @date   4/26/2011
 *
 */


#use "defines.ic"
#use "drive.ic"
#use "servo/exp_servo_lib.ic"
#use "CdS.ic"
#use "line.ic"
//#use "corn.ic"


/**
 * @brief main entry point
 */
void main() {
    
    int i, j, old;
    
    gps_initialize_knob();
    
    exp_servo_min[SERVO_CORN] = exp_servo_min[SERVO_HAY] = 400;
    exp_servo_max[SERVO_CORN] = exp_servo_max[SERVO_HAY] = 4000;
    
    exp_servo_enable();
    exp_servo_deg(SERVO_CORN, 0);
    exp_servo_deg(SERVO_HAY, 0);
    
    init_shaft_encoders();
    /*
    {
        int x,d=-1;
        for(;;) {
            x = knob()/2;
            if(x!=d)
              printf("Ang: %d\n", d=x);
            exp_servo_deg(SERVO_CORN, x);
        }
    }
*/
    
    #if 1
      for(;;) {
        int up_hill=1;
        printf("Are you ready tostart WINNING?\n");
        wait_for_start(CDS_FRONT);
        gps_enable();
        
        while(drive_up_hill()) {
            exp_servo_deg(SERVO_CORN, 60);
            msleep(500L);
        }
        
        exp_servo_deg(SERVO_CORN, 0);
        
        turn(-70.0);
        
        gps_turn(85, 4);
        
        drive_back_to_shiny(1, 2250L);
        
        msleep(2000L);
        gps_good_data();
        
        if(gps_y < 185)
          gps_turn(70, 4);
        else
          gps_turn(82, 4);
        
        
        drive_back_to_shiny(0, 1500L);
        
        follow_strip_back(0);
        follow_strip_back(0);
        follow_strip_back(0);
        
        drive_straight(-3);
        
        //        exp_servo_deg(SERVO_HAY, 45);
        msleep(250L);
        exp_servo_deg(SERVO_HAY, 90);
        msleep(250L);        
        exp_servo_deg(SERVO_HAY, 0);
        msleep(250L); 
        
        
        gps_good_data();
        for(i=0;!gps_button && i < RED_BUTTON_TRIES;++i) {
            motor(MOTOR_LEFT, -100);
            motor(MOTOR_RIGHT, -100);
            msleep(300L);
            
            motor(MOTOR_LEFT, 100);
            motor(MOTOR_RIGHT, 100);
            msleep(150L);
            gps_good_data();
        }
        
        curve_to_wagon();
        
        follow_strip_back(0);
        follow_strip_back(0);
        follow_strip_back(0);
        
        gps_align();
        
        motor(MOTOR_LEFT, -80);
        motor(MOTOR_RIGHT, -80);
        msleep(125L);
        ao();
        
        pull_wagon();
        
        drive_down_hill();
        
        ao();
    }
    #else
    
    printf("Are you ready tostart WINNING?\n");
    wait_for_start(CDS_SIDE);
    gps_enable();
    start_to_corn();
    test_corn();
    pick_corn();
    printf("Ready to cleanupPress start\n");
    press_start();
    left_corn_cleanup();
    corn_to_start();
    
    #endif
}