Colony

#include "firefly+_lib.h"

#include "bays.h"

#define MAX(a,b) ((a) > (b) ? (a) : (b))

#define MIN(a,b) ((a) < (b) ? (a) : (b))

void bay_tester(void)

{

    bayStates[1] = 50;

    bayStates[2] = 51;

    bayStates[3] = 52;

}

void print_bays_debug(void)

{

        usb_puts("\n\r");

        usb_puts("Bays: ");

        for(int i = 0; i < NUM_BAYS; i++) {

                usb_puti(bayStates[i]);

                usb_putc(' ');

        }

        usb_puts("\n\r");

        usb_puts("Seeking robot: ");

        usb_puti(robot_seeking);

        usb_puts("\n\r");

}

void do_consistency_checks(void)

{

    if(robot_seeking != 255 && robot_assignment(robot_seeking) == 255) // the only free bay from bay_tester()

    {

      usb_puts("BAD STUFF HAPPENED...\n\r");

      usb_puts("robot assigned to BAY 1 does not equal robot_seeking\n\r");

      usb_puts("\n\rrobot_seeking: ");

      usb_puti(robot_seeking);

      usb_puts("Bay 1 contains:  ");

      usb_puti(bayStates[1]);

      usb_puts("\n\r");

      //while(1);

    }

}

// END DEBUGGING FUNCTIONS

void bays_init(void)

{

        int i;

        for (i = 0; i < NUM_BAYS; i++)

                bayStates[i] = 255;

        robot_seeking = 255;

        bay_tester();

        digital_output(ENABLE_PIN, 0);

        digital_output(BOM_PIN, 1);

        digital_output(SELECT_0, 0);

        digital_output(SELECT_1, 0);

        digital_output(SELECT_2, 0);

        active_bay = -1;

}

/*int assign_bay(int robot)

{

        int max_bom = sensors[robot]; // where the station sees the robot (max bom)

        int closest = 255;

        int distance = 16;

        int i;

        for (i = 0; i < NUM_BAYS; i++)

        {

                if (bayStates[i] != 255)

                        continue;

                int d = max_bom - i * 4; // distance (BOM-wise) from bay

                if (d < -8)

                        d += 16;

                else

                        if (d > 8)

                                d -= 16;

                if (d < 0)

                        d *= -1;

                if (d < distance) // finds closest bay to robot's current position

                {

                        distance = d; // distance (BOM-wise)

                        closest = i; // actual bay number

                }

        }

        if (closest == 255) // all full

                return 255;

        bayStates[closest] = robot;

        return closest;

}*/

/*int assign_bay(int robot) // returns number of an empty bay (255 if all full)

{

    for(int i = 0; i < NUM_BAYS; i++) // right now, go thru linearly and allocate first available bay

    {

        if(bayStates[i] == 255)

        {

            bayStates[i] = robot;

            return i;

        }

    }

}*/

int left_index( int pos )

{

        int i;

        for(i = pos; i >= 0; i--)

        {

                if(bayStates[i] != 255)

                        return i;

        }

        return -1;

}

int right_index( int pos )

{

        int i;

        for(i = pos; i < NUM_BAYS; i++)

        {

                if(bayStates[i] != 255)

                        return i;

        }

        return NUM_BAYS;

}

int assign_bay( int robot ) // 'smart' assign_bay()

{

        int ldist = 0;

        int rdist = 0;

        int score = 0;

        int max = 0;

        int bay = 255;

        int count = 0;

        int i;

        for(i = 0; i < NUM_BAYS; i++)

        {

                if(bayStates[i] == 255)

                        count++;

        }

        if(count == NUM_BAYS)

        {

                bayStates[0] = robot;

                return 0;

        }

        for(i = 0; i < NUM_BAYS; i++)

        {

                if(bayStates[i] == 255)

                {

                        ldist = i - left_index(i);

                        rdist = right_index(i) - i;

                        if(ldist - i == 1)

                                score = rdist;

                        else if(i + rdist == NUM_BAYS)

                                score = ldist;

                        else

                                score = MIN(ldist,rdist);

                        if(score > max)

                        {

                                max = score;

                                bay = i;

                        }

                }

                ldist = rdist = 0;

        }

        bayStates[bay] = robot;

        return bay;

}

int robot_assignment(int robot) //returns bay to which 'robot' has been assigned, 255 if not yet assigned

{

    for(int i = 0; i < NUM_BAYS; i++)

    {

        if(bayStates[i] == robot)

            return i;

    }

    return 255;

}

int is_empty(void) //returns 1 if there is an empty bay, 0 if not

{

    for(int i = 0; i < NUM_BAYS; i++)

    {

        if(bayStates[i] == 255)

            return 1;

    }

    return 0;

}

void lbom_on(void)

{

        digital_output(BOM_PIN, 0);

}

void lbom_off(void)

{

        digital_output(BOM_PIN, 1);

}

void bay_enable(int which)

{

        active_bay = which;

        lbom_off();

        bay_disable();

        digital_output(SELECT_0, which & 1);

        digital_output(SELECT_1, (which>>1) & 1);

        digital_output(SELECT_2, (which>>2) & 1);

        digital_output(ENABLE_PIN, 1);

}

void bay_disable(void)

{

        digital_output(ENABLE_PIN, 0);

}