Colony

#include "test_bom.h"

#include "global.h"

#include "hardware_turntable.h"

#include "hardware_ibom.h"

#include "comm_robot.h"

static uint16_t bom_emitter_position (uint8_t num)

{

        // 0 is the robot facing the station (BOM #4 aligned with IBOM).

        // Positive is counter-clockwise on the bom (robot turning clockwise)

        // The difference in angle between two positions. 1024 steps on a full turn, 16 emitters/detectors.

#define increment 1024/16

        switch (num)

        {

                case  0: return (-4)*increment; break;

                case  1: return (-3)*increment; break;

                case  2: return (-2)*increment; break;

                case  3: return (-1)*increment; break;

                case  4: return ( 0)*increment; break;

                case  5: return ( 1)*increment; break;

                case  6: return ( 2)*increment; break;

                case  7: return ( 3)*increment; break;

                case  8: return ( 4)*increment; break;

                case  9: return ( 5)*increment; break;

                case 10: return ( 6)*increment; break;

                case 11: return ( 7)*increment; break;

                case 12: return ( 8)*increment; break;

                case 13: return ( 9)*increment; break;

                case 14: return (10)*increment; break;

                case 15: return (11)*increment; break;

                default: return 0; break;

        }

#undef increment

}

static uint16_t bom_detector_position (uint8_t num)

{

        // Austin sez: detectors and emitters are supposed to be parallel.

        return bom_emitter_position (num);

}

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[3];

                        usb_puts("# Testing BOM emitter ");

                        usb_puti(n);

                        usb_puts(NL);

                        turntable_rotate_to_position (bom_emitter_position (n));

                        robot_set_bom (1<<n);

                        ibom_update ();

#define TOP 0

#define LEFT 1

#define RIGHT 2

                        ibom_read (&values[TOP], &values[LEFT], &values[RIGHT]);

                        robot_set_bom (0);

                        // Send data

                        usb_puts ("data bom emitter");

                        usb_puts (" top/"  ); usb_puti (values[TOP  ]);

                        usb_puts (" left/" ); usb_puti (values[LEFT ]);

                        usb_puts (" right/"); usb_puti (values[RIGHT]);

#undef TOP

#undef LEFT

#undef RIGHT

                        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));

                        uint16_t on_value, off_value;

                        ibom_set (true);

                        robot_read_bom (n, &on_value);

                        ibom_set (false);

                        robot_read_bom (n, &off_value);

                        // 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);

                }

        }

        turntable_rotate_to_position(TURNTABLE_HOME_POSITION);

        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);

}