Skip to content

wave_rover.tc

WAVE ROVER (Waveshare, ESP32 "General Driver for Robots") driven by TinyC instead of the Waveshare firmware:

Source on GitHub

// @name: WAVE ROVER drive
// WAVE ROVER (Waveshare, ESP32 "General Driver for Robots") driven by TinyC instead of the Waveshare firmware:
// a touch joystick page with a dead-man watchdog, speed limit and battery guard. See docs/WAVE_ROVER.md for the
// hardware, the Tasmota settings (I2C on GPIO 32/33, leave the motor pins UNASSIGNED) and how to flash / go back.
//
//   http://<ip>/rover        the joystick page (touch or mouse; arrow keys / WASD on a computer)
//   http://<ip>/rover_cmd?d=<-100..100>&t=<-100..100>&m=<10..100>   one drive command, answers a small JSON
//
// Safety, in the order it matters:
// - DEAD-MAN: every drive command must be renewed. The page sends one every 100 ms while a finger / key is down;
//   when nothing arrives for WD_MS (300 ms) the motors stop (the Waveshare firmware waits 3 s).
// - The page sends "stop" when the finger lifts, the tab is hidden or the window loses focus.
// - Speed limit m (default 40 %), the page lets you raise it to 100 %.
// - Battery guard: below VBAT_MIN_MV the motors are refused (3S Li-ion; INA219 on I2C 0x42; not checked if absent).
// - A slot stop (CleanUp) and the start of the program switch all motor pins off.
// Motor side A = LEFT, B = RIGHT (as in the Waveshare source, WAVE ROVER). If the rover drives mirrored or backwards,
// change DIR_L / DIR_R below. The N20 motors do not turn below PWM 23 (Waveshare THRESHOLD_PWM): the first step is 23.
// The PWM carrier here is the Arduino default (1 kHz); the Waveshare firmware uses 100 kHz. 1 kHz works but whines.
//
// NOT tested on the rover yet (written from the Waveshare source before the rover arrived).

#define OUTPUT      3

#define PIN_PWMA    25
#define PIN_AIN1    21
#define PIN_AIN2    17
#define PIN_BIN1    22
#define PIN_BIN2    23
#define PIN_PWMB    26

#define DIR_L       1        // -1 turns the left side around
#define DIR_R       1
#define PWM_MIN     23       // lowest PWM at which the N20 motors turn
#define WD_MS       300      // dead-man time
#define VMAX_START  40       // speed limit at start, percent
#define INA_ADDR    0x42     // INA219 on the driver board
#define I2C_BUS     0
#define VBAT_MIN_MV 9300     // 3S Li-ion: stop driving below this (3.1 V per cell)

int drive   = 0;             // -100..100, forward positive
int turn    = 0;             // -100..100, right positive
int vmax    = VMAX_START;    // speed limit in percent (10..100)
int lastcmd = 0;             // millis() of the last drive command
int motL    = 0;             // last PWM left, -255..255
int motR    = 0;
int stops   = 0;             // how often the watchdog stopped a moving rover
int vbat_mv = 0;             // 0 = INA219 not read
int low     = 0;             // 1 = battery guard active
char vb[8];
char cb[24];
char ob[96];

int rover_clamp(int v, int lo, int hi) {
    if (v < lo) { return lo; }
    if (v > hi) { return hi; }
    return v;
}

// percent -100..100 (after the speed limit) -> PWM -255..255 with the dead zone of the motors
int rover_pwm(int pct) {
    int a = pct;
    int p = 0;
    if (a < 0) { a = 0 - a; }
    if (a == 0) { return 0; }
    p = PWM_MIN + ((255 - PWM_MIN) * a) / 100;
    if (pct < 0) { p = 0 - p; }
    return p;
}

void rover_side(int pwm, int pin_pwm, int pin_in1, int pin_in2) {
    int a = pwm;
    if (a < 0) { a = 0 - a; }
    if (pwm == 0) {
        digitalWrite(pin_in1, 0);
        digitalWrite(pin_in2, 0);
        analogWrite(pin_pwm, 0);
    } else if (pwm > 0) {
        digitalWrite(pin_in1, 1);          // forward = IN1 high, IN2 low (Waveshare WAVE ROVER)
        digitalWrite(pin_in2, 0);
        analogWrite(pin_pwm, a);
    } else {
        digitalWrite(pin_in1, 0);
        digitalWrite(pin_in2, 1);
        analogWrite(pin_pwm, a);
    }
}

// mix drive/turn into two side values and put them on the pins
void rover_apply() {
    int l = drive + turn;
    int r = drive - turn;
    int pl = 0;
    int pr = 0;
    l = rover_clamp(l, -100, 100);
    r = rover_clamp(r, -100, 100);
    if (low == 1) {
        l = 0;
        r = 0;
    }
    pl = rover_pwm((l * vmax) / 100) * DIR_L;
    pr = rover_pwm((r * vmax) / 100) * DIR_R;
    if (pl != motL) {
        rover_side(pl, PIN_PWMA, PIN_AIN1, PIN_AIN2);
        motL = pl;
    }
    if (pr != motR) {
        rover_side(pr, PIN_PWMB, PIN_BIN1, PIN_BIN2);
        motR = pr;
    }
}

void rover_off() {
    drive = 0;
    turn = 0;
    motL = 1;                  // force the pins to be written
    motR = 1;
    rover_apply();
}

// battery voltage in mV from the INA219 bus voltage register (13 bit, 4 mV per step); 0 = not readable
int rover_vbat() {
    int raw = 0;
    if (i2cRead(INA_ADDR, 2, vb, 2, I2C_BUS) != 1) { return 0; }
    raw = ((vb[0] & 255) << 8) | (vb[1] & 255);
    return (raw >> 3) * 4;
}

