Colony

#include "test_bom.h"

#include "global.h"

#include "hardware_turntable.h"

#include "hardware_rbom.h"

#include "comm_robot.h"

static uint16_t bom_emitter_position (uint8_t num)

{

        switch (num)

        {

                case 0:  return  100; break;

                case 1:  return  200; break;

                case 2:  return  300; break;

                case 3:  return  400; break;

                case 4:  return  500; break;

                case 5:  return  600; break;

                case 6:  return  700; break;

                case 7:  return  800; break;

                case 8:  return  900; break;

                case 9:  return 1000; break;

                case 10: return 1100; break;

                case 11: return 1200; break;

                case 12: return 1300; break;

                case 13: return 1400; break;

                case 14: return 1500; break;

                case 15: return 1600; break;

                default: return 0; break;

        }

}

static uint16_t bom_detector_position (uint8_t num)

{

        return bom_emitter_position (num)-50;

}

void test_bom_bitmask (uint16_t emitter_bitmask, uint16_t detector_bitmask)

{

        usb_puts("# Testing BOM" NL);

        for (uint8_t n=0; n<16; ++n, emitter_bitmask>>=1, detector_bitmask>>=1)

        {

                if (emitter_bitmask&1)

                {

                        uint16_t values[4];

                        usb_puts("# Testing BOM emitter ");

                        usb_puti(n);

                        usb_puts(NL);

                        turntable_rotate_to_position (bom_emitter_position (n));

                        robot_set_bom (1<<n);

                        rbom_update ();

#define N 0

#define E 1

#define S 2

#define W 3

                        values[N]=rbom_read (RBOM_SENSOR_N);

                        values[E]=rbom_read (RBOM_SENSOR_E);

                        values[S]=rbom_read (RBOM_SENSOR_S);

                        values[W]=rbom_read (RBOM_SENSOR_W);

                        robot_set_bom (0);

                        // Send data

                        usb_puts ("data bom emitter");

                        usb_puts (" N/"); usb_puti (values[N]);

                        usb_puts (" E/"); usb_puti (values[E]);

                        usb_puts (" S/"); usb_puti (values[S]);

                        usb_puts (" W/"); usb_puti (values[W]);

#undef N

#undef E

#undef S

#undef W

                        usb_puts (NL);

                }

                if (detector_bitmask&1)

                {

                        usb_puts("# Testing BOM detector ");

                        usb_puti(n);

                        usb_puts(NL);

                        turntable_rotate_to_position (bom_detector_position (n));

                        rbom_set (true);

                        uint16_t on_value=robot_read_bom (n);

                        rbom_set (false);

                        uint16_t off_value=robot_read_bom (n);

                        // Send data

                        usb_puts ("data bom detector ");

                        usb_puti (n);

                        usb_puts (" on/");

                        usb_puti (on_value);

                        usb_puts (" off/");

                        usb_puti (off_value);

                        usb_puts (NL);

                }

        }

        usb_puts("# Testing BOM finished" NL);

}

void test_bom (uint8_t num, bool test_emitter, bool test_detector)

{

        uint16_t emitter_bitmask=0, detector_bitmask=0;

        if (test_emitter  && num<16) emitter_bitmask =1<<num;

        if (test_detector && num<16) detector_bitmask=1<<num;

        test_bom_bitmask (emitter_bitmask, detector_bitmask);

}

void test_bom_all (bool test_emitters, bool test_detectors)

{

        uint16_t emitter_bitmask=0, detector_bitmask=0;

        if (test_emitters ) emitter_bitmask =0xFFFF;

        if (test_detectors) detector_bitmask=0xFFFF;

        test_bom_bitmask (emitter_bitmask, detector_bitmask);

}