/* Sweeper.ic Peter Greenwood April 1, 2008 This program seeks 4 fuzzballs on the table of either color, picks them up, and places them into their correct hopper*/#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#define FALSE 0#define TRUE 1int color_channel; // Global variable to record the located color channelvoid main() { int collected_balls = 0; create_connect(); init_camera(); while (collected_balls < 4) { track_update(); create_drive_straight(100); // Drive straight until a blob is within range of the camera while (!check_channel(RED) && !check_channel(GREEN)) { track_update(); } // Once in range, record the color of the closest blob, for use later if (find_closest_blob_y(RED) > find_closest_blob_y(GREEN)) { color_channel = RED; } else { color_channel = GREEN; } printf("%d\n",color_channel); // Stop the create and spin to face the ball create_stop(); if (find_closest_blob_x(color_channel) < 150) { create_spin_CCW(100); while (find_closest_blob_x(color_channel) < 150) { track_update(); } create_stop(); } else { create_spin_CW(100); while (find_closest_blob_x(color_channel) > 170) { track_update(); } create_stop(); } // Drive towards it and keep it centered until the ball is close enough to get create_drive_straight(100); while (find_closest_blob_y(color_channel) < 220) { track_update(); if (find_closest_blob_x(color_channel) < 150) { create_spin_CCW(100); while (find_closest_blob_x(color_channel) < 150) { track_update(); } create_stop(); } else { create_spin_CW(100); while (find_closest_blob_x(color_channel) > 170) { track_update(); } create_stop(); } create_drive_straight(100); sleep(0.1); } create_stop(); get_fuzzball(); // After the ball has been gotten, back up and find another create_drive_straight(-300); sleep(1.0); collected_balls++; } create_disconnect();}/* find_closest_blob_x (int channel) Returns the x value of the centroid of the closest blob on channel [channel]*/int find_closest_blob_x (int channel) { return track_x(channel,find_closest_blob(channel));}/* find_closest_blob_y (int channel) Returns the y value of the centroid of the closest blob on channel [channel]*/int find_closest_blob_y (int channel) { return track_y(channel,find_closest_blob(channel));}/* find_closest_blob (int channel) Returns the blob number for the currently closest blob on channel [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;}/* check_channel (int channel) Returns TRUE if a blob or blobs are below 100 on the y axis of the camera*/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 TRUE; } } }}/* get_fuzzball () Performs the motions necessary to pick up a fuzzball right in front of the bot This includes dropping and raising the arm, opening and closing the claw, and moving the hopper if needed*/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(); }}/* claw_open () Turns the claw motor to open the claw grip*/void claw_open () { mrp(CLAW_MOTOR,1000,-40L); bmd(CLAW_MOTOR);}/* claw_close () Turns the claw motor to close the claw grip*/void claw_close () { mrp(CLAW_MOTOR,1000,40L); bmd(CLAW_MOTOR);}/* claw_arm_down () Turns the claw arm motor to bring the claw to the floor*/void claw_arm_down () { mrp(CLAW_ARM_MOTOR,1000,500L); bmd(CLAW_ARM_MOTOR);}/* claw_arm_up () Turns the claw arm motor to bring the claw above the hopper*/void claw_arm_up () { mrp(CLAW_ARM_MOTOR,1000,-500L); bmd(CLAW_ARM_MOTOR);}/* hopper_down () Rotates the hopper motor to lower the upper hopper out of the way*/void hopper_down () { int ticks = 0; bk(HOPPER_MOTOR); while (ticks < 12) { while(digital(HOPPER_ROTATE_SENSOR)) { } while(!digital(HOPPER_ROTATE_SENSOR)) { } ticks++; } ao();}/* hopper_up () Rotates the hopper motor until the hopper touches the upper limit switch*/void hopper_up () { fd(HOPPER_MOTOR); while (!digital(HOPPER_TOP_SENSOR)) { } ao();}