// Beyond Botball 2007 Bot A
//  This bot moves 4 Botguys to the safe home area
//  and 4 foam balls to the side area
//
// Starting Light = analog 2
// 
// Positioning Servos
//   servo1 = platform
//   servo2 = bottom rotate
//   servo3 = top rotate
//
// Grabber Servos
//   servo4 = arm
//   servo5 = noose

#use botball.ic

// Eight objects with seven attributes
// 0 type 1=Botguy, 2=Ball
// 1 platform location for grab
// 2 arm location for grab
// 3 platform location for drop or 0 for no pick up
// 4 arm location for drop or 0 for no pick up
// 5 platform location for push or 0 for no push
// 6 arm location for push or 0 for no push

int object[8][7] = {
      {1, 3000, 3120,    0,    0,  800, 2800}, // Object 0 Botguy 1
      {1, 2770, 3845, 1700, 2940,  800, 2940}, // Object 1 Botguy 3
      {1, 3100, 1390, 1050, 2710,    0,    0}, // Object 2 Botguy 2
      {2, 2550, 4100, 3370, 1300,    0,    0}, // Object 3 Ball 6
      {2, 3370, 3510, 3050, 1400,    0,    0}, // Object 4 Ball 5
      {1, 1900, 3720, 1250, 2720,    0,    0}, // Object 5 Botguy 4
      {2, 1250, 4200, 2830, 1470,    0,    0}, // Object 6 Ball 8
      {2, 1850, 3470, 3050, 1400,    0,    0}  // Object 7 Ball 7 
};

int i = 0; // Object number

// Position servo adjustments
#define servo1_base 0
#define servo2_base 0
#define servo3_base 0

// Grabber servo positions
#define open_noose 1200
#define close_noose 3500

#define down_guy 3970
#define down_release_guy 3800

#define down_ball 4250
#define down_release_ball 4100

#define down_start 3900
#define up 1650

int cal = 0; // Calibrate start light flag (1=do it, -1=bypass)
int slow = 0; // Slow flag (1=compete, -1=test)
int place = 0; // Placment test flag (1=do it, -1=bypass)

#define sticky 0 // Addition to servo1 position because of sticky board

// Positioning servo current position
int servo1_position = 0;
int servo2_position = 0;
int servo3_position = 0;

// =====================================================================
void main() {
    printf("Grabber Bot V17\n");
    msleep(1000L);
    
    printf("Start to Compete Stop to Slow\n"); 
    while(slow==0) {  
        if (start_button()==1) slow = 1;
        if (stop_button()==1) slow = -1;
    }
    beep();
    msleep(300L);
    // Initial servo positions
    set_initial();
    msleep(2000L);
    init_expbd_servos(0);
    
    printf("Start Test Place Stop to Bypass\n");
    while(place==0) {
        if (start_button()==1) {  // Test positions
            place = 1;
            beep(); msleep(100L);
            beep(); msleep(100L);
            beep(); msleep(100L);
            beep(); msleep(100L);
            // Initial servo positions
            set_initial();
            msleep(500L); // Wait a little more
            servo4 = up;
            msleep(500L);
            // Test Position for Botguy 1 Pick Up
            set_servo1(object[0][1]);
            servo2 = object[0][2];
            msleep(500L);       
            servo3 = object[0][2];
            msleep(500L);
            test_position();
            // Test Position for Ball 8 Pick Up
            set_servo1(object[6][1]);
            servo2 = object[6][2];
            msleep(500L);       
            servo3 = object[6][2];
            msleep(500L);
            test_position();
            // Test Position for Ball 6 Drop Off
            servo2 = object[4][4];
            msleep(500L);       
            servo3 = object[4][4];
            msleep(500L);
            set_servo1(object[4][3]);
            test_position();
            // Servos off
            init_expbd_servos(0);
        }
        if (stop_button()==1) {   // Bypass placement test
            place = -1;
            beep();
            msleep(300L);
        }
    }
    
    printf("Start Cal Light  Stop to Bypass\n");
    while(cal==0) {
        if (start_button()==1) {  // Calibrate
            cal = 1;
            beep();
            wait_for_light(2);            
        }
        if (stop_button()==1) {   // Bypass calibrate
            cal = -1;
            beep();
            msleep(300L);
        }
    }
    
    // -----------------------------------------------------------------
    // Start shut down process
    if (slow==1) shut_down_in(89.0);
    
    // Initial servo positions
    msleep(1000L); // Wait for Reactor Bot
    set_initial();
    servo4 = up;
    msleep(500L);
    
    // -----------------------------------------------------------------
    // Loop through object list
    for(i=0; i<8; i++) {
        // Position to object
        set_servo1(object[i][1]);
        servo2 = object[i][2];
        msleep(500L);       
        servo3 = object[i][2];
        msleep(500L);
        
        // Acquire object
        if (object[i][0]==1) { 
            // Botguy
            if (object[i][3]==0) { 
                // Push only
                grab_guy();
            } else {
                // Pick up
                pick_guy();
            }
        } else {
            // Ball
            pick_ball();
        }
        
        if (slow<1) msleep(2000L);
        
        // Position to home, if not just push
        if (object[i][3]>0) {
            if (object[i][4]!=object[i][2]) {
                servo2 = object[i][4];
                msleep(500L);       
                servo3 = object[i][4];
                msleep(500L);
            }
            set_servo1(object[i][3]);
        }
        
        // Give up object
        if (object[i][0]==1) { 
            // Botguy
            // If push in
            if (object[i][5]>0) { 
                // If picked up
                if (object[i][3]>0) { 
                    // Hold Botguy down
                    hold_guy();
                }
                // Do the push
                if (object[i][6]!=object[i][4]) {
                    printf("Push\n");
                    servo2 = object[i][6];
                    msleep(500L);       
                    servo3 = object[i][6];
                    msleep(500L);
                }
                set_servo1(object[i][5]);           
                // Release
                release_guy();
            } else {
                // Just drop
                drop_guy();                
            }
        } else {
            // Ball
            if (i==7) hold_ball(); // Hold last ball
            else drop_ball();
        }
        
        if (slow<1) msleep(2000L);
    }
    
    // -----------------------------------------------------------------
    // Hold servos for a while
    msleep(4000L);
    
    // Servos off
    init_expbd_servos(0);
    printf("Done\n");
}   

