Colony

/**

 * @file test_rangefinder.c

 * @brief Contains unit test for rangefinder.c module of libdragonfly

 *

 * Contains a function allowing the user to test whether the rangefinder module

 * works correctly.

 *

 * @author Colony Project, CMU Robotics Club

 **/

/* Testing Procedure

 * - Place hand in front of orbs in 4, 3, 2, 1, 5 order

 * - Observe orbs

 *

 * Expected Behavior:

 * - Orbs flash BLUE 3 times to signal start of test

 * - During test

 *  - Orb 1 changes to reflect which sensor is under test

 *  - Orb 2 stays RED until that sensor gets a usable reading, i.e. within visible distance

 * - Orbs flash PURPLE 3 times to signal end of test

 */

#include <dragonfly_lib.h>

#define TEST_TIME 500        // duration of success indicator (in ms)

#define ON_DELAY 500        // duration of flashes at beginning and end (in ms)

#define OFF_DELAY 250        // delay between flashes at beginning and end (in ms)

int testrangefinder(void) {

    int i;  // index

    int sensor[5] = {IR4, IR3, IR2, IR1, IR5};

    int color[5] = {RED, ORANGE, YELLOW, GREEN, BLUE};

    int ret;

    // flash orbs BLUE 3 times

    for (i = 0; i < 3; i++) {

        orb_set_color(BLUE);

        delay_ms(ON_DELAY);

        orb_set_color(ORB_OFF);

        delay_ms(OFF_DELAY);

    }

    for (i = 0; i < 5; i++) {

        // indicate sensor under test

        orb2_set_color(color[i]);

        // wait for sensor interaction

        while ((ret = range_read_distance(sensor[i])) == -1) {

            // stay RED until IR4 sees something

            orb1_set_color(RED);

            if(ret == -3) { //library not init'd

              while(1) {

                orb2_set_color(GREEN);

                orb1_set_color(RED);

                delay_ms(250);

                orb1_set_color(GREEN);

                orb2_set_color(RED);

                delay_ms(250);

              }

            }

        }

        orb1_set_color(GREEN);

        delay_ms(TEST_TIME);

    }

    // flash orbs PURPLE 3 times

    for (i = 0; i < 3; i++) {

        orb_set_color(PURPLE);

        delay_ms(ON_DELAY);

        orb_set_color(ORB_OFF);

        delay_ms(OFF_DELAY);

    }

    return 0;

}