hexapod-robot/2026-current/Hexapod_Simulation_Unity/Assets/HexapodSim/SimTestPanel.cs

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 = { "Joystick", "Keyboard", "Robot serial" };

        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, 390, Screen.height - 20), GUI.skin.box);
            scroll_ = GUILayout.BeginScrollView(scroll_);

            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.RobotSerialHarness)
            {
                GUILayout.Label("Keys go to robot.ino's TEST_SERIAL_INPUT harness (no radio):\n" +
                                "a arm  s sit/stand  w/x fwd/back  q/e strafe L/R\n" +
                                "z/c yaw  g gait  m mode  space stop", mono_);
            }
            else
            {
                if (remote_.source == SimRemote.InputSource.Keyboard)
                    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_);
                if (remote_.source == SimRemote.InputSource.Joystick && remote_.JoystickAxesMissing)
                    GUILayout.Label("Input axes \"1\"..\"6\" are missing from the Input Manager.");

                // ---- 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 raw = remote_.rawAxes;
            var p = remote_.LastPacket;
            GUILayout.Label(
                $"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" +
                $"axes 1-6 {raw[0],5:F2} {raw[1],5:F2} {raw[2],5:F2} {raw[3],5:F2} {raw[4],5:F2} {raw[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();
        }

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