Colony

#include <dragonfly_lib.h>

#include <wireless.h>

#include <wl_token_ring.h>

/**

 * Tests the token ring and BOM

 * - prints table of bom values for every robot in token ring

 * - takes several seconds to initialize token ring

 */

#define MAX_ROBOTS 16

// set a list of integers to 0

void clearRobots(int* list) {

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

    list[i] = 0;

}

int testtokenring(void) {

  usb_init();

        usb_puts("usb turned on\r\n");

  wl_init();

  usb_puts("wireless turned on\r\n");

  wl_token_ring_register();

  wl_token_ring_join(); // join token ring

  usb_puts("token ring joined\r\n");

  int robotList[sizeof(int)*MAX_ROBOTS];

  int numRobots = 0;

  delay_ms(1000);

  // start testing wireless/token ring/BOM

  int i=0;

  while(1) {

    wl_do();

    // only print table every 200 loops

    if (i%200==0) {

      // get token ring size values

      usb_puts("\r\nnumber of robots in token ring:");

      usb_puti(wl_token_get_robots_in_ring());

      usb_puts("\r\nnumber of robots in matrix:"); 

      usb_puti(wl_token_get_num_robots());

      // get list of robots

      numRobots = 0;

      clearRobots(robotList);

      wl_token_iterator_begin();

      while(wl_token_iterator_has_next()) {

        int tmp = wl_token_iterator_next();

        if (tmp < 0)

          break;

        robotList[tmp] = 1;

        numRobots++;

      }

      if (numRobots < 1) {

        usb_puts("\r\nNo BOM table available.");

        continue; // skip table printing

      } else {

        usb_puts("\r\nBOM table: (* indicates this robot)");

      }

      // print table of bom readings between robots

      usb_puts("\r\ns \\ d");

      // print header

      for(int j=0;j<MAX_ROBOTS;j++)

        if (robotList[j]) {

          usb_puts("\t|");

          if (j == wl_get_xbee_id())

            usb_puts("*"); // indicate that this is the current bot

          usb_puti(j);

        }

      usb_puts("\r\n");

      // print body

      for(int l=0;l<MAX_ROBOTS;l++) 

        if (robotList[l]) {        

          if (l == wl_get_xbee_id())

            usb_puts("*"); // indicate that this is the current bot

          usb_puti(l); // print label col

          for(int k=0;k<MAX_ROBOTS;k++) 

            if (robotList[k]) {

              usb_puts("\t|");

              if (k != l) {

                int bom = wl_token_get_sensor_reading(l,k);

                if (bom >= 0 && bom <= 15)

                  usb_puti(bom); // print bom value

              }

            }

          usb_puts("\r\n");

        }

      usb_puts("\r\n");

    }

  }

  // end testing token ring and bom

        return 0;

}