void EverySecond() {
    vbat_mv = rover_vbat();
    if (vbat_mv > 0 && vbat_mv < VBAT_MIN_MV) {
        if (low == 0) { addLog("ROVER: battery low (%d mV), motors off", vbat_mv); }
        low = 1;
        rover_apply();
    } else if (vbat_mv >= VBAT_MIN_MV + 300) {
        low = 0;                                     // 0.3 V hysteresis
    }
}

void Every50ms() {
    if (millis() - lastcmd > WD_MS) {
        if (motL != 0 || motR != 0) {
            stops = stops + 1;
        }
        drive = 0;
        turn = 0;
        rover_apply();
    }
}

void WebCall() {
    webSend("{s}Rover battery{m}");
    sprintf(ob, "%d.%02d V", vbat_mv / 1000, (vbat_mv % 1000) / 10);
    webSend(ob);
    if (low == 1) { webSend(" LOW"); }
    webSend("{e}{s}Rover motors L / R{m}");
    sprintf(ob, "%d / %d  (watchdog stops %d)", motL, motR, stops);
    webSend(ob);
    webSend("{e}{s}Rover page{m}<a href='/rover'>/rover</a>{e}");
}

void rover_page() {
    webSend("<!DOCTYPE html><meta name=viewport content='width=device-width,initial-scale=1'><title>WAVE ROVER</title>");
    webSend("<style>body{font-family:sans-serif;margin:12px;text-align:center}");
    webSend("#pad{width:280px;height:280px;margin:10px auto;border:2px solid #888;border-radius:50%;touch-action:none;position:relative}");
    webSend("#dot{width:40px;height:40px;border-radius:50%;background:#2a7;position:absolute;left:120px;top:120px}</style>");
    webSend("<h3>WAVE ROVER</h3><div id=st>-</div><div id=pad><div id=dot></div></div>");
    webSend("<p>Speed limit <input type=range id=mx min=10 max=100 value=40 oninput='mxv.textContent=this.value'> <span id=mxv>40</span> %</p>");
    webSend("<button onclick='halt()'>STOP</button><p>Arrow keys / WASD also work. Motors stop 0.3 s after the last command.</p>");
    webSend("<script>var d=0,t=0,act=0,tm=null,ks={},p=document.getElementById('pad'),dot=document.getElementById('dot');");
    webSend("function send(){fetch('rover_cmd?d='+d+'&t='+t+'&m='+mx.value).then(function(r){return r.json()}).then(function(j){");
    webSend("st.textContent=j.v+' V   L '+j.l+'   R '+j.r+'   stops '+j.wd+(j.lo?'   LOW BATTERY':'')}).catch(function(){st.textContent='no link'})}");
    webSend("function go(){if(!tm){tm=setInterval(send,100)}send()}");
    webSend("function halt(){d=0;t=0;dot.style.left='120px';dot.style.top='120px';send();clearInterval(tm);tm=null}");
    webSend("function mv(e){var r=p.getBoundingClientRect(),x=(e.clientX-r.left-140)/140,y=(e.clientY-r.top-140)/140,n=Math.hypot(x,y);");
    webSend("if(n>1){x/=n;y/=n}t=Math.round(x*100);d=Math.round(-y*100);dot.style.left=(120+x*120)+'px';dot.style.top=(120+y*120)+'px'}");
    webSend("p.onpointerdown=function(e){p.setPointerCapture(e.pointerId);act=1;mv(e);go()};");
    webSend("p.onpointermove=function(e){if(act)mv(e)};p.onpointerup=function(e){act=0;halt()};p.onpointercancel=p.onpointerup;");
    webSend("function kk(){d=(ks.ArrowUp||ks.w?100:0)-(ks.ArrowDown||ks.s?100:0);t=(ks.ArrowRight||ks.d?60:0)-(ks.ArrowLeft||ks.a?60:0);if(d||t){go()}else{halt()}}");
    webSend("document.onkeydown=function(e){if(!ks[e.key]){ks[e.key]=1;kk()}};document.onkeyup=function(e){ks[e.key]=0;kk()};");
    webSend("window.onblur=function(){ks={};act=0;halt()};document.onvisibilitychange=function(){if(document.hidden){ks={};act=0;halt()}};");
    webSend("</script>");
}

void WebOn() {
    int h = webHandler();
    if (h == 1) {
        rover_page();
    }
    if (h == 2) {
        if (webArg("d", cb) > 0) { drive = rover_clamp(strToInt(cb), -100, 100); }
        if (webArg("t", cb) > 0) { turn = rover_clamp(strToInt(cb), -100, 100); }
        if (webArg("m", cb) > 0) { vmax = rover_clamp(strToInt(cb), 10, 100); }
        lastcmd = millis();
        rover_apply();
        sprintf(ob, "{\"v\":%d.%02d,\"l\":%d,\"r\":%d,\"wd\":%d,\"lo\":%d}", vbat_mv / 1000, (vbat_mv % 1000) / 10, motL, motR, stops, low);
        webSend(ob);
    }
}

void CleanUp() {
    rover_off();
}

int main() {
    pinMode(PIN_PWMA, OUTPUT);
    pinMode(PIN_AIN1, OUTPUT);
    pinMode(PIN_AIN2, OUTPUT);
    pinMode(PIN_PWMB, OUTPUT);
    pinMode(PIN_BIN1, OUTPUT);
    pinMode(PIN_BIN2, OUTPUT);
    rover_off();
    lastcmd = millis();
    webOn(1, "/rover");
    webOn(2, "/rover_cmd");
    addLog("ROVER: ready, page /rover, watchdog %d ms, speed limit %d %%", WD_MS, VMAX_START);
    return 0;
}