Skip to content

cliff_test.tc

Bench test for the four floor sensors of the WAVE ROVER (VL53L0X behind a TCA9548A, channels 0..3 = front left, front right, rear left,

Source on GitHub

// @name: CLIFF TEST
// Bench test for the four floor sensors of the WAVE ROVER (VL53L0X behind a TCA9548A, channels 0..3 = front left, front right, rear left,
// rear right). No motors, no cliff logic: it only measures, so it is safe to run with the rover in the hand.
//
//   WebCall rows (main page): per sensor the distance now, the minimum and maximum since the last reset, the share of good readings.
//   console:  CtReset            clear min / max / counters
//             CtLearn            10 s: hold the rover (or one sensor) over the floor you want to drive on; the readings are collected
//                                and a window for  RoverCliff <min> <max>  is proposed (min * 0.7 .. max * 1.4, rounded)
//             CtWin              print the proposal again
//             CtSet <min> <max>  nothing is stored here; the numbers are for  RoverCliff <min> <max>  in rover.tc
//
// HOW TO USE: mount (or hold) the sensors the way they will sit - pointing outward / down about 45 degrees, 40..70 mm above the floor.
// Measure every floor of the house (tiles, wood, dark and shiny carpet, sunlight), then over an edge (stair step, table edge): over a drop
// the reading jumps far above the window or turns into "no result" (shown as -). The window has to contain every floor reading and
// exclude the drop.
//
// NOT yet run on real sensors (they were soldered after this was written).

#include "vl53l0x_mux.tc"

#define I2C_BUS 0

int mn[4];
int mx[4];
int good[4];
int bad[4];
int ch = 0;
int learn_until = 0;
int win_min = 0;
int win_max = 0;
int all_lo = 0;
int all_hi = 0;
char ob[160];

void ct_clear() {
    int i = 0;
    while (i < 4) {
        mn[i] = 65535;
        mx[i] = 0;
        good[i] = 0;
        bad[i] = 0;
        i = i + 1;
    }
}

void ct_proposal() {
    int lo = 65535;
    int hi = 0;
    int i = 0;
    while (i < 4) {
        if (good[i] > 0) {
            if (mn[i] < lo) { lo = mn[i]; }
            if (mx[i] > hi) { hi = mx[i]; }
        }
        i = i + 1;
    }
    all_lo = lo;
    all_hi = hi;
    if (hi == 0) { all_lo = 0; win_min = 0; win_max = 0; return; }
    win_min = (lo * 7) / 10;
    win_max = (hi * 14) / 10;
    win_min = (win_min / 5) * 5;
    win_max = ((win_max + 4) / 5) * 5;
}

void TaskLoop() {
    int d = 0;
    d = vl_poll(ch);
    if (d == 65535) {
        bad[ch] = bad[ch] + 1;
    } else {
        good[ch] = good[ch] + 1;
        if (d < mn[ch]) { mn[ch] = d; }
        if (d > mx[ch]) { mx[ch] = d; }
    }
    ch = (ch + 1) & 3;
    if (learn_until != 0 && millis() > learn_until) {
        learn_until = 0;
        ct_proposal();
        if (win_max == 0) {
            addLog("CLIFFTEST: nothing measured, is a sensor connected?");
        } else {
            addLog("CLIFFTEST: floor readings %d..%d mm -> try  RoverCliff %d %d", all_lo, all_hi, win_min, win_max);
        }
    }
    delay(5);
}

void WebCall() {
    int i = 0;
    int tot = 0;
    while (i < 4) {
        tot = good[i] + bad[i];
        if (vl_up[i] == 0) {
            sprintf(ob, "{s}Sensor %d{m}not found{e}", i);
        } else if (vl_dist[i] == 65535) {
            sprintf(ob, "{s}Sensor %d{m}-   (min %d  max %d  good %d of %d){e}", i, mn[i] < 65535 ? mn[i] : 0, mx[i], good[i], tot);
        } else {
            sprintf(ob, "{s}Sensor %d{m}%d mm   (min %d  max %d  good %d of %d){e}", i, vl_dist[i], mn[i], mx[i], good[i], tot);
        }
        webSend(ob);
        i = i + 1;
    }
    if (learn_until != 0) {
        webSend("{s}Learning{m}hold the sensors over the floor ...{e}");
    } else if (win_max != 0) {
        sprintf(ob, "{s}Window proposal{m}RoverCliff %d %d{e}", win_min, win_max);
        webSend(ob);
    }
}

void Command(char cmd[]) {
    if (strcmp(cmd, "RESET") == 0) {
        ct_clear();
        win_min = 0;
        win_max = 0;
        responseCmnd("cleared");
    } else if (strcmp(cmd, "LEARN") == 0) {
        ct_clear();
        win_min = 0;
        win_max = 0;
        learn_until = millis() + 10000;
        responseCmnd("learning for 10 s");
    } else if (strcmp(cmd, "WIN") == 0) {
        ct_proposal();
        sprintf(ob, "RoverCliff %d %d", win_min, win_max);
        responseCmnd(ob);
    } else {
        responseCmnd("CtReset, CtLearn, CtWin");
    }
}

void CleanUp() {
    vl_release();
}

int main() {
    int i = 0;
    ct_clear();
    addCommand("Ct");
    if (vl_mux_init(I2C_BUS) == 1) {
        vl_timeout = 60;
        while (i < 4) {
            if (vl_start(i) == 0) { addLog("CLIFFTEST: floor sensor %d not found", i); }
            i = i + 1;
        }
        addLog("CLIFFTEST: multiplexer found, %d %d %d %d (1 = sensor up)", vl_up[0], vl_up[1], vl_up[2], vl_up[3]);
    } else {
        addLog("CLIFFTEST: no TCA9548A at 0x70 - a single sensor on channel 0 is tried");
        vl_timeout = 60;
        if (vl_start(0) == 0) { addLog("CLIFFTEST: no sensor"); }
    }
    return 0;
}