// =====================================================================
// Set Initial Servo Positions
void set_initial() {
    servo1_position = 3950;
    servo1 = servo1_position;
    servo2_position = 2180;
    servo2 = servo2_position;
    servo3_position = 2180;
    servo3 = servo3_position;
    // Initial grabber servo positions
    servo4 = down_start;
    servo5 = open_noose;
    init_expbd_servos(1);
    msleep(500L);
}

// Move servo1
void set_servo1(int target) {
    if (servo1_position==target) {
        // If no change, just get out
        msleep(50L);
    } else {
        // Else move servo1
        // Add sticky board factor
        if (servo1_position<target) {
            target = target+sticky;
        } else {
            if (servo1_position>target) {
                target = target-sticky;
            }
        }
        while (servo1_position!=target) {
            if (servo1_position<target) {
                servo1_position = servo1_position+1;
            } else {
                servo1_position = servo1_position-1;
            }
            servo1 = servo1_position;
        }
        msleep(500L);  
    }
}

// Grab Botguy
void grab_guy() {
    servo4 = down_guy;
    msleep(600L);
    if (slow<1) msleep(2000L);
    servo5 = close_noose;
    msleep(500L);
}

// Release Botguy
void release_guy() {
    servo5 = open_noose;
    msleep(500L);
    servo4 = down_release_guy;
    msleep(500L);
    servo4 = down_release_guy+100;
    msleep(200L);
    servo4 = down_release_guy-100;
    msleep(200L);
    servo4 = down_release_guy;
    msleep(200L);
    servo4 = down_release_guy-200;
    msleep(200L);
    servo4 = down_release_guy-100;
    msleep(100L);
    servo4 = down_release_guy-300;
    msleep(100L);
    servo4 = up;
    msleep(500L);
}

// Pickup Botguy
void pick_guy() {
    servo4 = down_guy;
    msleep(900L);
    if (slow<1) msleep(2000L);
    servo5 = close_noose;
    msleep(500L);
    // Dampen up motion
    servo4 = up+500;
    msleep(500L);
    servo4 = up+300;
    msleep(300L);
    servo4 = up+200;
    msleep(200L);
    servo4 = up+100;
    msleep(100L);
    servo4 = up;
    msleep(100L);
}

// Drop Botguy
void drop_guy() {
    servo4 = down_guy;
    msleep(500L);
    servo5 = open_noose;
    msleep(500L);
    servo4 = down_release_guy;
    msleep(500L);
    servo4 = down_release_guy+100;
    msleep(100L);
    servo4 = down_release_guy-100;
    msleep(100L);
    servo4 = down_release_guy;
    msleep(100L);
    servo4 = down_release_guy-200;
    msleep(100L);
    servo4 = up;
    msleep(500L);
}

// Hold Botguy Down
void hold_guy() {
    servo4 = down_guy;
    msleep(500L);
}

// Pickup Ball
void pick_ball() {
    servo4 = down_ball;
    msleep(900L);
    if (slow<1) msleep(2000L);
    servo5 = close_noose;
    msleep(500L);
    // Dampen up motion
    servo4 = up+400;
    msleep(300L);
    servo4 = up+200;
    msleep(200L);
    servo4 = up+100;
    msleep(200L);
    servo4 = up;
    msleep(200L);
}

// Drop Ball
void drop_ball() {
    servo4 = down_ball;
    msleep(500L);
    servo5 = open_noose;
    msleep(500L);
    servo4 = down_release_ball;
    msleep(500L);
    servo4 = up;
    msleep(500L);
}

// Hold Ball
void hold_ball() {
    servo4 = down_ball-300;
    msleep(500L);
}

// Test Position
void test_position() {
    servo4 = down_release_guy;
    msleep(1500L);
    servo5 = close_noose;
    msleep(500L);
    servo5 = open_noose;
    msleep(1500L);
    servo4 = up;
    msleep(1000L);
}