#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); }