Colony

#include <dragonfly_lib.h>

#include <wl_basic.h>

#define TIME_DELAY 2000

int main (void) {

        int rangefinders[5] = {0, 0, 0, 0, 0};

        int BOMs[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};

        int encoders[2] = {0, 0};

        int i;

        /* Initialize the dragonfly boards, the xbee, and the encoders */

        dragonfly_init(ALL_ON);

        xbee_init();

        encoders_init();

        while (1) {

                // Collect rangefinder data and send it

                rangefinders[0] = range_read_distance(IR1);

                usb_puts("RO:IR1:"); usb_puti(rangefinders[0]); usb_puts("\n");

                rangefinders[1] = range_read_distance(IR2);

                usb_puts("RO:IR2:"); usb_puti(rangefinders[1]); usb_puts("\n");

                rangefinders[2] = range_read_distance(IR3);

                usb_puts("RO:IR3:"); usb_puti(rangefinders[2]); usb_puts("\n");

                rangefinders[3] = range_read_distance(IR4);

                usb_puts("RO:IR4:"); usb_puti(rangefinders[3]); usb_puts("\n");

                rangefinders[4] = range_read_distance(IR5);

                usb_puts("RO:IR5:"); usb_puti(rangefinders[4]); usb_puts("\n");

                // Collect BOM data and send it

                bom_refresh(BOM_ALL);

                for (i = 0; i < 16; i++) {

                        BOMs[i] = bom_get(i);

                        usb_puts("RO:BOM"); usb_puti(i); usb_puts(":");

                        usb_puti(BOMs[i]); usb_puts("\n");

                }

                // Collect encoder data and send it

                encoders[0] = encoder_get_dx(LEFT);

                usb_puts("RO:ENCL:"); usb_puti(encoders[0]); usb_puts("\n");

                encoders[1] = encoder_get_dx(RIGHT);

                usb_puts("RO:ENCR:"); usb_puti(encoders[1]); usb_puts("\n");

                delay_ms(TIME_DELAY);

        }

}