Colony

#include <dragonfly_lib.h>

/**

 * RangeFinder Rules

 * - analog_init() is needed

 * - under 10cm is not detected

 * - over 100cm is not detected

 */

int testrangefinders(void) {

  usb_init();

        usb_puts("usb turned on\n");

  analog_init(1);

  range_init();

  usb_puts("rangefinders turned on\n");

  int d1,d2,d3,d4,d5;

  delay_ms(1000);

  // start testing rangefinders

  while(1) {

    // get ranges

    d1 = range_read_distance(1);

    d2 = range_read_distance(2);

    d3 = range_read_distance(3);

    d4 = range_read_distance(4);

    d5 = range_read_distance(5);

    usb_puts("d1=");

    usb_puti(d1);

    usb_puts(", d2=");

    usb_puti(d2);

    usb_puts(", d3=");

    usb_puti(d3);

    usb_puts(", d4=");

    usb_puti(d4);

    usb_puts(", d5=");

    usb_puti(d5);

    usb_puts("\n");    

    delay_ms(100);

  }

  // end testing rangefinders

        return 0;

}