Files
hexapod-robot/2026-current/remote/RemoteState.cpp
T

227 lines
8.1 KiB
C++
Raw Normal View History

#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;
}
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;
}
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; }
if (servo_cal_) { setMessage("Stop servo calibration first", now); return false; }
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;
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;
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;
case UiCmd::NextionUpload:
return false; // not operator state - remote.ino handles it
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;
case UiCmd::CalCancel:
if (!calibrating_) return false;
stick_hw.cancelCalibration();
calibrating_ = false;
setMessage("Calibration cancelled", now);
return true;
}
return false;
}
void RemoteState::update(float dt, bool link_ok, bool tele_fresh, const Telemetry& tele, uint32_t now_ms) {
// 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
// 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);
}
}
link_was_ok_ = link_ok;
// 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;
}
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_;
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_;
v.speed_target = speed_target_;
v.height = height_;
v.message = message(now_ms);
}