Colony

#include <dragonfly_lib.h>

/**

 * RangeFinder Rules

 * - analog_init() is needed

 * - value 100 - under 5cm is not detected

 * - value 800 - over ~30cm is not detected

 * - at ~20cm values become increasingly erroneous

 *

 */

int to_mm(int rangeFinder)

{

        if(rangeFinder != -1)

                return (int)(rangeFinder/15.112*10);

        else

                return -1;

}

int testrangefinders(void) {

  dragonfly_init(ANALOG);

  usb_init();

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

  range_init();

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

  int d1,d2,d3,d4,d5;

  delay_ms(1000);

  //int sum = 0;

  //int count = 0;

  //int average = 0;

  // start testing rangefinders

  while(1) {

        //if(count == 10000)

        //{

        //        average = sum/count;

        //        count = 0;

        //}

    // get ranges

    //d1 = range_read_distance(IR1);

    d2 = range_read_distance(IR2);

    //d3 = range_read_distance(IR3);

    //d4 = range_read_distance(IR4);

    //d5 = range_read_distance(IR5);

    //usb_puts("d1=");

    //usb_puti(d1);

        //sum += d2;

    //usb_puts("\taverage of 10000=");

    //usb_puti(d2);

    //usb_puts("\td3=");

    //usb_puti(d3);

    //usb_puts("\td4=");

    //usb_puti(d4);

    //usb_puts("\td5=");

    //usb_puti(d5);

        usb_puts("\tIn mm=");

        usb_puti(to_mm(d2));

    usb_puts("\r\n");

        //count++;

    delay_ms(100);

  }

  // end testing rangefinders

        return 0;

}