using UnityEngine; namespace HexapodSim { // Where the simulated robot's commands come from. Two sources: // Keyboard - a stand-in remote: builds CmdPackets per the protocol contract // and sends them over the simulated radio. Toggles and // interlocks mirror remote/RemoteState.cpp and // remote/PacketBuilder.cpp - keep them in sync if those change. // RealRemote - the real handheld remote, over the air: the USB nRF24 dongle // (2026-current/dongle/, bridge mode) receives its packets, they // go into the firmware unchanged, and the robot's Telemetry goes // back as the radio's ack payload. The remote's display then // shows the simulated robot. Nothing about the remote is // mirrored here: its own firmware does all of it. // Toggles, the speed slider and the handset tilt sliders live in SimTestPanel. public class SimRemote : MonoBehaviour { public enum InputSource { Keyboard, RealRemote } public InputSource source = InputSource.Keyboard; [Header("Real remote (USB nRF24 dongle)")] [Tooltip("\"auto\" finds the dongle's CH340 port; or e.g. COM7.")] public string donglePort = "auto"; [Header("Operator state (same toggles as the Nextion screen)")] public bool armed; public bool stand; public bool gaitWave; public bool modeB; public bool rstickXlate; public bool remoteGyro; public bool robotGyro; [Range(0, 255)] public int speedTarget = 128; [Tooltip("Standing height setting, -1000 lowest .. 0 default .. 1000 highest (CmdPacket.body_height).")] [Range(-1000, 1000)] public int height = 0; [Header("Simulated handset tilt (the remote's MPU6050)")] [Range(-30, 30)] public float handsetRollDeg; [Range(-30, 30)] public float handsetPitchDeg; public float handsetFullScaleDeg = 30f; // remote Config.h REMOTE_TILT_FULL_SCALE_DEG [Header("Smoothing, as on the remote")] public float speedRatePerSec = 255f; // SPEED_SCALE_RATE_PER_SEC public float walkLingerSec = 0.25f; // WALK_LINGER_MS public float armCenterTolerance = 0.05f; // ARM_CENTER_TOLERANCE / 1000 public float keyStickRatePerSec = 4f; // how fast a held key moves the virtual stick // Latest processed sticks, -1..1: [L_X, L_Y, L_TWIST, R_X, R_Y, R_TWIST] (keyboard). public readonly float[] sticks = new float[6]; public CmdPacket LastPacket { get; private set; } public string Message { get; private set; } = ""; // Real remote, for the panel. public DongleLink Dongle { get; private set; } public int RemotePacketsPerSec { get; private set; } public int LastPacketLength { get; private set; } public float SecondsSinceRemotePacket => Time.time - lastRemotePacket_; float speedCurrent_ = 128; float lastWalkInputTime_ = -999f; byte seq_; float messageUntil_; float lastRemotePacket_ = -999f; int packetsThisSecond_; float secondStart_; readonly byte[] ack_ = new byte[32]; public void ResetState() { armed = stand = false; speedCurrent_ = speedTarget; lastWalkInputTime_ = -999f; } void Update() { // Open the dongle while the real remote is selected, release it otherwise. if (source == InputSource.RealRemote && Dongle == null) ConnectDongle(); else if (source != InputSource.RealRemote && Dongle != null) DisconnectDongle(); if (source == InputSource.Keyboard) { HandleToggleKeys(); ReadKeyboardSticks(Time.deltaTime); } if (Time.time > messageUntil_) Message = ""; } void OnDisable() => DisconnectDongle(); public void ConnectDongle() { DisconnectDongle(); Dongle = DongleLink.Open(donglePort); } public void DisconnectDongle() { Dongle?.Dispose(); Dongle = null; } // ---- Operator commands (also called by SimTestPanel buttons) ---- public void ToggleArm() { if (armed) { Disarm(); return; } foreach (float v in sticks) if (Mathf.Abs(v) > armCenterTolerance) { Say("Center both sticks to arm"); return; } armed = true; } public void Disarm() { armed = false; stand = false; } // disarm always clears stand public void ToggleStand() { if (!armed) { Say("Arm first"); return; } stand = !stand; } public void ToggleRemoteGyro() => remoteGyro = !remoteGyro; void Say(string msg) { Message = msg; messageUntil_ = Time.time + 2.5f; } void HandleToggleKeys() { if (Input.GetKeyDown(KeyCode.Alpha1)) ToggleArm(); if (Input.GetKeyDown(KeyCode.Alpha2)) ToggleStand(); if (Input.GetKeyDown(KeyCode.Alpha3)) gaitWave = !gaitWave; if (Input.GetKeyDown(KeyCode.Alpha4)) modeB = !modeB; if (Input.GetKeyDown(KeyCode.Alpha5)) rstickXlate = !rstickXlate; if (Input.GetKeyDown(KeyCode.Alpha6)) ToggleRemoteGyro(); if (Input.GetKeyDown(KeyCode.Alpha7)) robotGyro = !robotGyro; if (Input.GetKeyDown(KeyCode.Backspace)) Disarm(); if (Input.GetKeyDown(KeyCode.Equals) || Input.GetKeyDown(KeyCode.KeypadPlus)) speedTarget = Mathf.Min(255, speedTarget + 25); if (Input.GetKeyDown(KeyCode.Minus) || Input.GetKeyDown(KeyCode.KeypadMinus)) speedTarget = Mathf.Max(0, speedTarget - 25); if (Input.GetKeyDown(KeyCode.PageUp)) height = Mathf.Min(1000, height + 100); if (Input.GetKeyDown(KeyCode.PageDown)) height = Mathf.Max(-1000, height - 100); } // WASD + Q/E = left stick (walk, turn), IJKL + U/O = right stick. void ReadKeyboardSticks(float dt) { float Key(KeyCode pos, KeyCode neg) => (Input.GetKey(pos) ? 1 : 0) - (Input.GetKey(neg) ? 1 : 0); float[] target = { Key(KeyCode.D, KeyCode.A), Key(KeyCode.W, KeyCode.S), Key(KeyCode.Q, KeyCode.E), Key(KeyCode.L, KeyCode.J), Key(KeyCode.I, KeyCode.K), Key(KeyCode.U, KeyCode.O), }; for (int i = 0; i < 6; i++) sticks[i] = Mathf.MoveTowards(sticks[i], target[i], keyStickRatePerSec * dt); } // ---- Transmit ---- // Called by HexapodSimulation once per remote TX period. public void Transmit(HexapodSimNative native, float dt) { if (source == InputSource.RealRemote) { ForwardRealRemote(native); return; } speedCurrent_ = Mathf.MoveTowards(speedCurrent_, speedTarget, speedRatePerSec * dt); LastPacket = BuildPacket(); native.RadioSend(LastPacket); } // Everything the dongle heard since last time goes into the firmware in // order, and each packet's ack carries the telemetry the robot loaded in // reply - the same exchange as on the real robot (robot/Comms.cpp). void ForwardRealRemote(HexapodSimNative native) { if (Time.time - secondStart_ >= 1f) { RemotePacketsPerSec = packetsThisSecond_; packetsThisSecond_ = 0; secondStart_ = Time.time; } if (Dongle == null || !Dongle.IsOpen) return; while (Dongle.TryTakePacket(out byte[] pkt)) { native.RadioSend(pkt); int n = native.GetAckPayload(ack_); if (n > 0) Dongle.SendAckPayload(ack_, n); LastPacketLength = pkt.Length; if (HexapodSimNative.TryDecode(pkt, out var p)) LastPacket = p; lastRemotePacket_ = Time.time; packetsThisSecond_++; } } static short Wire(float v) => (short)Mathf.RoundToInt(Mathf.Clamp(v, -1f, 1f) * 1000f); // Same mapping as remote/PacketBuilder.cpp. CmdPacket BuildPacket() { var p = new CmdPacket { seq = seq_++, speed_scale = (byte)Mathf.RoundToInt(speedCurrent_), body_height = (short)Mathf.Clamp(height, -1000, 1000), // a setting: sent even while disarmed cal_servo = 0xFF, // CAL_NO_SERVO: never calibrates }; byte f = 0; if (armed) f |= CmdPacket.FLAG_ARMED; if (stand) f |= CmdPacket.FLAG_STAND; if (modeB) f |= CmdPacket.FLAG_MODE_B; if (rstickXlate) f |= CmdPacket.FLAG_RSTICK_XLATE; if (gaitWave) f |= CmdPacket.FLAG_GAIT_WAVE; if (robotGyro) f |= CmdPacket.FLAG_ROBOT_GYRO; if (!armed) { p.flags = f; return p; } // disarmed: neutral command // Left stick: forward -> +vx, right -> -vy (vy + = left), twist CCW -> +yaw_rate. p.vx = Wire(sticks[1]); p.vy = Wire(-sticks[0]); p.yaw_rate = Wire(sticks[2]); if (rstickXlate) { p.rx = Wire(sticks[4]); // X offset: forward p.ry = Wire(-sticks[3]); // Y offset: right -> -Y p.rz = Wire(sticks[5]); // Z offset: twist CCW -> up } else { p.rx = Wire(sticks[3]); // roll: right side down p.ry = Wire(sticks[4]); // pitch: nose down p.rz = Wire(sticks[5]); // yaw: CCW } bool walkInput = p.vx != 0 || p.vy != 0 || (!modeB && p.yaw_rate != 0); if (walkInput) lastWalkInputTime_ = Time.time; if (stand && (walkInput || Time.time - lastWalkInputTime_ < walkLingerSec)) f |= CmdPacket.FLAG_WALK; if (remoteGyro) { f |= CmdPacket.FLAG_REMOTE_GYRO; p.remote_roll = Wire(handsetRollDeg / handsetFullScaleDeg); p.remote_pitch = Wire(handsetPitchDeg / handsetFullScaleDeg); } p.flags = f; return p; } } }