Anonymous

#include "xbccamlib.ic"
#include "botball.ic"
#define CLAW_MOTOR            0      
#define CLAW_ARM_MOTOR        1
#define HOPPER_MOTOR          2
#define HOPPER_TOP_SENSOR     14
#define HOPPER_ROTATE_SENSOR  15
#define RED                   0
#define GREEN                 1

int color_channel;

void main() {
    create_connect();
    
    init_camera();
    track_update();
    while (!check_channel(RED) && !check_channel(GREEN)) {
        track_update();
    }
    if (find_closest_blob_y(RED) > find_closest_blob_y(GREEN)) {
        color_channel = RED;
    } else {
        color_channel = GREEN;
    }
    
    printf("%d\n",color_channel);
    
    create_disconnect();
}

int find_closest_blob_y (int channel) {
    return track_y(channel,find_closest_blob(channel));
}

int find_closest_blob (int channel) {
    int closest = 0;
    int i;
    for (i = 1; i < track_count(channel); i++) {
        if (track_y(channel,i) > track_y(channel,closest)) {
            closest = i;
        }
    }
    return closest;
}

int check_channel (int channel) {
    if (track_count(channel)) {
        int i;
        for (i = 0; i < track_count(channel); i++) {
            if (track_y(channel,i) > 100) {
                return 1;
            }
        }
    }
}

void get_fuzzball () {
    claw_arm_down();
    create_drive_straight(400);
    sleep(.3);
    create_stop();
    claw_close();
    if (color_channel == RED) {
        hopper_down();
    }
    claw_arm_up();
    claw_open();
    if (color_channel == RED) {
        hopper_up();
    }
}

void claw_open () {
    mrp(CLAW_MOTOR,1000,-40L);
    bmd(CLAW_MOTOR);
}

void claw_close () {
    mrp(CLAW_MOTOR,1000,40L);
    bmd(CLAW_MOTOR);
}

void claw_arm_down () {
    mrp(CLAW_ARM_MOTOR,1000,500L);
    bmd(CLAW_ARM_MOTOR);
}

void claw_arm_up () {
    mrp(CLAW_ARM_MOTOR,1000,-500L);
    bmd(CLAW_ARM_MOTOR);
}

void hopper_down () {
    int ticks = 0;
    bk(HOPPER_MOTOR);
    while (ticks < 12) {   
        while(digital(HOPPER_ROTATE_SENSOR)) { }
        while(!digital(HOPPER_ROTATE_SENSOR)) { }
        ticks++;
    }
    ao();
}

void hopper_up () {
    fd(HOPPER_MOTOR);
    while (!digital(HOPPER_TOP_SENSOR)) { }
    ao();
}