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

 * - Wait for initial BLUE flashes

 * - Position hand at ~12cm from leftmost, rearmost rangefinder (IR4)

 * - Sweep hand around front of robot, maintaining distance from rangefinders

 * - Observe orbs

 *

 * Expected Behavior:

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

 * - Orbs turn RED when IR4 gets a useable reading

 * - Orbs turn ORANGE when IR1 gets a useable reading

 * - Orbs turn YELLOW when IR2 gets a useable reading

 * - Orbs turn GREEN when IR3 gets a useable reading

 * - Orbs turn BLUE when IR5 gets a useable reading

 */

#include <dragonfly_lib.h>

#define delay 750   // delay between sensors (in ms)

int test_rangefinder() {

    uint8_t i, ir, color;   // plain old index, rangefinder index, color index

    int detectors[5] = {IR4, IR1, IR2, IR3, IR5};   // rangefinders L to R

    int colors[5] = {RED, ORANGE, YELLOW, GREEN, BLUE};        // colors of rainbow

    // flash orbs BLUE 3 times

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

        orb_set_color(BLUE);

        delay_ms(500);

        orb_set_color(ORB_OFF);

        delay_ms(250);

    }

    // show appropriate color when sensor gets a good read, then wait a second

    // do this for each sensor in left-to-right order

    for (ir = 1, color = 0; ir <= 5; ir++, color++) {

        while (range_read_distance(ir) == -1);

        orb_set_color(color);

        delay_ms(DELAY);

    }

    return 0;

}