2026-09-28 18:14:00 +02:00
|
|
|
#include "RemoteState.h"
|
|
|
|
|
|
|
|
|
|
void RemoteState::begin() {
|
|
|
|
|
speed_.reset(speed_target_);
|
|
|
|
|
tilt_blend_.reset(0);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void RemoteState::setMessage(const char* msg, uint32_t now_ms) {
|
|
|
|
|
message_ = msg;
|
|
|
|
|
message_until_ms_ = now_ms + STATUS_MESSAGE_MS;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Disarming always clears the stand request: the robot arms into SIT, and
|
|
|
|
|
// re-arming must never make it stand up on its own.
|
|
|
|
|
void RemoteState::disarm() {
|
|
|
|
|
armed_ = false;
|
|
|
|
|
stand_ = false;
|
|
|
|
|
}
|
|
|
|
|
|
2026-11-17 14:42:37 +01:00
|
|
|
void RemoteState::stopServoCal() {
|
|
|
|
|
servo_cal_ = false;
|
|
|
|
|
trim_synced_ = false;
|
|
|
|
|
cal_op_ = CAL_NONE;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// A new selection is only trimmed once the robot has reported that servo's
|
|
|
|
|
// current trim (CAL_HOLD until then), so a stale value is never sent.
|
|
|
|
|
void RemoteState::selectServo(uint8_t servo) {
|
|
|
|
|
if (servo == servo_sel_) return;
|
|
|
|
|
servo_sel_ = servo;
|
|
|
|
|
trim_synced_ = false;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
uint8_t RemoteState::servoCalCmd(uint32_t now_ms) const {
|
|
|
|
|
if (!servo_cal_ || armed_) return CAL_NONE;
|
|
|
|
|
if (cal_op_ != CAL_NONE && now_ms < cal_op_until_ms_) return cal_op_;
|
|
|
|
|
return trim_synced_ ? CAL_SET : CAL_HOLD;
|
|
|
|
|
}
|
|
|
|
|
|
2026-09-28 18:14:00 +02:00
|
|
|
bool RemoteState::handle(const UiEvent& e, Sticks& stick_hw, RemoteIMU& imu, uint32_t now) {
|
|
|
|
|
const StickFrame& sticks = stick_hw.frame();
|
|
|
|
|
switch (e.cmd) {
|
|
|
|
|
case UiCmd::None:
|
|
|
|
|
return false;
|
|
|
|
|
|
|
|
|
|
case UiCmd::Disarm:
|
|
|
|
|
disarm();
|
|
|
|
|
return true;
|
|
|
|
|
|
|
|
|
|
case UiCmd::ToggleArm:
|
|
|
|
|
if (armed_) { disarm(); return true; }
|
|
|
|
|
if (calibrating_) { setMessage("Finish calibration first", now); return false; }
|
2026-11-17 14:42:37 +01:00
|
|
|
if (servo_cal_) { setMessage("Stop servo calibration first", now); return false; }
|
2026-09-28 18:14:00 +02:00
|
|
|
if (!sticks.allCentered(ARM_CENTER_TOLERANCE)) { setMessage("Center both sticks to arm", now); return false; }
|
|
|
|
|
armed_ = true;
|
|
|
|
|
return true;
|
|
|
|
|
|
|
|
|
|
case UiCmd::ToggleStand:
|
|
|
|
|
if (!armed_) { setMessage("Arm first", now); return false; }
|
|
|
|
|
stand_ = !stand_;
|
|
|
|
|
return true;
|
|
|
|
|
|
|
|
|
|
case UiCmd::ToggleGait: gait_wave_ = !gait_wave_; return true;
|
|
|
|
|
case UiCmd::ToggleWalkMode: mode_b_ = !mode_b_; return true;
|
|
|
|
|
case UiCmd::ToggleRStickXlate: rstick_xlate_ = !rstick_xlate_; return true;
|
|
|
|
|
case UiCmd::ToggleRobotGyro: robot_gyro_ = !robot_gyro_; return true;
|
|
|
|
|
|
|
|
|
|
case UiCmd::ToggleRemoteGyro:
|
|
|
|
|
if (!remote_gyro_ && !imu.ok()) { setMessage("Remote IMU not detected", now); return false; }
|
|
|
|
|
remote_gyro_ = !remote_gyro_;
|
|
|
|
|
// Re-zero on enable: whatever angle you're holding the remote at when
|
|
|
|
|
// you switch it on becomes "level", so the body doesn't lurch.
|
|
|
|
|
if (remote_gyro_) imu.zeroLevel();
|
|
|
|
|
return true;
|
|
|
|
|
|
|
|
|
|
case UiCmd::SetSpeed: speed_target_ = e.value; return true;
|
|
|
|
|
case UiCmd::SpeedUp: speed_target_ = (uint8_t)min(255, speed_target_ + SPEED_SCALE_STEP); return true;
|
|
|
|
|
case UiCmd::SpeedDown: speed_target_ = (uint8_t)max(0, speed_target_ - SPEED_SCALE_STEP); return true;
|
|
|
|
|
|
2026-09-29 00:37:12 +02:00
|
|
|
case UiCmd::SetHeight: height_ = (int16_t)constrain(((int)e.value - 100) * 10, -1000, 1000); return true;
|
|
|
|
|
case UiCmd::HeightUp: height_ = (int16_t)min(1000, height_ + HEIGHT_STEP); return true;
|
|
|
|
|
case UiCmd::HeightDown: height_ = (int16_t)max(-1000, height_ - HEIGHT_STEP); return true;
|
|
|
|
|
|
2026-09-28 18:14:00 +02:00
|
|
|
case UiCmd::ZeroGyro:
|
|
|
|
|
imu.zeroLevel();
|
|
|
|
|
setMessage("Remote level zeroed", now);
|
|
|
|
|
return true;
|
|
|
|
|
|
|
|
|
|
case UiCmd::CalStartSave:
|
|
|
|
|
if (!calibrating_) {
|
|
|
|
|
if (armed_) { setMessage("Disarm to calibrate", now); return false; }
|
|
|
|
|
stick_hw.startCalibration();
|
|
|
|
|
calibrating_ = true;
|
|
|
|
|
setMessage("Sweep all axes to limits, release, SAVE", now);
|
|
|
|
|
return true;
|
|
|
|
|
}
|
|
|
|
|
if (!stick_hw.saveCalibration()) { setMessage("Sweep too small - keep sweeping", now); return false; }
|
|
|
|
|
calibrating_ = false;
|
|
|
|
|
setMessage("Calibration saved", now);
|
|
|
|
|
return true;
|
|
|
|
|
|
2026-10-01 15:29:21 +02:00
|
|
|
case UiCmd::NextionUpload:
|
|
|
|
|
return false; // not operator state - remote.ino handles it
|
|
|
|
|
|
2026-11-17 14:42:37 +01:00
|
|
|
case UiCmd::UploadRequest:
|
|
|
|
|
if (armed_) { setMessage("Disarm first", now); return false; }
|
|
|
|
|
if (now < upload_confirm_until_ms_) { upload_go_ = true; return true; }
|
|
|
|
|
upload_confirm_until_ms_ = now + UPLOAD_CONFIRM_MS;
|
|
|
|
|
setMessage("Tap again to enter display update mode", now);
|
|
|
|
|
return true;
|
|
|
|
|
|
|
|
|
|
case UiCmd::PageShown:
|
|
|
|
|
page_ = e.value;
|
|
|
|
|
// Servo calibration only runs while its page is showing.
|
|
|
|
|
if (servo_cal_ && page_ != PAGE_CAL) stopServoCal();
|
|
|
|
|
upload_confirm_until_ms_ = 0;
|
|
|
|
|
return true;
|
|
|
|
|
|
|
|
|
|
case UiCmd::ServoCalToggle:
|
|
|
|
|
if (servo_cal_) { stopServoCal(); setMessage("Servos relaxed", now); return true; }
|
|
|
|
|
if (armed_) { setMessage("Disarm to calibrate servos", now); return false; }
|
|
|
|
|
servo_cal_ = true;
|
|
|
|
|
trim_synced_ = false;
|
|
|
|
|
setMessage("Robot holds every servo at centre", now);
|
|
|
|
|
return true;
|
|
|
|
|
|
|
|
|
|
case UiCmd::ServoCalLeg:
|
|
|
|
|
if (e.value >= 6) return false;
|
|
|
|
|
selectServo(e.value * 3 + servo_sel_ % 3);
|
|
|
|
|
return true;
|
|
|
|
|
|
|
|
|
|
case UiCmd::ServoCalJoint:
|
|
|
|
|
if (e.value >= 3) return false;
|
|
|
|
|
selectServo(servo_sel_ / 3 * 3 + e.value);
|
|
|
|
|
return true;
|
|
|
|
|
|
|
|
|
|
case UiCmd::ServoCalNext:
|
|
|
|
|
selectServo((servo_sel_ + 1) % 18);
|
|
|
|
|
return true;
|
|
|
|
|
|
|
|
|
|
case UiCmd::ServoCalTrim: {
|
|
|
|
|
if (!servo_cal_) { setMessage("Tap START first", now); return false; }
|
|
|
|
|
if (!trim_synced_) { setMessage("Waiting for the robot", now); return false; }
|
|
|
|
|
int t = trim_target_;
|
|
|
|
|
switch (e.value) {
|
|
|
|
|
case SERVO_TRIM_MINUS_BIG: t -= SERVO_TRIM_STEP_BIG_US; break;
|
|
|
|
|
case SERVO_TRIM_MINUS_SMALL: t -= SERVO_TRIM_STEP_SMALL_US; break;
|
|
|
|
|
case SERVO_TRIM_PLUS_SMALL: t += SERVO_TRIM_STEP_SMALL_US; break;
|
|
|
|
|
case SERVO_TRIM_PLUS_BIG: t += SERVO_TRIM_STEP_BIG_US; break;
|
|
|
|
|
case SERVO_TRIM_ZERO: t = 0; break;
|
|
|
|
|
default: return false;
|
|
|
|
|
}
|
|
|
|
|
trim_target_ = (int16_t)constrain(t, -CAL_TRIM_LIMIT_US, CAL_TRIM_LIMIT_US);
|
|
|
|
|
return true;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
case UiCmd::ServoCalSave:
|
|
|
|
|
case UiCmd::ServoCalRevert:
|
|
|
|
|
if (!servo_cal_) { setMessage("Tap START first", now); return false; }
|
|
|
|
|
cal_op_ = e.cmd == UiCmd::ServoCalSave ? CAL_SAVE : CAL_REVERT;
|
|
|
|
|
cal_op_until_ms_ = now + SERVO_CAL_OP_MS;
|
|
|
|
|
setMessage(e.cmd == UiCmd::ServoCalSave ? "Trims saved on the robot" : "Unsaved trims dropped", now);
|
|
|
|
|
return true;
|
|
|
|
|
|
2026-09-28 18:14:00 +02:00
|
|
|
case UiCmd::CalCancel:
|
|
|
|
|
if (!calibrating_) return false;
|
|
|
|
|
stick_hw.cancelCalibration();
|
|
|
|
|
calibrating_ = false;
|
|
|
|
|
setMessage("Calibration cancelled", now);
|
|
|
|
|
return true;
|
|
|
|
|
}
|
|
|
|
|
return false;
|
|
|
|
|
}
|
|
|
|
|
|
2026-11-17 14:42:37 +01:00
|
|
|
void RemoteState::update(float dt, bool link_ok, bool tele_fresh, const Telemetry& tele, uint32_t now_ms) {
|
2026-09-28 18:14:00 +02:00
|
|
|
// Mirror the robot's link failsafe: it disarms (servos relax) when packets
|
|
|
|
|
// stop, so the remote disarms too and the operator must re-arm on purpose
|
2026-11-17 14:42:37 +01:00
|
|
|
// once the link is back. Servo calibration stops the same way, so the
|
|
|
|
|
// servos don't spring back to centre on their own when the link returns.
|
|
|
|
|
if (!link_ok && link_was_ok_) {
|
|
|
|
|
if (armed_) {
|
|
|
|
|
disarm();
|
|
|
|
|
setMessage("Link lost - disarmed", now_ms);
|
|
|
|
|
}
|
|
|
|
|
if (servo_cal_) {
|
|
|
|
|
stopServoCal();
|
|
|
|
|
setMessage("Link lost - servo cal stopped", now_ms);
|
|
|
|
|
}
|
2026-09-28 18:14:00 +02:00
|
|
|
}
|
|
|
|
|
link_was_ok_ = link_ok;
|
|
|
|
|
|
2026-11-17 14:42:37 +01:00
|
|
|
// SAVE / REVERT done: after a revert the robot's trims changed under us, so
|
|
|
|
|
// read the selected one back again.
|
|
|
|
|
if (cal_op_ != CAL_NONE && now_ms >= cal_op_until_ms_) {
|
|
|
|
|
if (cal_op_ == CAL_REVERT) trim_synced_ = false;
|
|
|
|
|
cal_op_ = CAL_NONE;
|
|
|
|
|
}
|
|
|
|
|
if (servo_cal_ && !trim_synced_ && cal_op_ == CAL_NONE && tele_fresh &&
|
|
|
|
|
tele.state == TSTATE_SERVO_CAL && tele.cal_servo == servo_sel_) {
|
|
|
|
|
trim_target_ = tele.cal_trim_us;
|
|
|
|
|
trim_synced_ = true;
|
|
|
|
|
}
|
|
|
|
|
|
2026-09-28 18:14:00 +02:00
|
|
|
speed_.setTarget(speed_target_);
|
|
|
|
|
speed_.update(dt);
|
|
|
|
|
tilt_blend_.setTarget(remote_gyro_ ? 1.0f : 0.0f);
|
|
|
|
|
tilt_blend_.update(dt);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void RemoteState::fillView(UiView& v, uint32_t now_ms) const {
|
|
|
|
|
v.armed = armed_;
|
|
|
|
|
v.stand = stand_;
|
|
|
|
|
v.gait_wave = gait_wave_;
|
|
|
|
|
v.mode_b = mode_b_;
|
|
|
|
|
v.rstick_xlate = rstick_xlate_;
|
|
|
|
|
v.remote_gyro = remote_gyro_;
|
|
|
|
|
v.robot_gyro = robot_gyro_;
|
2026-11-17 14:42:37 +01:00
|
|
|
v.stick_calibrating = calibrating_;
|
|
|
|
|
v.servo_cal = servo_cal_;
|
|
|
|
|
v.servo_sel = servo_sel_;
|
|
|
|
|
v.trim_synced = trim_synced_;
|
|
|
|
|
v.trim_us = trim_target_;
|
|
|
|
|
v.upload_confirm = now_ms < upload_confirm_until_ms_;
|
2026-09-28 18:14:00 +02:00
|
|
|
v.speed_target = speed_target_;
|
2026-09-29 00:37:12 +02:00
|
|
|
v.height = height_;
|
2026-09-28 18:14:00 +02:00
|
|
|
v.message = message(now_ms);
|
|
|
|
|
}
|