corn.ic

#use "drive.ic"
#use "line.ic"
#use "defines.ic"


/**
 * @brief Drive from the starting area (facing west) to the corn area
 * 
 * Assumes starting in the template position.
 */
void start_to_corn(void) {
    int shiny_ctr;
    
    turn(5.0);
    drive_straight(45);
    turn(-50.0);
    gps_turn(45, 4);
    
    SHAFT_LEFT_COUNT = SHAFT_RIGHT_COUNT = 0;
    
    reset_system_time();
    motor(MOTOR_LEFT,  100);
    motor(MOTOR_RIGHT, 100);
    while(!digital(SWITCH_WALL_LEFT) && SHAFT_LEFT_COUNT + SHAFT_RIGHT_COUNT < 220 && mseconds() < 7500L)
      ;
    
    ao();
    msleep(100L);
    turn_cw_to_shiny();
}


/**
 * @brief Drive from the rightmost corn strip (facing south) to the start area
 *
 * Using empirically determined values, drive back to the starting area.  Start
 * on the middle corn strip, facing (approximately) south.  This function follows
 * that strip, turns toward the starting area (parallel to the bumps), then drives
 * between two bumps back to the start.  It then lines up facing north, ready
 * to transition into drive_up_hill.
 */
void corn_to_start(void) {
    follow_strip_forward(1, 3000L);
    
    drive_straight(13);
    
    turn(45.0);
    gps_turn(40, 4);
    
    drive_straight(95);
    
    turn(60.0);
    
    drive_straight(20);
    turn(90.0);
    gps_turn(0, 4);
}

void check_switch(void) {
    long last = 0L;
    
    for(;;) {
        if(mseconds() - last > 50L) {
            printf("Sw: %d\n", digital(SWITCH_CORN));
            last = mseconds();
        }
        defer();    
    }
}

/**
 * @brief Checks to see if the corn light underneath the robot is red or blue.
 * @return Returns 1 if the light is blue, 0 if not.
 *
 * Currently has a press_start() in there for debugging.
 */
int test_corn(void) {
    int cds;
    
    follow_strip_forward(1, 3000L);
    
    drive_straight(-6);
    
    cds = analog(CDS_FRONT);
    
    printf("CDS: %d - ", cds);
    
    if(cds < 20)
      printf("Red\n");
    else
      printf("Blue\n");
    
    return cds >= 20;
}

void pick_corn(void) {
    int i, pid;
    
    msleep(2000L);
    
    pid = start_process(check_switch());
    
    drive_straight(-12);
    
    exp_servo_deg(SERVO_CORN, 110);
    msleep(500L);
    
    drive_straight(12);
    
    gps_align();
    
    exp_servo_deg(SERVO_CORN, 65);
    // turn on electromagnet
    exp_servo_deg(SERVO_HAY, 20);
    msleep(200L);
    
    drive_straight(-7);
    msleep(500L);
    turn(90.0);
    msleep(1500L);
    turn(-180.0);
    msleep(1500L);
    turn(90.0);
    msleep(1500L);

    exp_servo_deg(SERVO_CORN, 20);
    msleep(200L);
    exp_servo_deg(SERVO_HAY, 30);
    
    kill_process(pid);
}

void do_corny_stuff(void) {
    if(test_corn()) {
        pick_corn();
        return;
    }
    
    drive_straight(-33);
    turn(-45.0);
    drive_forward_to_shiny(0, 2000L);
    if(test_corn()) {
        pick_corn();
        return;
    }
   
    drive_straight(-33);
    turn(-45.0);
    drive_forward_to_shiny(0, 2000L);
    
}


/**
 * @brief After picking up left corn, go to middle strip and prepare to return to start
 *
 * Precondition: Robot is approximately at south end of left (westernmost) corn strip, facing
 * roughly north.  This should be what the corn picking code does, if called on the west corn.
 * Postcondition: Robot will be on the middle strip, ready to go back to start (facing south).
 */ 
void left_corn_cleanup(void) {
    turn(-180.0);
    printf("GPS to 135\n");
    press_start();
    
    gps_turn(135, 4);
    
    SHAFT_LEFT_COUNT = SHAFT_RIGHT_COUNT = 0;
    gps_continuous_enable();
    
    while((gps_y < 80 || gps_y >= 256) && (analog(OPTO_FRONT_LEFT) >= OPTO_THRESHOLD || analog(OPTO_FRONT_RIGHT) >= OPTO_THRESHOLD)) {
        if((int)mseconds() & 0x6F == 0)
          printf("Y: %d\n", gps_y);
        step_straight_back(4);
    }
    
    if(analog(OPTO_FRONT_LEFT) < OPTO_THRESHOLD || analog(OPTO_FRONT_RIGHT) < OPTO_THRESHOLD) {
    }
    
    gps_continuous_disable();
    
}

void drop_corn(void) {
    follow_strip_forward(0, 3000L);
    exp_servo_deg(SERVO_CORN, 90);
    msleep(500L);
    
    motor(MOTOR_LEFT,-60);
    motor(MOTOR_RIGHT,-60);
    sleep(3.0);
    ao();
    // motors need love too
    sleep(0.1);
    exp_servo_deg(SERVO_CORN, 0);
    msleep(500L);
}