Files
hexapod-robot/2026-current/Hexapod_Simulation_Unity/Assets/HexapodSim/SimRemote.cs
T
mangavalkandClaude Opus 5.5 2b4750f8f4 2026 sim: drive the Unity simulation with the real remote over the radio
Dongle (app):
- bridge mode: stands in for the robot's radio (ch 96 / 250k / HXPD1,
  auto-ack, dynamic payloads, ack payloads). Received packets go to the PC
  as A5-framed 'R' frames; the PC's 'A' frames become the ack payload of
  the next packet, like robot/Comms.cpp; dropped after ~0.5 s without a
  refresh. 'S' status once a second.
- USART RX through circular DMA into a ring buffer, overrun stall
  disabled, error flags cleared: a burst (e.g. SignalRGB probing the port)
  used to deafen the app until a replug.
- RX test mode ('x') accepts protocol v4's dynamic-length packets.
- safeboot_flash.py --knock: keeps knocking across a replug to catch
  safeboot's cold-boot window when the app doesn't answer.

Unity:
- SimRemote sources: Keyboard, or the real remote via the dongle
  (DongleLink.cs: kernel32 serial, auto-finds the CH340, all port I/O on
  one background thread). Packets go into the firmware byte for byte, the
  robot's telemetry goes back on the ack, so the remote's display shows
  the sim. Tested end to end with the real remote.
- Dropped the USB joystick and robot-serial-harness inputs and the old
  Serial/Serial2/Serial3/HandleHoystick scripts (and their components in
  Main.unity): no USB links to remote or robot any more.
- Test panel: dongle status; no sideways scroll bar.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
2026-11-17 22:48:41 +01:00

243 lines
10 KiB
C#

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