rover.tc¶
ONE program for the whole WAVE ROVER driver board (ESP32-WROOM-32UE, no PSRAM): motors, battery, IMU, compass, four floor
// @name: WAVE ROVER
// ONE program for the whole WAVE ROVER driver board (ESP32-WROOM-32UE, no PSRAM): motors, battery, IMU, compass, four floor
// sensors against a staircase, the touch joystick page and the display. One slot instead of several, because every TinyC
// slot costs about 9 KB of RAM on its own (measured on a classic ESP32, see docs/WAVE_ROVER.md).
//
// http://<ip>/rover joystick page (touch or mouse; arrow keys / WASD)
// http://<ip>/rover_cmd?d=<-100..100>&t=<-100..100>&m=<10..100> one drive command, answers a small JSON
// console: RoverEnc [0|1], RoverEncZero, RoverEncCal <mm>, RoverEncTpr <n>, RoverCalGyro, RoverYaw0, RoverCalMag 1|0, RoverCliff [0|1|<min> <max>], RoverObst [0|1|<mm>], RoverWander [<seconds>|0], RoverPark [0|1], RoverTof <ch> <mm> (-1 = silent sensor), RoverVbat <mV>
//
// SAFETY, in the order it matters:
// 1. STAIRCASE: four VL53L0X floor sensors (TCA9548A channels 0..3 = rear left, rear right, front right, front left (as wired on gemu's rover, 17.11.2026), looking
// outward / down about 45 degrees). A sensor sees the floor when its distance is inside the window cl_min..cl_max (mm). A reading
// outside it (nothing returned = a drop) or a sensor that is missing / silent for CLIFF_STALE_MS counts as a CLIFF. Then:
// front cliff -> only straight reverse, rear cliff -> only straight forward, both (or a sensor fault) -> no movement at all.
// This runs in this program, independent of anything that sends commands. RoverCliff 0 switches the protection off (log warning).
// 1b. OBSTACLE BRAKE (lidar, ROVER_LIDAR 1): ahead = lidar 240..300 deg, behind = 60..120 (measured with a hand in front of the nose). An echo closer than
// OBST_MM (250 mm, released at +50) refuses driving toward it; turning stays free; a silent lidar holds the motors (RoverObst 0 = off). d > 0 drives toward
// the NOSE (the end with FL / FR); the encoder ticks count negative for forward. Checked on the rover 17.11.2026: brake at 250 mm, stood at 236 mm
// (40 %, 0.17 m/s); the staircase guard on a table edge in front and behind.
// 2. DEAD-MAN: a drive command must be renewed within WD_MS (300 ms), else the motors stop. The page sends one every 100 ms.
// 3. Battery guard: below VBAT_MIN_MV the motors are refused (3S Li-ion, INA219 at 0x42).
// 4. A slot stop (CleanUp) and the start of the program switch all motor pins off.
// 4b. MOTOR STALL (wheel encoders, on by default since 17.11.2026, RoverEnc 0 switches it off): a side that is commanded but gives no tick for ENC_STALL_MS stops the rover until
// the stick is released (see common/wheel_enc.tc for wiring, counters and calibration).
// 5. PARK MODE (RoverPark 1, or /rover_cmd?p=1): for a rover that stands still, on its charger or not. The motors are refused, the
// floor sensors, IMU, compass and the display are no longer served (the chips keep running, only the I2C traffic and the display
// stop), the loop sleeps, only the battery is read (every 5 s). A drive command wakes the rover (or RoverPark 0, /rover_cmd?p=0):
// the floor sensors then have to answer again before any movement is allowed. PARK_AUTO_S > 0 parks it after that many seconds
// without a command. The big consumers are not in this program: switch the lidar off on its own 5 V switch and let the S3 sleep.
// 6. SPEED CONTROL (RoverSpeed <mm/s>): with the wheel encoders the speed can be set exactly instead of a PWM share. RoverSpeed 30 means
// a full stick deflection drives each wheel at 30 mm/s, half a stick 15 mm/s; a PI controller per wheel keeps it there by the ticks of
// the last 200 ms (the slider "m" is not used then). RoverSpeed 0 = back to the open loop. Below SPD_PULSE_BELOW (24 mm/s) a wheel is
// driven in bursts of 13 PWM (0.8 s period, the duty follows the measured mean speed): measured on the rover, unloaded, over 14 s: 15 mm/s
// -> 16.6, 8 -> 9.8, 4 -> 5.4 mm/s. The motion is jerky there, only the mean is controlled. RoverSpeed shows the measured speeds.
// Motor side A = LEFT, B = RIGHT. Forward = IN1 high, IN2 low. Below PWM 23 the N20 motors do not turn.
//
// DISPLAY: the first 4 lines (21 characters, font 6x8, 128x32 pixel block) are what the Waveshare firmware shows on its OLED
// (title, IP address, state, battery); the same code works on the SSD1306 of the rover. DSP_EXTRA 1 adds a larger panel with the rest
// on a TFT.
//
// TEST WITHOUT THE ROVER: ROVER_SIM 1 does not touch the motor pins, takes the battery voltage from RoverVbat and the four distances
// from RoverTof (default 60 mm), so the display, the web page and the safety logic can be tried on any ESP32.
// Tested on the rover 17.11.2026: floor sensors, lidar, drive, brake, joystick page (not at higher speed).
#define ROVER_SIM 0 // 1 = simulated battery / floor sensors, no motor pins
#define ROVER_LIDAR 1 // 1 = LD06/LD19 lidar on LIDAR_RX (about 10 kB RAM, a fifth of the VM time); 0 = off
#define LIDAR_RX 4 // GPIO of the lidar TX line: IO4 of the 10-pin header (checked 17.11.2026; TX on IO5 stopped the lidar). IO27 would be encoder channel B
#define OBST_MM 250 // obstacle brake: stop driving toward anything closer than this (mm); RoverObst <mm> changes it, RoverObst 0 switches it off
#define OBST_HYST 50 // released again only at OBST_MM + this (no flicker at the edge)
#define OBST_FRONT_A0 240 // lidar degrees of "ahead" (measured 17.11.2026: a hand in front of the nose shows at 240..299, centre 270)
#define OBST_FRONT_A1 300
#define OBST_REAR_A0 60 // "behind" = opposite
#define OBST_REAR_A1 120
#define ROVER_ESPNOW 0 // 1 = ESP-NOW link (radio, no router): a cam module sends the same text commands as the UDP link ("d=40 t=0 m=40 p=0"), the rover answers
// with a short status. Without ESPNOW_ENC anyone in radio range can drive the rover (log line "OPEN"); with ESPNOW_ENC 1 only ESPNOW_PEER can
#define ESPNOW_ENC 0 // 1 = encrypted (needs ESPNOW_KEY of 16 characters and the sender's MAC in ESPNOW_PEER, the sender sets the same key and lists the rover)
#define ESPNOW_KEY "0123456789abcdef"
#define ESPNOW_PEER "AA:BB:CC:DD:EE:FF"
#define ROVER_LINK 1 // 1 = UDP link: a cam module (or any program) sends drive commands to LINK_PORT, the rover answers with its status
#define LINK_PORT 5006
#define DSP_EXTRA 1 // 1 = extra panel below the 4 OLED lines (TFT)
#if ROVER_SIM
#define ROVER_REAL 0
#else
#define ROVER_REAL 1
#endif
#include "ina219.tc"
#include "qmi8658.tc"
#include "ak09918.tc"
#include "vl53l0x_mux.tc"
#if ROVER_SIM
#define ENC_SIM 1
#endif
#define ENC_STALL_MS 1500 // (800 before 18.11.2026) the speed controller needs about a second to bring a standing wheel from the minimum PWM up to its breakaway
#define ENC_ON 1 // the stall guard is on (encoders wired 13.11.2026): a side commanded but without a tick for 800 ms stops the rover
#include "wheel_enc.tc"
#if ROVER_LIDAR
#define LID_TX_PIN 13 // the lidar sends nothing: GPIO13 (SD slot MOSI, unused) is only the UART's dummy tx. With tx = -1 the UART takes GPIO17 = left motor AIN2 and the left wheel stops (found 17.11.2026)
#include "ld19.tc"
#endif
#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
#define WD_MS 300
#define VMAX_START 40
#define INA_ADDR 0x42
#define I2C_BUS 0
#define TOF_BUS 0 // bus of the floor sensors' multiplexer: 0 = the board's bus (GPIO32 / 33, since 17.11.2026); 1 = the SECOND Tasmota I2C bus on GPIO4 (SDA) / GPIO5 (SCL), set with `Gpio4 641`, `Gpio5 609`
#define VBAT_MIN_MV 9300
#define CLIFF_MIN 40 // floor window (mm along the beam); set to your mounting with RoverCliff <min> <max>. The floor reads 51..73 mm here: a low obstacle ahead shortens the reading under 40 and counts as a cliff (25 before 17.11.2026)
#define CLIFF_MAX 110
#define CLIFF_STALE_MS 1000 // a sensor that did not answer for this long is a fault
#define SPD_WIN_MS 200 // speed control: the ticks of this window give the measured speed
#define SPD_PWM_LO 5 // lowest PWM the speed controller may use
#define SPD_PULSE_BELOW 24 // below this speed (mm/s) a wheel is driven in bursts (pulse mode), at or above it the PI controller
#define SPD_BURST_PWM 13 // PWM during a burst (the wheel keeps turning at about 30 mm/s with it once it runs)
#define SPD_KICK_PWM 22 // PWM of the first 50 ms of a burst: gets the wheel out of standstill
#define SPD_VON 30.0 // speed during a burst, mm/s (start value of the duty; the controller corrects it)
#define SPD_PER 16 // pulse period in 50 ms steps (16 = 0.8 s)
#define SPD_KD 0.5 // duty correction per period for a speed error equal to SPD_VON
#define SPD_KP 0.03 // PWM per mm/s of speed error change
#define SPD_KI 0.05 // PWM per mm/s of speed error per window
#define PARK_AUTO_S 0 // > 0: park after this many seconds without a drive command (0 = never)
int drive = 0;
int turn = 0;
int vmax = VMAX_START;
int lastcmd = 0;
int motL = 0;
int motR = 0;
int stops = 0;
int vbat_mv = 0;
int low = 0;
int why = 0; // why the motors are held: 0 free, 1 battery, 2 sensor fault, 3 front cliff, 4 rear cliff, 5 both, 6 parked, 7 motor stall, 8 lidar silent, 9 obstacle ahead, 10 obstacle behind
int park = 0; // 1 = parked (see header)
int spd_full = 0; // RoverSpeed: mm/s at a full stick (0 = open loop)
int spd_t0 = 0;
int spd_last[2];
int spd_pct[2]; // signed share of each side (-100..100) as rover_apply() decided it
float spd_v[2]; // measured speed per side, mm/s
float spd_out[2]; // controller output (PWM magnitude), 0 = side at rest
float spd_err[2];
int spd_mode[2]; // 1 = pulse mode (bursts), 0 = PI controller
int spd_pls[2]; // pulse mode: PWM of this 50 ms step (0 = pause)
float spd_duty[2]; // pulse mode: share of the period the wheel runs
int spd_ph = 0; // pulse mode: step in the period
int spd_t1 = 0;
int spd_last1[2];
int cl_on = 1;
int cl_min = CLIFF_MIN;
int cl_max = CLIFF_MAX;
int cl_flag[4]; // 1 = no floor in the window
int obs_on = 1; // obstacle brake (RoverObst 0 = off)
int obs_mm = OBST_MM;
int obs_front = 0; // 1 = something closer than obs_mm ahead
int obs_rear = 0;
int obs_fault = 0; // 1 = the lidar sends nothing (the brake cannot see)
int obs_df = 0; // nearest distance ahead / behind in mm (0 = nothing seen)
int obs_dr = 0;
int wan_on = 0; // RoverWander is running (declared here: the links below must know it)
int cl_front = 0;
int cl_rear = 0;
int cl_fault = 1; // 1 until all four sensors answered
int tof_ch = 0;
int sim_tof[4];
int sim_vbat = 11800;
int ina_next = 0;
int dsp_n = 0;
char ob[160];
char cb[24];
char ip[20];
char dl[40];
int rover_clamp(int v, int lo, int hi) {
if (v < lo) { return lo; }
if (v > hi) { return hi; }
return v;
}
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 ROVER_REAL
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);
digitalWrite(pin_in2, 0);
analogWrite(pin_pwm, a);
} else {
digitalWrite(pin_in1, 0);
digitalWrite(pin_in2, 1);
analogWrite(pin_pwm, a);
}
#endif
}
// speed control: the PWM of one side for a signed share pct (-100..100); a side that starts is given the minimum PWM first
int spd_pwm(int s, int pct) {
int p = 0;
int a = pct;
float tg = 0.0;
if (a < 0) { a = 0 - a; }
if (a == 0) {
spd_out[s] = 0.0;
spd_err[s] = 0.0;
spd_pls[s] = 0;
spd_mode[s] = 0;
return 0;
}
tg = (float)a * (float)spd_full / 100.0;
if (tg < (float)SPD_PULSE_BELOW) {
if (spd_mode[s] == 0) { // enter pulse mode: start with the duty the nominal burst speed gives
spd_mode[s] = 1;
spd_duty[s] = tg / SPD_VON;
spd_out[s] = 0.0;
spd_last1[s] = enc_raw(s);
spd_t1 = millis();
spd_ph = 0;
spd_pls[s] = SPD_KICK_PWM;
}
p = spd_pls[s];
} else {
if (spd_mode[s] == 1) { spd_mode[s] = 0; spd_out[s] = 0.0; spd_pls[s] = 0; }
if (spd_out[s] == 0.0) {
spd_out[s] = (float)PWM_MIN;
spd_last[s] = enc_raw(s);
spd_t0 = millis();
}
p = (int)spd_out[s];
}
if (pct < 0) { p = 0 - p; }
return p;
}
// the one place where commands become motor values; every safety rule is applied here
void rover_apply() {
int d = drive;
int t = turn;
int l = 0;
int r = 0;
int pl = 0;
int pr = 0;
why = 0;
if (low == 1) { d = 0; t = 0; why = 1; }
if (cl_on == 1) {
if (cl_fault == 1) {
d = 0; t = 0; why = 2;
} else if (cl_front == 1 && cl_rear == 1) {
d = 0; t = 0; why = 5;
} else if (cl_front == 1) {
if (d > 0) { d = 0; }
t = 0; why = 3;
} else if (cl_rear == 1) {
if (d < 0) { d = 0; }
t = 0; why = 4;
}
}
if (obs_on == 1) {
if (obs_fault == 1) {
d = 0; t = 0;
if (why == 0) { why = 8; }
} else {
if (obs_front == 1 && d > 0) { d = 0; if (why == 0) { why = 9; } }
if (obs_rear == 1 && d < 0) { d = 0; if (why == 0) { why = 10; } }
}
}
if (enc_on == 1 && enc_stall != 0) { d = 0; t = 0; why = 7; }
if (park == 1) { d = 0; t = 0; why = 6; }
// d > 0 = toward the NOSE, the end with the floor sensors FL / FR (lidar 270 degrees). The motors turn the other way for a positive
// value, so d is negated here (17.11.2026: a drive test took the rover AWAY from the obstacle the brake watched); turning is unchanged.
l = rover_clamp(t - d, -100, 100);
r = rover_clamp(0 - d - t, -100, 100);
spd_pct[0] = l;
spd_pct[1] = r;
if (spd_full > 0) {
pl = spd_pwm(0, l) * DIR_L;
pr = spd_pwm(1, r) * DIR_R;
} else {
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;
motR = 1;
rover_apply();
}
// speed controller, called every 50 ms: returns 1 when a PWM changed (the caller then applies it)
int spd_ctl() {
int now = millis();
int dt = now - spd_t0;
int s = 0;
int raw = 0;
int d = 0;
int a = 0;
int ch = 0;
int on = 0;
int np = 0;
float tg = 0.0;
float err = 0.0;
float v = 0.0;
if (spd_full <= 0) { return 0; }
// pulse mode: the phase of the period decides, the period's mean speed corrects the duty
spd_ph = spd_ph + 1;
if (spd_ph >= SPD_PER) {
spd_ph = 0;
dt = now - spd_t1;
spd_t1 = now;
if (dt < 1) { dt = 1; }
s = 0;
while (s < 2) {
if (spd_mode[s] == 1) {
raw = enc_raw(s);
d = raw - spd_last1[s];
spd_last1[s] = raw;
if (d < 0) { d = 0; }
v = (float)d * enc_mm_tick * 1000.0 / (float)dt;
spd_v[s] = v;
a = spd_pct[s];
if (a < 0) { a = 0 - a; }
tg = (float)a * (float)spd_full / 100.0;
spd_duty[s] = spd_duty[s] + SPD_KD * (tg - v) / SPD_VON;
if (spd_duty[s] < 0.03) { spd_duty[s] = 0.03; }
if (spd_duty[s] > 1.0) { spd_duty[s] = 1.0; }
}
s = s + 1;
}
}
s = 0;
while (s < 2) {
if (spd_mode[s] == 1 && spd_pct[s] != 0) {
on = 0;
if ((float)spd_ph < spd_duty[s] * (float)SPD_PER) { on = 1; }
np = 0;
if (on == 1) {
np = SPD_BURST_PWM;
if (spd_pls[s] == 0) { np = SPD_KICK_PWM; }
}
if (np != spd_pls[s]) {
spd_pls[s] = np;
ch = 1;
}
}
s = s + 1;
}
// PI controller for the sides that run at 24 mm/s or more, once per window
dt = now - spd_t0;
if (dt < SPD_WIN_MS) { return ch; }
spd_t0 = now;
s = 0;
while (s < 2) {
if (spd_mode[s] == 0) {
raw = enc_raw(s);
d = raw - spd_last[s];
spd_last[s] = raw;
if (d < 0) { d = 0; }
spd_v[s] = (float)d * enc_mm_tick * 1000.0 / (float)dt;
a = spd_pct[s];
if (a < 0) { a = 0 - a; }
if (a > 0 && spd_out[s] > 0.0) {
tg = (float)a * (float)spd_full / 100.0;
err = tg - spd_v[s];
if (spd_v[s] < 0.3 * tg && spd_out[s] < 90.0) {
spd_out[s] = spd_out[s] + 12.0; // a wheel that has not got going yet (breakaway of the tyres on tiles needs about 40..60 PWM): ramp instead of the slow PI
spd_err[s] = err;
} else {
spd_out[s] = spd_out[s] + SPD_KP * (err - spd_err[s]) + SPD_KI * err;
spd_err[s] = err;
}
if (spd_out[s] < (float)SPD_PWM_LO) { spd_out[s] = (float)SPD_PWM_LO; }
if (spd_out[s] > 255.0) { spd_out[s] = 255.0; }
ch = 1;
}
}
s = s + 1;
}
return ch;
}
// lidar obstacle brake: nearest echo ahead / behind against obs_mm; returns 1 when the picture changed (then the motors must be re-evaluated)
int obst_eval() {
int f = 0;
int r = 0;
int fault = 0;
int df = 0;
int dr = 0;
int lf = obs_mm;
int lr = obs_mm;
#if ROVER_LIDAR
if (lid_ok() == 0) {
fault = 1;
} else {
df = lid_min(OBST_FRONT_A0, OBST_FRONT_A1);
dr = lid_min(OBST_REAR_A0, OBST_REAR_A1);
if (obs_front == 1) { lf = obs_mm + OBST_HYST; }
if (obs_rear == 1) { lr = obs_mm + OBST_HYST; }
if (df > 0 && df < lf) { f = 1; }
if (dr > 0 && dr < lr) { r = 1; }
}
#endif
obs_df = df;
obs_dr = dr;
if (f == obs_front && r == obs_rear && fault == obs_fault) { return 0; }
obs_front = f;
obs_rear = r;
obs_fault = fault;
return 1;
}
int obs_mask() {
return obs_front + obs_rear * 2 + obs_fault * 4;
}
// evaluate the four floor sensors; returns 1 when the picture changed (then the motors must be re-evaluated)
int cliff_eval() {
int ch = 0;
int d = 0;
int up = 0;
int age = 0;
int f = 0;
int fr = 0;
int re = 0;
int fault = 0;
int changed = 0;
while (ch < 4) {
up = vl_up[ch];
#if ROVER_SIM
up = 1;
#endif
age = millis() - vl_age[ch];
d = vl_dist[ch];
f = 0;
if (up == 0 || age > CLIFF_STALE_MS || age < 0) {
fault = 1;
f = 1;
} else if (d < cl_min || d > cl_max) {
f = 1;
}
if (f != cl_flag[ch]) { changed = 1; }
cl_flag[ch] = f;
if (f == 1) {
if (ch >= 2) { fr = 1; } else { re = 1; } // channels 2, 3 = front; 0, 1 = rear
}
ch = ch + 1;
}
if (fault != cl_fault || fr != cl_front || re != cl_rear) { changed = 1; }
cl_fault = fault;
cl_front = fr;
cl_rear = re;
return changed;
}
// all four floor sensors per call, without waiting (vl_service starts / collects; before 18.11.2026 ONE sensor per call, 43 ms each, so a sensor was
// refreshed only every 600..800 ms and the cliff guard reacted after 12 cm of travel)
void tof_step() {
int ch = 0;
while (ch < 4) {
#if ROVER_SIM
if (sim_tof[ch] >= 0) { // a negative value simulates a silent sensor (RoverTof <ch> -1)
vl_dist[ch] = sim_tof[ch];
vl_age[ch] = millis();
}
#else
vl_service(ch);
#endif
ch = ch + 1;
}
}
int tl_cnt = 0;
int tl_rate = 0;
int tl_t = 0;
// the command links are polled several times per pass of TaskLoop (every step below can take 5..40 ms): the dead-man (WD_MS 300 ms) needs a command to be
// noticed within about 50 ms, and a callback (Every50ms) gets the VM only while this loop sleeps
void link_poll_all() {
#if ROVER_LINK
lk_poll();
#endif
#if ROVER_ESPNOW
en_poll();
#endif
fast_tasks();
}
void TaskLoop() {
tl_cnt = tl_cnt + 1;
if (millis() - tl_t >= 1000) { tl_rate = tl_cnt; tl_cnt = 0; tl_t = millis(); }
if (park == 0) {
#if ROVER_LIDAR
lid_poll(); // the ring holds 200 ms: poll between the slow steps
#endif
link_poll_all();
tof_step();
#if ROVER_LIDAR
lid_poll();
#endif
link_poll_all();
if (cliff_eval() == 1) {
if (cl_fault == 0 && cl_on == 1) { addLog("ROVER: cliff F%d%d R%d%d", cl_flag[3], cl_flag[2], cl_flag[0], cl_flag[1]); }
rover_apply();
}
if (obst_eval() == 1) {
if (obs_on == 1) { addLog("ROVER: obstacle ahead %d behind %d lidar fault %d (nearest %d / %d mm)", obs_front, obs_rear, obs_fault, obs_df, obs_dr); }
rover_apply();
}
qmi_poll();
link_poll_all();
ak_poll();
#if ROVER_LIDAR
lid_poll();
#endif
link_poll_all();
#if ROVER_LINK && ROVER_LIDAR
lk_scan();
#endif
}
if (millis() >= ina_next) {
if (park == 1) { ina_next = millis() + 5000; } else { ina_next = millis() + 1000; }
#if ROVER_SIM
vbat_mv = sim_vbat;
#else
ina_poll();
vbat_mv = 0;
if (ina_ok == 1) { vbat_mv = ina_mv; }
#endif
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) {
if (low == 1) { low = 0; rover_apply(); }
}
}
if (park == 1) { delay(250); } else { delay(5); }
}
// ---- display: the 4 OLED lines of the Waveshare firmware, then the extra panel -----------------------------------------
void dsp_line(int x, int y, int n, char t[]) {
dspPos(x, y);
dspPad(n);
dspDraw(t);
}
void dsp_state(char t[]) {
if (why == 1) { strcpy(t, "LOW BATTERY"); }
else if (why == 2) { strcpy(t, "FLOOR SENSOR FAULT"); }
else if (why == 3) { strcpy(t, "CLIFF AHEAD"); }
else if (why == 4) { strcpy(t, "CLIFF BEHIND"); }
else if (why == 5) { strcpy(t, "CLIFF F+R"); }
else if (why == 6) { strcpy(t, "PARKED"); }
else if (why == 7) { strcpy(t, "MOTOR STALL"); }
else if (why == 8) { strcpy(t, "LIDAR SILENT"); }
else if (why == 9) { strcpy(t, "OBSTACLE AHEAD"); }
else if (why == 10) { strcpy(t, "OBSTACLE BEHIND"); }
else if (motL == 0 && motR == 0) { strcpy(t, "STOP"); }
else { sprintf(t, "L%d R%d", motL, motR); }
}
// Colour helper: the 128x32 OLED is 1 bpp, there dspColor(0xFFFF,..) draws nothing; any colour -> 1
void dcol(int fg) {
if (dspWidth() <= 128) { dspColor(1, 0); } else { dspColor(fg, 0x0000); }
}
// Li-ion charge in percent from the pack voltage (3S, per-cell curve, resting voltage; under motor load it reads lower).
// 3.10 V/cell = 0 % = same as VBAT_MIN_MV, where the motors are refused.
int lerp(int v, int v0, int v1, int p0, int p1) {
return p0 + (v - v0) * (p1 - p0) / (v1 - v0);
}
int bat_pct(int mv) {
int c = mv / 3;
if (mv <= 0) { return -1; }
if (c >= 4200) { return 100; }
if (c >= 4000) { return lerp(c, 4000, 4200, 80, 100); }
if (c >= 3800) { return lerp(c, 3800, 4000, 55, 80); }
if (c >= 3600) { return lerp(c, 3600, 3800, 25, 55); }
if (c >= 3400) { return lerp(c, 3400, 3600, 8, 25); }
if (c >= 3100) { return lerp(c, 3100, 3400, 0, 8); }
return 0;
}
int dpage = 0; // OLED page counter: battery + IP page for 4 updates, then the state page for 2
// 128x32 OLED, big text (size 2 = 10 characters per line). Page 1: battery in percent + IP. Page 2: state.
void display_mono() {
int ph = dpage % 6;
dpage++;
dspFont(0);
dcol(0xFFFF);
if (ph == 0 || ph == 4) { dspClear(); } // clear on every page switch (pads differ)
if (ph == 0) { tasmInfo(2, ip); } // the IP is not known yet when the program starts at boot
if (ph < 4) {
dspSize(2);
if (vbat_mv > 0) { sprintf(dl, "BAT %d%%", bat_pct(vbat_mv)); } else { strcpy(dl, "BAT --"); }
dsp_line(0, 0, 10, dl);
dspSize(1);
sprintf(dl, "IP %s", ip);
dsp_line(0, 20, 21, dl);
} else {
dsp_state(dl);
if (strlen(dl) <= 10) {
dspSize(2);
dsp_line(0, 8, 10, dl);
} else {
dspSize(1);
dsp_line(0, 12, 21, dl);
}
}
}
void display_update() {
if (dspWidth() <= 128) { display_mono(); return; }
dspFont(0);
dspSize(1);
dcol(0xFFFF);
dsp_line(2, 1, 21, "WAVE ROVER");
sprintf(dl, "IP:%s", ip);
dsp_line(2, 9, 21, dl);
dsp_state(dl);
dsp_line(2, 17, 21, dl);
if (vbat_mv > 0) {
sprintf(dl, "V:%d.%02d I:%d mA", vbat_mv / 1000, (vbat_mv % 1000) / 10, ina_ma);
} else {
strcpy(dl, "V:--");
}
dsp_line(2, 25, 21, dl);
#if DSP_EXTRA
{
dspFont(0);
dspSize(2);
dcol(0xFFFF);
sprintf(dl, "Yaw %.0f Hdg %.0f", qmi_yaw, ak_heading());
dsp_line(2, 40, 20, dl);
sprintf(dl, "Gz %.1f T %.1f C", qmi_gz, qmi_temp);
dsp_line(2, 58, 20, dl);
dcol(0xFFE0);
dsp_line(2, 82, 20, "Floor sensors (mm)");
dcol(0xFFFF);
sprintf(dl, "FL %d FR %d", vl_dist[3], vl_dist[2]);
dsp_line(2, 100, 20, dl);
sprintf(dl, "RL %d RR %d", vl_dist[0], vl_dist[1]);
dsp_line(2, 118, 20, dl);
if (cl_on == 0) {
dcol(0xFFE0);
dsp_line(2, 142, 20, "CLIFF GUARD OFF");
} else if (why >= 2 && why <= 5) {
dcol(0xF800);
dsp_state(dl);
dsp_line(2, 142, 20, dl);
} else {
dcol(0x07E0);
dsp_line(2, 142, 20, "floor ok");
}
}
#endif
}
void display_first() {
dspClear();
display_update();
}
void park_set(int on) {
int i = 0;
if (on == park) { return; }
if (on == 1) {
rover_off();
park = 1;
rover_apply();
dspOnOff(0);
addLog("ROVER: parked (motors refused, sensors and display idle)");
} else {
park = 0;
while (i < 4) { vl_age[i] = 0; i = i + 1; } // every floor sensor has to answer again before the rover may move
cl_fault = 1;
dspOnOff(1);
display_first();
rover_apply();
addLog("ROVER: awake, floor sensors are tested again");
}
}
// ── RANDOM WANDER (RoverWander <seconds>) ───────────────────────────────────────────────────────────────────────────────────────
// The rover drives straight at WAN_D, and when the lidar brake or the staircase guard stops it, it backs off a cliff or turns on the spot in a random direction
// for a random time, until the way ahead is free again. It stops by itself after the given time, when a safety guard is off or faulted, when ANY echo is
// closer than WAN_SIDE_MM (turning would hit it), when a turn finds no free way for 8 s, or when somebody sends a drive command (joystick). It only sees what the
// lidar plane and the four floor sensors see: nothing low on the floor, nothing above the lidar, no glass. Stay close and keep RoverWander short.
#define WAN_D 40 // drive share (with vmax 40 about 0.17 m/s)
#define WAN_T 50 // turn share
#define WAN_FREE_MM 400 // the way ahead counts as free from this distance
#define WAN_SIDE_MM 110 // an echo this close anywhere stops the run
int wan_end = 0;
int wan_st = 0; // 0 straight, 1 backing off a cliff, 2 turning
int wan_t0 = 0;
int wan_dir = 1;
int wan_min = 800; // ms the turn lasts at least
int wan_turns = 0;
int wan_flt = 0; // millis() since when a sensor fault has been seen (0 = none)
void wander_stop(int code) { // 0 command, 1 time over, 2 guard off or faulted, 3 echo too close, 4 turn without a free way, 5 drive command
if (wan_on == 0) { return; }
wan_on = 0;
drive = 0;
turn = 0;
rover_apply();
addLog("ROVER: wander off (reason %d, %d turns)", code, wan_turns);
}
void wander_turn() {
wan_st = 2;
wan_t0 = millis();
if (random(0, 100) < 50) { wan_dir = 1; } else { wan_dir = -1; }
wan_min = 500 + random(0, 1000);
wan_turns = wan_turns + 1;
}
int wander_start(int sec) {
#if ROVER_LIDAR
if (lid_ok() == 0 || park == 1 || low == 1 || obs_on == 0 || cl_on == 0 || obs_fault == 1 || cl_fault == 1) { return 0; }
vmax = VMAX_START;
wan_st = 0;
wan_turns = 0;
wan_flt = 0;
wan_end = millis() + sec * 1000;
wan_on = 1;
addLog("ROVER: wander on for %d s", sec);
return 1;
#else
return 0;
#endif
}
void wander_step() {
#if ROVER_LIDAR
int nf = 0;
if (wan_on == 0) { return; }
if (park == 1 || low == 1 || obs_on == 0 || cl_on == 0) { wander_stop(2); return; }
if (obs_fault == 1 || cl_fault == 1) { // the guards hold the motors by themselves; only a fault that stays ends the run
if (wan_flt == 0) { wan_flt = millis(); }
if (millis() - wan_flt > 1500) { wander_stop(2); return; }
} else {
wan_flt = 0;
}
if (millis() > wan_end) { wander_stop(1); return; }
lastcmd = millis(); // wandering renews the dead-man itself
nf = lid_min(0, 359);
if (nf > 0 && nf < WAN_SIDE_MM) { wander_stop(3); return; }
if (wan_st == 0) {
drive = WAN_D;
turn = 0;
if (cl_front == 1) { wan_st = 1; wan_t0 = millis(); }
else if (obs_front == 1) { wander_turn(); }
} else if (wan_st == 1) {
drive = 0 - WAN_D;
turn = 0;
if (millis() - wan_t0 > 900 || cl_rear == 1 || obs_rear == 1) { wander_turn(); }
} else {
drive = 0;
turn = wan_dir * WAN_T;
nf = lid_min(OBST_FRONT_A0, OBST_FRONT_A1);
if (cl_front == 1) {
wan_st = 1;
wan_t0 = millis();
} else if (millis() - wan_t0 > wan_min && (nf == 0 || nf > WAN_FREE_MM)) {
wan_st = 0;
turn = 0;
} else if (millis() - wan_t0 > 8000) {
wander_stop(4);
return;
}
}
rover_apply();
#endif
}
#if ROVER_LINK
// UDP LINK. Commands (text datagram to LINK_PORT, any subset of the fields): "d=50 t=0 m=60 p=0" (drive -100..100, turn -100..100,
// max speed 10..100, park 0/1). Same rules as /rover_cmd: every command re-arms the 300 ms watchdog, the battery / cliff / stall guards
// still win. The rover learns the sender's IP from the datagrams and sends its status there five times a second:
// {"v":12.49,"l":0,"r":0,"wd":0,"lo":0,"cl":15,"why":2,"yaw":-0.3,"el":0,"er":0,"st":0,"age":120} (age = ms since the last command)
// NO authentication, like /rover_cmd: anyone in the WLAN can drive the rover. Only for a trusted network.
char lk_rx[64];
char lk_tx[200];
char lk_ip[20];
char lk_tmp[12];
int lk_open = 0;
int lk_try = 0;
int lk_next = 0;
int lk_cmds = 0;
int lk_int(char key[], int def) {
int pos = strFind(lk_rx, key);
if (pos < 0) { return def; }
strSub(lk_tmp, lk_rx, pos + strlen(key), 8);
return strToInt(lk_tmp);
}
void lk_status() {
sprintf(lk_tx, "{\"v\":%d.%02d,\"l\":%d,\"r\":%d,\"wd\":%d,\"lo\":%d,\"cl\":%d,\"ob\":%d,\"sp\":%d,\"why\":%d,\"yaw\":%.1f,\"el\":%d,\"er\":%d,\"st\":%d,\"ox\":%d,\"oy\":%d,\"age\":%d}",
vbat_mv / 1000, (vbat_mv % 1000) / 10, motL, motR, stops, low, cliff_mask(), obs_mask(), spd_full, why, qmi_yaw, enc_ticks[0], enc_ticks[1], enc_stall, (int)enc_x, (int)enc_y, millis() - lastcmd);
udp(6, lk_ip, LINK_PORT, lk_tx);
}
// The lidar picture for the controlling program, 5 times a second, as 3 datagrams of 120 degrees each: "S<seq>,<first degree>," followed by 2 characters
// per degree = the distance in 16 mm steps (12 bit: chars 48 + (v >> 6) and 48 + (v & 63)). Instead of /lidar_data: that handler keeps the VM for about
// 300 ms per call and starved the floor sensors (the cliff guard saw them 'stale' and refused to drive, found 18.11.2026).
char lk_sc[260];
int lk_sc_next = 0;
int lk_sc_seq = 0;
void lk_scan() {
int i = 0;
int k = 0;
int p = 0;
int v = 0;
if (lk_open == 0 || strlen(lk_ip) == 0) { return; }
if (millis() - lastcmd > 2000) { return; } // nobody is steering
if (millis() < lk_sc_next) { return; }
lk_sc_next = millis() + 250;
lk_sc_seq = lk_sc_seq + 1;
while (p < 3) {
sprintf(lk_sc, "S%d,%d,", lk_sc_seq, p * 120);
k = strlen(lk_sc);
i = 0;
while (i < 120) {
v = lid_dist[p * 120 + i] / 16;
if (v > 4095) { v = 4095; }
lk_sc[k] = 48 + (v >> 6);
lk_sc[k + 1] = 48 + (v & 63);
k = k + 2;
i = i + 1;
}
lk_sc[k] = 0;
udp(6, lk_ip, LINK_PORT, lk_sc);
p = p + 1;
}
}
void lk_poll() {
int n = 0;
int got = 0;
int sp_new = 0;
if (lk_open == 0) {
lk_try = lk_try + 1;
if (lk_try >= 20) {
lk_try = 0;
if (udp(0, LINK_PORT) == 1) { lk_open = 1; addLog("ROVER: UDP link on port %d", LINK_PORT); }
}
return;
}
n = udp(1, lk_rx);
if (n > 0) {
udp(4, lk_ip);
got = 1;
while (got < 8) { // drain the socket: only the newest command counts (one packet per pass made the answers lag behind)
n = udp(1, lk_rx);
if (n <= 0) { break; }
udp(4, lk_ip);
got = got + 1;
}
lk_cmds = lk_cmds + got;
if (wan_on == 1) { // a wander runs: a command of zero leaves it alone, a real drive command ends it
if (lk_int("d=", 0) != 0 || lk_int("t=", 0) != 0) { wander_stop(5); } else { lk_status(); return; }
}
drive = rover_clamp(lk_int("d=", 0), -100, 100);
turn = rover_clamp(lk_int("t=", 0), -100, 100);
vmax = rover_clamp(lk_int("m=", vmax), 10, 100);
sp_new = lk_int("s=", -1); // s=<mm/s>: speed control like RoverSpeed, sent with EVERY command, so a rover that restarted is back in it at once
if (sp_new >= 0 && sp_new != spd_full) {
spd_full = rover_clamp(sp_new, 0, 600);
spd_out[0] = 0.0;
spd_out[1] = 0.0;
spd_mode[0] = 0;
spd_mode[1] = 0;
}
if (lk_int("p=", -1) >= 0) { park_set(lk_int("p=", 0)); }
if (park == 1 && (drive != 0 || turn != 0)) { park_set(0); }
lastcmd = millis();
if (drive == 0 && turn == 0) { enc_stall = 0; }
rover_apply();
lk_status();
}
if (strlen(lk_ip) > 0 && millis() >= lk_next) {
lk_next = millis() + 200;
lk_status();
}
}
#endif
#if ROVER_ESPNOW
// ESP-NOW LINK. Same commands as the UDP link, as text in ONE packet (up to 250 bytes, here at most 63): "d=40 t=0 m=40 p=0". The rover answers every packet to its
// sender with "v=<mV> l=<motor> r=<motor> w=<why> c=<cliff mask> a=<ms since the last command>". The radio's own ACK tells the sender whether the rover got the packet.
// Same safety as /rover_cmd: every command re-arms the 300 ms watchdog, the battery / cliff / obstacle / stall guards still win, a drive command ends a wander.
// Without ESPNOW_ENC anyone in radio range can send a packet: use it only for tests; with ESPNOW_ENC 1 (CCMP) only the listed peer is accepted.
char en_mac[20];
char en_rx[64];
char en_tx[80];
char en_tmp[12];
int en_ok = 0;
int en_cmds = 0;
int en_t0 = 0;
int en_rssi = 0;
int en_int(char key[], int def) {
int pos = strFind(en_rx, key);
if (pos < 0) { return def; }
strSub(en_tmp, en_rx, pos + strlen(key), 8);
return strToInt(en_tmp);
}
void en_poll() {
int n = 0;
if (en_ok == 0) { return; }
while (espnowAvailable() > 0) {
n = espnowRecv(en_mac, en_rx, 63);
if (n <= 0) { break; }
en_rx[n] = 0;
en_cmds = en_cmds + 1;
en_t0 = millis();
en_rssi = espnowRssi();
if (wan_on == 1) {
if (en_int("d=", 0) != 0 || en_int("t=", 0) != 0) { wander_stop(5); } else { continue; }
}
drive = rover_clamp(en_int("d=", 0), -100, 100);
turn = rover_clamp(en_int("t=", 0), -100, 100);
vmax = rover_clamp(en_int("m=", vmax), 10, 100);
if (en_int("p=", -1) >= 0) { park_set(en_int("p=", 0)); }
if (park == 1 && (drive != 0 || turn != 0)) { park_set(0); }
lastcmd = millis();
if (drive == 0 && turn == 0) { enc_stall = 0; }
rover_apply();
sprintf(en_tx, "v=%d l=%d r=%d w=%d c=%d a=%d", vbat_mv, motL, motR, why, cliff_mask(), millis() - lastcmd);
espnowSend(en_mac, en_tx, strlen(en_tx)); // waits for the radio's ACK, at most 100 ms (the sender just spoke, so it is there)
}
}
#endif
int e50_cnt = 0;
int e50_rate = 0;
int e50_t = 0;
// Dead-man, wheel encoders / stall guard, speed control and the wander run here, NOT only in Every50ms(): a callback gets the VM only while TaskLoop sleeps in
// delay(), and with the lidar streaming TaskLoop is busy 97 % of the time, so Every50ms ran 2..3 times per second instead of 20 (measured 18.11.2026: the dead-man
// was checked every 300..500 ms, the speed controller's 200 ms windows were seconds long and the stall guard tripped on a wheel that had just started).
// TaskLoop calls this between its steps (service_points); the throttle keeps it at about 20 Hz whoever gets there first.
int ft_t = 0;
int ft_cnt = 0;
int ft_rate = 0;
int ft_t1 = 0;
void fast_tasks() {
if (millis() - ft_t < 40) { return; }
ft_t = millis();
ft_cnt = ft_cnt + 1;
if (millis() - ft_t1 >= 1000) { ft_rate = ft_cnt; ft_cnt = 0; ft_t1 = millis(); }
wander_step();
if (millis() - lastcmd > WD_MS) {
if (motL != 0 || motR != 0) {
if (drive != 0 || turn != 0) { stops = stops + 1; }
}
drive = 0;
turn = 0;
rover_apply();
}
enc_update(motL * DIR_L, motR * DIR_R);
if (spd_ctl() == 1) { rover_apply(); }
if (enc_on == 1 && enc_stall != 0 && why != 7) { rover_apply(); }
}
void Every50ms() {
e50_cnt = e50_cnt + 1;
if (millis() - e50_t >= 1000) { e50_rate = e50_cnt; e50_cnt = 0; e50_t = millis(); }
fast_tasks();
if (PARK_AUTO_S > 0 && park == 0 && millis() - lastcmd > PARK_AUTO_S * 1000) { park_set(1); }
dsp_n = dsp_n + 1;
if (dsp_n >= 10) {
dsp_n = 0;
if (park == 0) { display_update(); }
}
#if ROVER_LINK
if (park == 1) { lk_poll(); }
#endif
#if ROVER_ESPNOW
if (park == 1) { en_poll(); } // parked: TaskLoop does nothing (delay 250), the callback has the VM; otherwise TaskLoop polls
#endif
}
// ---- web ---------------------------------------------------------------------------------------------------------------
int cliff_mask() {
return cl_flag[3] + cl_flag[2] * 2 + cl_flag[0] * 4 + cl_flag[1] * 8; // bits: FL FR RL RR
}
void WebCall() {
dsp_state(dl);
sprintf(ob, "{s}Rover{m}%s motors L / R %d / %d (watchdog stops %d){e}", dl, motL, motR, stops);
webSend(ob);
if (vbat_mv > 0) {
sprintf(ob, "{s}Battery{m}%d.%02d V %d %% %d mA{e}", vbat_mv / 1000, (vbat_mv % 1000) / 10, bat_pct(vbat_mv), ina_ma);
webSend(ob);
} else {
webSend("{s}Battery (INA219){m}no data{e}");
}
sprintf(ob, "{s}Floor sensors FL FR RL RR{m}%d %d %d %d mm window %d..%d{e}", vl_dist[3], vl_dist[2], vl_dist[0], vl_dist[1], cl_min, cl_max);
webSend(ob);
if (qmi_ok == 1) {
sprintf(ob, "{s}IMU yaw / gyro z{m}%.1f deg %.2f dps %.1f C{e}", qmi_yaw, qmi_gz, qmi_temp);
webSend(ob);
}
if (ak_addr != 0) {
sprintf(ob, "{s}Compass{m}heading %.0f{e}", ak_heading());
webSend(ob);
}
if (enc_on == 1 || enc_seen[0] == 1 || enc_seen[1] == 1 || ENC_SIM == 1) {
sprintf(ob, "{s}Wheel encoders L / R{m}%d / %d ticks %d / %d mm %d / %d mm/s{e}", enc_ticks[0], enc_ticks[1], (int)enc_mm[0], (int)enc_mm[1], (int)enc_v[0], (int)enc_v[1]);
webSend(ob);
sprintf(ob, "{s}Odometry{m}x %d y %d mm heading %.1f deg stall %d guard %d{e}", (int)enc_x, (int)enc_y, enc_th * 57.29578, enc_stall, enc_on);
webSend(ob);
} else {
webSend("{s}Wheel encoders{m}no ticks yet (RoverEnc 1 switches the stall guard on){e}");
}
#if ROVER_LIDAR
if (lid_ok() == 1) {
sprintf(ob, "{s}Lidar front / back{m}%d / %d mm brake %d (stop below %d mm) left %d / right %d mm (direction assumed){e}", lid_min(OBST_FRONT_A0, OBST_FRONT_A1), lid_min(OBST_REAR_A0, OBST_REAR_A1), obs_on, obs_mm, lid_min(150, 210), lid_min(330, 30));
webSend(ob);
webSend("{s}Lidar display{m}<a href='/lidar'>/lidar</a>{e}");
sprintf(ob, "{s}Lidar scans / speed{m}%d %d ms per turn packets ok / crc %d / %d{e}", lid_scans, lid_scan_ms, lid_n_ok, lid_n_crc);
webSend(ob);
} else {
webSend("{s}Lidar{m}no data (LIDAR_RX wired? 5 V on?){e}");
}
#endif
#if ROVER_ESPNOW
sprintf(ob, "{s}ESP-NOW commands / last RSSI / age{m}%d / %d dBm / %d ms{e}", en_cmds, en_rssi, millis() - en_t0);
webSend(ob);
#endif
sprintf(ob, "{s}Loop rates (per second){m}TaskLoop %d, Every50ms %d, fast tasks (dead-man, encoders, speed control) %d; a floor sensor is stale after %d ms{e}", tl_rate, e50_rate, ft_rate, CLIFF_STALE_MS);
webSend(ob);
webSend("{s}Rover page{m}<a href='/rover'>/rover</a>{e}");
}
void JsonCall() {
sprintf(ob, ",\"Rover\":{\"Vbat\":%d.%03d,\"Cliff\":%d,\"Why\":%d,\"Park\":%d", vbat_mv / 1000, vbat_mv % 1000, cliff_mask(), why, park);
responseAppend(ob);
if (qmi_ok == 1) {
sprintf(ob, ",\"Yaw\":%.1f", qmi_yaw);
responseAppend(ob);
}
sprintf(ob, ",\"EncL\":%d,\"EncR\":%d,\"Stall\":%d", enc_ticks[0], enc_ticks[1], enc_stall);
responseAppend(ob);
responseAppend("}");
}
// the joystick page lives in the file /rover.html on the device (upload examples/rover.html): about 2 KB less bytecode in RAM
void rover_page() {
webSendFile("/rover.html");
}
#if ROVER_LIDAR
// /lidar_data: the raw scan in lidar degrees (0 = lidar's own forward, "ahead" of the rover is 270): the page /lidar (file /lidar.html) turns it
void lidar_data() {
int i = 0;
sprintf(ob, "{\"ok\":%d,\"sc\":%d,\"ms\":%d,\"f0\":%d,\"f1\":%d,\"b0\":%d,\"b1\":%d,\"on\":%d,\"mm\":%d,\"nf\":%d,\"nb\":%d,\"d\":[", lid_ok(), lid_scans, lid_scan_ms, OBST_FRONT_A0, OBST_FRONT_A1, OBST_REAR_A0, OBST_REAR_A1, obs_on, obs_mm, lid_min(OBST_FRONT_A0, OBST_FRONT_A1), lid_min(OBST_REAR_A0, OBST_REAR_A1));
webSend(ob);
while (i < 360) {
sprintf(ob, "%d,%d,%d,%d,%d%s", lid_dist[i], lid_dist[i + 1], lid_dist[i + 2], lid_dist[i + 3], lid_dist[i + 4], i < 355 ? "," : "]}");
webSend(ob);
i = i + 5;
}
}
#endif
void WebOn() {
int h = webHandler();
int wd_a = 0;
int wd_b = 0;
if (h == 1) {
rover_page();
}
#if ROVER_LIDAR
if (h == 3) {
webSendFile("/lidar.html");
}
if (h == 4) {
lidar_data();
}
#endif
if (h == 2) {
if (wan_on == 1) { // a status poll (d=0, t=0) leaves the wander alone, a real drive command ends it
wd_a = 0;
wd_b = 0;
if (webArg("d", cb) > 0) { wd_a = strToInt(cb); }
if (webArg("t", cb) > 0) { wd_b = strToInt(cb); }
if (wd_a != 0 || wd_b != 0) { wander_stop(5); }
}
if (wan_on == 0) {
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); }
if (webArg("p", cb) > 0) { park_set(strToInt(cb)); }
if (park == 1 && (drive != 0 || turn != 0)) { park_set(0); }
lastcmd = millis();
if (drive == 0 && turn == 0) { enc_stall = 0; } // the stick is released: a stall is cleared
rover_apply();
}
sprintf(ob, "{\"v\":%d.%02d,\"l\":%d,\"r\":%d,\"wd\":%d,\"lo\":%d,\"cl\":%d,\"el\":%d,\"er\":%d,\"st\":%d,\"sl\":%d,\"sr\":%d,\"ob\":%d}", vbat_mv / 1000, (vbat_mv % 1000) / 10, motL, motR, stops, low, cliff_mask(), enc_ticks[0], enc_ticks[1], enc_stall, (int)spd_v[0], (int)spd_v[1], obs_mask());
webSend(ob);
}
}
void Command(char cmd[]) {
char a[12];
char b[12];
int ch = 0;
if (strcmp(cmd, "CALGYRO") == 0) {
qmi_cal_gyro(200);
responseCmnd("gyro bias set");
} else if (strcmp(cmd, "YAW0") == 0) {
qmi_yaw_reset();
responseCmnd("yaw 0");
} else if (strStartsWith(cmd, "CALMAG")) {
if (strFind(cmd, "1") > 0) {
ak_cal_begin();
responseCmnd("turn the rover slowly through a full circle, then RoverCalMag 0");
} else {
ak_cal_end();
responseCmnd("compass offsets set");
}
} else if (strStartsWith(cmd, "SPEED")) {
strToken(a, cmd, ' ', 2);
if (strlen(a) > 0) {
spd_full = rover_clamp(strToInt(a), 0, 600);
spd_out[0] = 0.0;
spd_out[1] = 0.0;
spd_mode[0] = 0;
spd_mode[1] = 0;
rover_apply();
}
sprintf(ob, "full stick %d mm/s (0 = open loop), measured %d / %d mm/s, pwm %d / %d, pulse %d / %d duty %d / %d %%", spd_full, (int)spd_v[0], (int)spd_v[1], (int)spd_out[0], (int)spd_out[1], spd_mode[0], spd_mode[1], (int)(spd_duty[0] * 100.0), (int)(spd_duty[1] * 100.0));
responseCmnd(ob);
} else if (strStartsWith(cmd, "ENCSIM")) {
strToken(a, cmd, ' ', 2);
if (strlen(a) > 0) { enc_sim_tps = strToInt(a); }
responseCmnd("ok");
} else if (strStartsWith(cmd, "ENCZERO")) {
enc_zero();
responseCmnd("encoders zeroed");
} else if (strStartsWith(cmd, "ENCCAL")) {
strToken(a, cmd, ' ', 2);
if (enc_cal(strToInt(a)) == 1) {
sprintf(ob, "%.3f mm per tick, %.0f ticks per wheel revolution", enc_mm_tick, (2.0 * 3.14159265 * ENC_WHEEL_R_MM) / enc_mm_tick);
responseCmnd(ob);
} else {
responseCmnd("need 10 ticks and 10 mm (RoverEncZero, roll, RoverEncCal <mm>)");
}
} else if (strStartsWith(cmd, "ENCTPR")) {
strToken(a, cmd, ' ', 2);
if (strlen(a) > 0) { enc_set_tpr(strToInt(a)); }
sprintf(ob, "%.3f mm per tick", enc_mm_tick);
responseCmnd(ob);
} else if (strStartsWith(cmd, "ENC")) {
strToken(a, cmd, ' ', 2);
if (strlen(a) > 0) {
enc_on = strToInt(a);
if (enc_on == 0) { enc_stall = 0; }
rover_apply();
}
sprintf(ob, "guard %d, ticks %d / %d, seen %d / %d, stall %d", enc_on, enc_ticks[0], enc_ticks[1], enc_seen[0], enc_seen[1], enc_stall);
responseCmnd(ob);
} else if (strStartsWith(cmd, "CLIFF")) {
strToken(a, cmd, ' ', 2);
strToken(b, cmd, ' ', 3);
if (strlen(a) == 0) {
sprintf(ob, "guard %d, window %d..%d mm, F%d%d R%d%d fault %d", cl_on, cl_min, cl_max, cl_flag[3], cl_flag[2], cl_flag[0], cl_flag[1], cl_fault);
responseCmnd(ob);
} else if (strlen(b) == 0) {
cl_on = strToInt(a);
if (cl_on == 0) { addLog("ROVER: CLIFF GUARD OFF - the rover can drive off an edge"); }
rover_apply();
responseCmnd("ok");
} else {
cl_min = strToInt(a);
cl_max = strToInt(b);
responseCmnd("window set");
}
} else if (strStartsWith(cmd, "OBST")) {
strToken(a, cmd, ' ', 2);
if (strlen(a) > 0) {
ch = strToInt(a);
if (ch == 0) {
obs_on = 0;
addLog("ROVER: OBSTACLE BRAKE OFF - the rover can drive into things");
} else if (ch == 1) {
obs_on = 1;
} else if (ch >= 50) {
obs_on = 1;
obs_mm = ch;
}
rover_apply();
}
sprintf(ob, "brake %d, stop below %d mm, ahead %d mm, behind %d mm, F%d R%d lidar fault %d", obs_on, obs_mm, obs_df, obs_dr, obs_front, obs_rear, obs_fault);
responseCmnd(ob);
} else if (strStartsWith(cmd, "WANDER")) {
strToken(a, cmd, ' ', 2);
if (strlen(a) > 0) {
ch = strToInt(a);
if (ch <= 0) {
wander_stop(0);
} else if (wander_start(rover_clamp(ch, 1, 600)) == 0) {
responseCmnd("not started: needs the lidar, no fault, no park, the obstacle brake and the staircase guard on");
return;
}
}
sprintf(ob, "wander %d, %d s left, state %d, turns %d", wan_on, wan_on * ((wan_end - millis()) / 1000), wan_st, wan_turns);
responseCmnd(ob);
} else if (strStartsWith(cmd, "PARK")) {
strToken(a, cmd, ' ', 2);
if (strlen(a) > 0) { park_set(strToInt(a)); }
sprintf(ob, "park %d", park);
responseCmnd(ob);
} else if (strStartsWith(cmd, "TOF")) {
strToken(a, cmd, ' ', 2);
strToken(b, cmd, ' ', 3);
ch = strToInt(a);
if (ch >= 0 && ch < 4 && strlen(b) > 0) { sim_tof[ch] = strToInt(b); }
responseCmnd("ok");
} else if (strStartsWith(cmd, "VBAT")) {
strToken(a, cmd, ' ', 2);
if (strlen(a) > 0) { sim_vbat = strToInt(a); }
responseCmnd("ok");
} else {
responseCmnd("unknown");
}
}
void CleanUp() {
if (park == 1) { dspOnOff(1); }
rover_off();
ina_release();
qmi_release();
ak_release();
vl_release();
}
// OnExit = program stopped or replaced; CleanUp only runs at a restart. Motors off and the I2C addresses back, so a reload starts clean.
void OnExit() {
CleanUp();
}
int main() {
int i = 0;
while (i < 4) { sim_tof[i] = 60; cl_flag[i] = 1; i = i + 1; }
addCommand("Rover");
#if ROVER_REAL
{
pinMode(PIN_PWMA, OUTPUT);
pinMode(PIN_AIN1, OUTPUT);
pinMode(PIN_AIN2, OUTPUT);
pinMode(PIN_PWMB, OUTPUT);
pinMode(PIN_BIN1, OUTPUT);
pinMode(PIN_BIN2, OUTPUT);
}
#endif
rover_off();
enc_init();
lastcmd = millis();
// A reload (unload + run) leaves the I2C addresses of the previous run claimed, then every init below fails
// silently (Vbat 0, no yaw): give them back first, through the drivers' own release functions.
ina_addr = INA_ADDR; ina_bus = I2C_BUS; ina_release();
qmi_addr = 0x6B; qmi_bus = I2C_BUS; qmi_release();
ak_addr = AK_ADDR; ak_bus = I2C_BUS; ak_release();
vl_bus = TOF_BUS; vl_claimed = 1; vl_mux_ok = 1; vl_release();
ina_init(INA_ADDR, I2C_BUS);
#if ROVER_LIDAR
lid_init(LIDAR_RX);
#endif
qmi_init(I2C_BUS, 2, 5, 3);
ak_init(I2C_BUS, 8);
if (qmi_ok == 1) { qmi_cal_gyro(200); }
#if ROVER_REAL
{
if (vl_mux_init(TOF_BUS) == 1) {
vl_timeout = 60;
i = 0;
while (i < 4) {
if (vl_start(i) == 0) { addLog("ROVER: floor sensor %d not found", i); }
i = i + 1;
}
} else {
addLog("ROVER: TCA9548A not found, the cliff guard keeps the motors off (RoverCliff 0 to override)");
}
}
#endif
tasmInfo(2, ip);
webOn(1, "/rover");
webOn(2, "/rover_cmd");
#if ROVER_LIDAR
webOn(3, "/lidar");
webOn(4, "/lidar_data");
#endif
#if ROVER_ESPNOW
#if ESPNOW_ENC
espnowKey(ESPNOW_KEY); // the key counts for the peers added AFTER this call, on both sides
espnowPeer(ESPNOW_PEER);
#else
addLog("ROVER: ESP-NOW link is OPEN (no key): anyone in radio range can drive the rover");
#endif
en_ok = espnowBegin(0); // 0 = keep the channel of the WiFi link
addLog("ROVER: ESP-NOW begin %d, encrypted %d", en_ok, ESPNOW_ENC);
#endif
display_first();
addLog("ROVER: ready (sim %d), page /rover, watchdog %d ms, cliff window %d..%d mm", ROVER_SIM, WD_MS, cl_min, cl_max);
return 0;
}