Files
hexapod-robot/2026-current/Hexapod_Simulation_Unity/Assets/HexapodSim/SimRemote.cs
T

242 lines
10 KiB
C#
Raw Normal View History

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