2026-09-28 18:14:00 +02:00
|
|
|
#include "Config.h"
|
|
|
|
|
#include "Sticks.h"
|
|
|
|
|
#include "RemoteIMU.h"
|
|
|
|
|
#include "RemoteState.h"
|
|
|
|
|
#include "PacketBuilder.h"
|
|
|
|
|
#include "RadioLink.h"
|
|
|
|
|
#include "NextionUI.h"
|
|
|
|
|
#include "SerialConsole.h"
|
2026-11-17 14:42:37 +01:00
|
|
|
#include "Battery.h"
|
2026-09-28 18:14:00 +02:00
|
|
|
|
2026-11-14 12:19:16 +01:00
|
|
|
// Hexapod remote (Teensy 4.0): reads two 3-axis sticks and the handset
|
2026-09-28 18:14:00 +02:00
|
|
|
// MPU6050, takes operator commands from the Nextion touchscreen (and the USB
|
|
|
|
|
// serial console), and transmits a CmdPacket to the robot at TX_HZ.
|
|
|
|
|
// See README.md for wiring, calibration and the serial key map.
|
|
|
|
|
|
|
|
|
|
Sticks sticks;
|
|
|
|
|
RemoteIMU imu;
|
|
|
|
|
RemoteState state;
|
|
|
|
|
PacketBuilder builder;
|
|
|
|
|
RadioLink radio_link;
|
|
|
|
|
NextionUI nextion;
|
|
|
|
|
SerialConsole console;
|
2026-11-17 14:42:37 +01:00
|
|
|
Battery battery;
|
2026-09-28 18:14:00 +02:00
|
|
|
|
|
|
|
|
uint32_t last_tx_us = 0;
|
|
|
|
|
bool dump_packets = false;
|
|
|
|
|
uint32_t last_dump_ms = 0;
|
|
|
|
|
CmdPacket last_packet;
|
|
|
|
|
|
|
|
|
|
static void printStatus() {
|
|
|
|
|
static const char* const names[NUM_AXES] = { "L_X", "L_Y", "L_TW", "R_X", "R_Y", "R_TW" };
|
|
|
|
|
Serial.println("---- remote status ----");
|
|
|
|
|
for (int i = 0; i < NUM_AXES; i++)
|
|
|
|
|
Serial.printf("%-5s raw %4d -> %5d\n", names[i], sticks.raw(i), sticks.frame().v[i]);
|
|
|
|
|
Serial.printf("IMU %s roll %6.1f pitch %6.1f (deg, vs zeroed level)\n",
|
|
|
|
|
imu.ok() ? "ok" : "MISSING", imu.rollDeg(), imu.pitchDeg());
|
|
|
|
|
Serial.printf("radio %s link %s %u%%\n", radio_link.present() ? "ok" : "MISSING",
|
|
|
|
|
radio_link.linkOk() ? "OK" : "DOWN", radio_link.qualityPct());
|
2026-09-29 00:37:12 +02:00
|
|
|
Serial.printf("armed %d stand %d gait %s mode %s rstick %s remote_gyro %d robot_gyro %d speed %u->%u height %d\n",
|
2026-09-28 18:14:00 +02:00
|
|
|
state.armed(), state.standRequested(), state.gaitWave() ? "wave" : "tripod",
|
|
|
|
|
state.modeB() ? "B" : "A", state.rstickXlate() ? "xlate" : "rot",
|
2026-09-29 00:37:12 +02:00
|
|
|
state.remoteGyro(), state.robotGyro(), state.speedTarget(), state.speedCurrent(), state.height());
|
2026-09-28 18:14:00 +02:00
|
|
|
sticks.printCalibration(Serial);
|
2026-11-17 14:42:37 +01:00
|
|
|
if (BATTERY_SENSE_PIN != 255) Serial.printf("battery %u mV\n", battery.millivolts());
|
|
|
|
|
|
|
|
|
|
const Telemetry& t = radio_link.telemetry();
|
|
|
|
|
if (radio_link.telemetryFresh(millis())) {
|
|
|
|
|
Serial.printf("robot: state %u status %02X roll %.1f pitch %.1f battery %u mV, hears %u/s\n",
|
|
|
|
|
t.state, t.status, t.roll_ddeg / 10.0f, t.pitch_ddeg / 10.0f, t.battery_mv, t.rx_per_sec);
|
|
|
|
|
Serial.print("robot joints (deg):");
|
|
|
|
|
for (int i = 0; i < 18; i++)
|
|
|
|
|
Serial.printf(" %d", t.joint_deg[i] - (i % 3 == 2 ? TELEMETRY_TIBIA_OFFSET_DEG : 0));
|
|
|
|
|
Serial.println();
|
|
|
|
|
} else {
|
|
|
|
|
Serial.println("robot: no telemetry");
|
|
|
|
|
}
|
|
|
|
|
Serial.printf("servo cal %s, servo %u (leg %u joint %u) trim %d us%s, robot reports servo %u trim %d\n",
|
|
|
|
|
state.servoCal() ? "ON" : "off", state.servoSel(), state.servoSel() / 3, state.servoSel() % 3,
|
|
|
|
|
state.trimTarget(), state.trimSynced() ? "" : " (not read back yet)", t.cal_servo, t.cal_trim_us);
|
2026-09-28 18:14:00 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
static void dumpPacket(const CmdPacket& p) {
|
2026-09-29 00:37:12 +02:00
|
|
|
Serial.printf("#%3u vx %5d vy %5d yaw %5d | r %5d %5d %5d | tilt %5d %5d | h %5d spd %3u flags %02X\n",
|
2026-09-28 18:14:00 +02:00
|
|
|
p.seq, p.vx, p.vy, p.yaw_rate, p.rx, p.ry, p.rz,
|
2026-09-29 00:37:12 +02:00
|
|
|
p.remote_roll, p.remote_pitch, p.body_height, p.speed_scale, p.flags);
|
2026-09-28 18:14:00 +02:00
|
|
|
}
|
|
|
|
|
|
2026-11-17 14:42:37 +01:00
|
|
|
// Nextion Editor upload through this USB port (SETUP > UPDATE DISPLAY, or 'U').
|
2026-10-01 15:29:21 +02:00
|
|
|
// Never returns: the TX loop stops, so the robot loses the link and
|
|
|
|
|
// disarms itself after LINK_TIMEOUT_SEC. Power-cycle the remote to leave.
|
|
|
|
|
static void enterNextionUpload() {
|
|
|
|
|
state.handle(UiEvent{ UiCmd::Disarm, 0 }, sticks, imu, millis());
|
|
|
|
|
Serial.println("Nextion upload mode: connect Nextion Editor to this USB port and upload.");
|
|
|
|
|
Serial.println("Power-cycle the remote to leave.");
|
|
|
|
|
Serial.flush();
|
|
|
|
|
nextion.runUploadPassthrough();
|
|
|
|
|
}
|
|
|
|
|
|
2026-09-28 18:14:00 +02:00
|
|
|
static void pollInputs(uint32_t now_ms) {
|
|
|
|
|
UiEvent ev;
|
2026-10-01 15:29:21 +02:00
|
|
|
while (nextion.poll(ev)) {
|
|
|
|
|
state.handle(ev, sticks, imu, now_ms);
|
2026-11-17 14:42:37 +01:00
|
|
|
if (state.takeUploadRequest()) enterNextionUpload(); // UPDATE DISPLAY, tapped twice
|
2026-10-01 15:29:21 +02:00
|
|
|
}
|
2026-09-28 18:14:00 +02:00
|
|
|
|
|
|
|
|
SerialConsole::Local local;
|
|
|
|
|
while (console.poll(ev, local)) {
|
2026-10-01 15:29:21 +02:00
|
|
|
if (ev.cmd == UiCmd::NextionUpload) enterNextionUpload();
|
2026-09-28 18:14:00 +02:00
|
|
|
if (ev.cmd != UiCmd::None) {
|
|
|
|
|
bool ok = state.handle(ev, sticks, imu, now_ms);
|
|
|
|
|
if (!ok && state.message(now_ms)[0]) Serial.println(state.message(now_ms));
|
|
|
|
|
}
|
|
|
|
|
switch (local) {
|
|
|
|
|
case SerialConsole::Local::PrintStatus: printStatus(); break;
|
|
|
|
|
case SerialConsole::Local::ToggleDump: dump_packets = !dump_packets; break;
|
|
|
|
|
case SerialConsole::Local::ResetCal:
|
|
|
|
|
if (state.armed()) { Serial.println("disarm first"); break; }
|
|
|
|
|
sticks.resetCalibrationToDefaults();
|
|
|
|
|
Serial.println("stick calibration reset to Config.h defaults");
|
|
|
|
|
break;
|
|
|
|
|
case SerialConsole::Local::None: break;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void setup() {
|
|
|
|
|
Serial.begin(115200);
|
|
|
|
|
nextion.begin();
|
|
|
|
|
|
|
|
|
|
// Keep the remote still and both sticks released during power-on: the
|
|
|
|
|
// IMU captures its gyro bias and the sticks capture their centers here.
|
|
|
|
|
if (!sticks.begin()) Serial.println("stick center capture skipped on some axes - was a stick held at power-on?");
|
|
|
|
|
if (!imu.begin()) Serial.println("remote IMU not responding - remote-gyro tilt unavailable");
|
|
|
|
|
if (!radio_link.begin()) Serial.println("radio not responding - check nRF24 wiring/power");
|
|
|
|
|
|
|
|
|
|
state.begin();
|
|
|
|
|
last_tx_us = micros();
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void loop() {
|
|
|
|
|
uint32_t now_ms = millis();
|
|
|
|
|
|
|
|
|
|
// UI input is drained every pass, not just on TX ticks, so no touch
|
|
|
|
|
// events are lost between packets.
|
|
|
|
|
pollInputs(now_ms);
|
|
|
|
|
|
|
|
|
|
uint32_t now_us = micros();
|
|
|
|
|
float dt = (now_us - last_tx_us) / 1e6f;
|
|
|
|
|
if (dt < (1.0f / TX_HZ)) return; // hold to the fixed TX rate
|
|
|
|
|
last_tx_us = now_us;
|
|
|
|
|
|
|
|
|
|
sticks.update();
|
|
|
|
|
imu.update(dt);
|
|
|
|
|
radio_link.update(now_ms);
|
2026-11-17 14:42:37 +01:00
|
|
|
battery.update(dt);
|
|
|
|
|
bool tele_fresh = radio_link.telemetryFresh(now_ms);
|
|
|
|
|
state.update(dt, radio_link.linkOk(), tele_fresh, radio_link.telemetry(), now_ms);
|
2026-09-28 18:14:00 +02:00
|
|
|
|
|
|
|
|
last_packet = builder.build(sticks.frame(), imu, state, now_ms);
|
|
|
|
|
radio_link.send(last_packet);
|
|
|
|
|
|
|
|
|
|
UiView view;
|
|
|
|
|
state.fillView(view, now_ms);
|
|
|
|
|
view.link_ok = radio_link.linkOk();
|
|
|
|
|
view.link_pct = radio_link.qualityPct();
|
2026-11-17 14:42:37 +01:00
|
|
|
view.tele_ok = tele_fresh;
|
|
|
|
|
view.tele = radio_link.telemetry();
|
|
|
|
|
view.imu_ok = imu.ok();
|
|
|
|
|
view.imu_roll = imu.rollDeg();
|
|
|
|
|
view.imu_pitch = imu.pitchDeg();
|
|
|
|
|
for (int i = 0; i < NUM_AXES; i++) view.sticks[i] = sticks.frame().v[i];
|
|
|
|
|
view.battery_mv = battery.millivolts();
|
2026-09-28 18:14:00 +02:00
|
|
|
nextion.render(view, now_ms);
|
|
|
|
|
|
|
|
|
|
if (dump_packets && now_ms - last_dump_ms >= 100) {
|
|
|
|
|
last_dump_ms = now_ms;
|
|
|
|
|
dumpPacket(last_packet);
|
|
|
|
|
}
|
|
|
|
|
}
|