Files
hexapod-robot/2026-current/Hexapod_Simulation_Unity/Assets/HexapodSim/SimTestPanel.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

187 lines
10 KiB
C#

using System.Text;
using UnityEngine;
namespace HexapodSim
{
// On-screen control/status panel (IMGUI, so it needs no scene setup).
// Every remote toggle, the speed slider, simulated handset tilt, ground
// tilt, firmware reload, live robot state, the packet on air and the
// robot's serial output. F1 hides it.
public class SimTestPanel : MonoBehaviour
{
public bool visible = true;
HexapodSimulation sim_;
SimRemote remote_;
Vector2 scroll_;
GUIStyle mono_;
static readonly string[] SourceNames = { "Keyboard", "Real remote (radio)" };
void Awake()
{
sim_ = GetComponent<HexapodSimulation>();
remote_ = GetComponent<SimRemote>();
}
void Update()
{
if (Input.GetKeyDown(KeyCode.F1)) visible = !visible;
}
void OnGUI()
{
if (!visible || sim_ == null || remote_ == null) return;
if (mono_ == null)
mono_ = new GUIStyle(GUI.skin.label) { font = Font.CreateDynamicFontFromOSFont("Consolas", 12), fontSize = 12, wordWrap = false };
GUILayout.BeginArea(new Rect(10, 10, 420, Screen.height - 20), GUI.skin.box);
// Vertical scrolling only: the monospace lines run a little past the
// width, and a sideways bar for that is just noise.
scroll_ = GUILayout.BeginScrollView(scroll_, false, false, GUIStyle.none, GUI.skin.verticalScrollbar);
GUILayout.Label("<b>Hexapod firmware simulation</b> (F1 hides)", new GUIStyle(GUI.skin.label) { richText = true });
if (sim_.LoadError != null)
{
GUILayout.Label("<color=#ff6060>" + sim_.LoadError + "</color>", new GUIStyle(GUI.skin.label) { richText = true, wordWrap = true });
if (GUILayout.Button("Retry loading firmware")) sim_.ReloadFirmware();
GUILayout.EndScrollView();
GUILayout.EndArea();
return;
}
// ---- Robot status (read from the firmware's own globals) ----
var s = sim_.Status;
GUILayout.Label(
$"state {s.StateName,-8} blend {s.sitStandBlend:F2} link {(s.linkOk != 0 ? "OK" : "DOWN")}\n" +
$"heading {s.headingDeg,7:F1} IMU r {s.imuRollDeg,5:F1} p {s.imuPitchDeg,5:F1} {(s.imuEnabled != 0 ? "on" : "off")}\n" +
$"t {s.simTimeMs / 1000f,7:F1}s ticks {s.controlTicks} stance {StanceString(s.stanceMask)}\n" +
$"body pose {s.poseReachScale * 100f,3:F0}% walk {s.twistScale * 100f,3:F0}%" +
$"{(s.poseReachScale < 0.995f || s.twistScale < 0.995f ? " (limited: reach / leg clearance)" : "")}\n" +
$"body x {sim_.BodyPosMm.x,7:F0} y {sim_.BodyPosMm.y,7:F0} z {sim_.BodyPosMm.z,5:F0} mm yaw {sim_.BodyYawDeg,6:F1}",
mono_);
foreach (var w in sim_.RigWarnings)
GUILayout.Label("<color=#ffb040>rig: " + w + "</color>", new GUIStyle(GUI.skin.label) { richText = true, wordWrap = true, fontSize = 11 });
if (GUILayout.Button("Reload firmware (fresh power-on, picks up a new DLL build)")) sim_.ReloadFirmware();
// ---- Input source ----
GUILayout.Space(6);
remote_.source = (SimRemote.InputSource)GUILayout.Toolbar((int)remote_.source, SourceNames);
if (remote_.source == SimRemote.InputSource.RealRemote)
{
DongleSection();
}
else
{
GUILayout.Label("WASD walk Q/E turn IJKL + U/O right stick\n1 arm 2 stand 3 gait 4 mode 5 R-stick 6/7 gyros\nBackspace disarm +/- speed PgUp/PgDn height", mono_);
// ---- Toggles: same set as the remote's touchscreen ----
GUILayout.BeginHorizontal();
if (GUILayout.Button(remote_.armed ? "ARMED" : "DISARMED")) remote_.ToggleArm();
if (GUILayout.Button("DISARM")) remote_.Disarm();
if (GUILayout.Button(remote_.stand ? "STAND" : "SIT")) remote_.ToggleStand();
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
if (GUILayout.Button(remote_.gaitWave ? "WAVE" : "TRIPOD")) remote_.gaitWave = !remote_.gaitWave;
if (GUILayout.Button(remote_.modeB ? "HEADING" : "DIRECT")) remote_.modeB = !remote_.modeB;
if (GUILayout.Button(remote_.rstickXlate ? "R: MOVE" : "R: ROTATE")) remote_.rstickXlate = !remote_.rstickXlate;
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
remote_.remoteGyro = GUILayout.Toggle(remote_.remoteGyro, " remote gyro");
remote_.robotGyro = GUILayout.Toggle(remote_.robotGyro, " robot gyro");
GUILayout.EndHorizontal();
if (!string.IsNullOrEmpty(remote_.Message)) GUILayout.Label("> " + remote_.Message);
GUILayout.Label($"speed {remote_.speedTarget * 100 / 255}%");
remote_.speedTarget = Mathf.RoundToInt(GUILayout.HorizontalSlider(remote_.speedTarget, 0, 255));
var geo = sim_.Geometry;
float targetMm = geo.stanceHeightMm + remote_.height / 1000f * geo.stanceHeightRangeMm;
GUILayout.Label($"height {targetMm:F0} mm (now {sim_.Status.stanceHeightMm:F0}) PgUp/PgDn");
remote_.height = Mathf.RoundToInt(GUILayout.HorizontalSlider(remote_.height, -1000, 1000) / 50f) * 50;
GUILayout.Label($"handset tilt roll {remote_.handsetRollDeg:F0} pitch {remote_.handsetPitchDeg:F0} deg");
remote_.handsetRollDeg = GUILayout.HorizontalSlider(remote_.handsetRollDeg, -30, 30);
remote_.handsetPitchDeg = GUILayout.HorizontalSlider(remote_.handsetPitchDeg, -30, 30);
}
GUILayout.Label($"ground tilt roll {sim_.groundTiltDeg.x:F0} pitch {sim_.groundTiltDeg.y:F0} deg (robot gyro test)");
sim_.groundTiltDeg.x = GUILayout.HorizontalSlider(sim_.groundTiltDeg.x, -15, 15);
sim_.groundTiltDeg.y = GUILayout.HorizontalSlider(sim_.groundTiltDeg.y, -15, 15);
if (GUILayout.Button("Level ground")) sim_.groundTiltDeg = Vector2.zero;
// ---- Live values ----
GUILayout.Space(6);
var st = remote_.sticks;
var p = remote_.LastPacket;
GUILayout.Label(
(remote_.source == SimRemote.InputSource.Keyboard
? $"sticks L {st[0],5:F2} {st[1],5:F2} {st[2],5:F2} R {st[3],5:F2} {st[4],5:F2} {st[5],5:F2}\n" : "") +
$"packet #{p.seq,3} vx {p.vx,5} vy {p.vy,5} yaw {p.yaw_rate,5}\n" +
$" r {p.rx,5} {p.ry,5} {p.rz,5} tilt {p.remote_roll,5} {p.remote_pitch,5}\n" +
$" height {p.body_height,5} speed {p.speed_scale,3} flags {System.Convert.ToString(p.flags, 2).PadLeft(8, '0')}",
mono_);
GUILayout.Label(JointTable(), mono_);
GUILayout.Label("robot serial:", mono_);
var log = new StringBuilder();
for (int i = Mathf.Max(0, sim_.SerialLog.Count - 10); i < sim_.SerialLog.Count; i++) log.AppendLine(sim_.SerialLog[i]);
GUILayout.Label(log.ToString(), mono_);
GUILayout.EndScrollView();
GUILayout.EndArea();
}
// The real remote drives the simulation over the air through the USB dongle;
// every control is on the remote itself, so this only shows the link.
void DongleSection()
{
var d = remote_.Dongle;
var rich = new GUIStyle(GUI.skin.label) { richText = true, wordWrap = true };
GUILayout.BeginHorizontal();
GUILayout.Label("dongle port", GUILayout.Width(80));
remote_.donglePort = GUILayout.TextField(remote_.donglePort, GUILayout.Width(70));
if (GUILayout.Button("Reconnect")) remote_.ConnectDongle();
GUILayout.EndHorizontal();
if (d == null) { GUILayout.Label("connecting..."); return; }
if (!d.IsOpen || d.Error != null)
{
GUILayout.Label("<color=#ff6060>" + (d.Error ?? "not open") + "</color>", rich);
if (!d.IsOpen) return;
}
float age = remote_.SecondsSinceRemotePacket;
string link = age < 0.5f ? "<color=#60ff60>receiving</color>" : "<color=#ffb040>no packets - is the remote on?</color>";
GUILayout.Label(
$"{d.PortName}: {link}\n" +
$"remote {remote_.RemotePacketsPerSec} pkt/s here, dongle {d.DonglePacketsPerSec} pkt/s, acks {d.DongleAcksPerSec}/s" +
(remote_.LastPacketLength != 0 && remote_.LastPacketLength != System.Runtime.InteropServices.Marshal.SizeOf<CmdPacket>()
? $"\n<color=#ff6060>packets are {remote_.LastPacketLength} bytes: remote and sim built from different protocol versions</color>" : "") +
(d.BadFrames > 0 ? $"\n{d.BadFrames} corrupt frames" : ""),
rich);
GUILayout.Label("Use the remote's sticks and touchscreen. Its display shows this simulated robot.", rich);
}
static string StanceString(int mask)
{
var sb = new StringBuilder(6);
for (int i = 0; i < 6; i++) sb.Append((mask & (1 << i)) != 0 ? '#' : '.');
return sb.ToString();
}
// Per leg: decoded joint angles, whether the servo got fresh pulses
// (a leg that stops updating while armed = IK rejected the target),
// and whether the foot is on the ground.
string JointTable()
{
var sb = new StringBuilder("leg coxa femur tibia att gnd\n");
for (int i = 0; i < 6; i++)
{
sb.Append($"{i,3} {sim_.JointsDeg[i * 3],6:F1} {sim_.JointsDeg[i * 3 + 1],6:F1} {sim_.JointsDeg[i * 3 + 2],6:F1}");
sb.Append($" {(sim_.ServoAttached[i * 3] != 0 ? " y " : " - ")} {(sim_.Contact[i] ? " # " : " . ")}\n");
}
return sb.ToString();
}
}
}