// Elwin's grappling hook
//
// Release servo on servo 3
// Winding motor on motor 3 
// Starting Light Sensor on analog 6

#use botball.ic

int cal = 0; // Calibration flag
long position = 0L;
int i = 0;

int main()
{
    printf("\nA to Calibrate, B to Bypass");
    msleep(500L);
    while(cal==0) 
      {
        if (a_button()==1) // Calibrate
          {  
            cal = 1;
            wait_for_light(6);            
        }
        if (b_button()==1) // Bypass calibrate
          { 
            cal = -1;
            msleep(500L);
        }
    }
    
    shut_down_in(89.0);  // Shut down in 89 seconds
    
    start_process(grapple());
}    

void grapple() {
    msleep(100L);  // Wait for other bot
    
    // Fire grappling hook
    enable_servos();
    set_servo_position(3, 125);
    
    for(i=120; i<190; i+=12)
      {
        set_servo_position(3, i);
    }
    
    set_servo_position(3, 190);
    msleep(2000L);
    disable_servos();
    
    // Wind up string
    clear_motor_position_counter(3);
    
    while(position < 22000L)
      {
        motor(3, 100);
        position = get_motor_position_counter(3);
        printf("\nmotor position counter ");
        printf("%d", position);
    }  
    
    //    mrp(0, 500, 16000);
    //    bmd(0);
    // 300
    // 8100
    // 26000
    
    // Stop all    
    ao();
    return;
}