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

 * - Position robot facing you

 * - Move hand in front of IR2

 * - Observe orbs

 *

 * Expected Behavior:

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

 * - During test

 *  - Orbs are GREEN when hand is at visible distance

 *  - Orbs are RED when hand is too close or too far to see

 *  - Orbs are never YELLOW

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

 */

#include <dragonfly_lib.h>

#define TEST_TIME 10        // duration of test (in s)

#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() {

    int start_time; // rtc time when test starts

    int distance;    // rangefinder reading

    int i;  // index

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

    }

    start_time = rtc_get(); // get test's start time

    // orbs RED when IR2 reads -1

    // orbs GREEN when IR2 gets a useable reading

    // times out after TEST_TIME elapsed

    while (1) {

        distance = range_read_distance(IR5);

        // unusable reading

        if (distance == -1)

            orb_set_color(RED);

        // invalid reading

        else if (distance < 101 || distance > 800)

            orb_set_color(YELLOW);

        // useable reading

        else

            orb_set_color(GREEN);

    }

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

}