2026-09-29 00:37:12 +02:00
|
|
|
using UnityEngine;
|
|
|
|
|
|
|
|
|
|
namespace HexapodSim
|
|
|
|
|
{
|
2026-11-17 22:48:41 +01:00
|
|
|
// 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.
|
2026-09-29 00:37:12 +02:00
|
|
|
// Toggles, the speed slider and the handset tilt sliders live in SimTestPanel.
|
|
|
|
|
public class SimRemote : MonoBehaviour
|
|
|
|
|
{
|
2026-11-17 22:48:41 +01:00
|
|
|
public enum InputSource { Keyboard, RealRemote }
|
2026-09-29 00:37:12 +02:00
|
|
|
|
|
|
|
|
public InputSource source = InputSource.Keyboard;
|
|
|
|
|
|
2026-11-17 22:48:41 +01:00
|
|
|
[Header("Real remote (USB nRF24 dongle)")]
|
|
|
|
|
[Tooltip("\"auto\" finds the dongle's CH340 port; or e.g. COM7.")]
|
|
|
|
|
public string donglePort = "auto";
|
2026-09-29 00:37:12 +02:00
|
|
|
|
|
|
|
|
[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
|
|
|
|
|
|
2026-11-17 22:48:41 +01:00
|
|
|
// Latest processed sticks, -1..1: [L_X, L_Y, L_TWIST, R_X, R_Y, R_TWIST] (keyboard).
|
2026-09-29 00:37:12 +02:00
|
|
|
public readonly float[] sticks = new float[6];
|
|
|
|
|
public CmdPacket LastPacket { get; private set; }
|
|
|
|
|
public string Message { get; private set; } = "";
|
2026-11-17 22:48:41 +01:00
|
|
|
|
|
|
|
|
// 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_;
|
2026-09-29 00:37:12 +02:00
|
|
|
|
|
|
|
|
float speedCurrent_ = 128;
|
|
|
|
|
float lastWalkInputTime_ = -999f;
|
|
|
|
|
byte seq_;
|
|
|
|
|
float messageUntil_;
|
2026-11-17 22:48:41 +01:00
|
|
|
float lastRemotePacket_ = -999f;
|
|
|
|
|
int packetsThisSecond_;
|
|
|
|
|
float secondStart_;
|
|
|
|
|
readonly byte[] ack_ = new byte[32];
|
2026-09-29 00:37:12 +02:00
|
|
|
|
|
|
|
|
public void ResetState()
|
|
|
|
|
{
|
|
|
|
|
armed = stand = false;
|
|
|
|
|
speedCurrent_ = speedTarget;
|
|
|
|
|
lastWalkInputTime_ = -999f;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void Update()
|
|
|
|
|
{
|
2026-11-17 22:48:41 +01:00
|
|
|
// 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();
|
2026-09-29 00:37:12 +02:00
|
|
|
|
2026-11-17 22:48:41 +01:00
|
|
|
if (source == InputSource.Keyboard)
|
2026-09-29 00:37:12 +02:00
|
|
|
{
|
2026-11-17 22:48:41 +01:00
|
|
|
HandleToggleKeys();
|
|
|
|
|
ReadKeyboardSticks(Time.deltaTime);
|
2026-09-29 00:37:12 +02:00
|
|
|
}
|
|
|
|
|
if (Time.time > messageUntil_) Message = "";
|
|
|
|
|
}
|
|
|
|
|
|
2026-11-17 22:48:41 +01:00
|
|
|
void OnDisable() => DisconnectDongle();
|
|
|
|
|
|
|
|
|
|
public void ConnectDongle()
|
|
|
|
|
{
|
|
|
|
|
DisconnectDongle();
|
|
|
|
|
Dongle = DongleLink.Open(donglePort);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
public void DisconnectDongle()
|
|
|
|
|
{
|
|
|
|
|
Dongle?.Dispose();
|
|
|
|
|
Dongle = null;
|
|
|
|
|
}
|
|
|
|
|
|
2026-09-29 00:37:12 +02:00
|
|
|
// ---- 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)
|
|
|
|
|
{
|
2026-11-17 22:48:41 +01:00
|
|
|
if (source == InputSource.RealRemote) { ForwardRealRemote(native); return; }
|
2026-09-29 00:37:12 +02:00
|
|
|
speedCurrent_ = Mathf.MoveTowards(speedCurrent_, speedTarget, speedRatePerSec * dt);
|
|
|
|
|
LastPacket = BuildPacket();
|
|
|
|
|
native.RadioSend(LastPacket);
|
|
|
|
|
}
|
|
|
|
|
|
2026-11-17 22:48:41 +01:00
|
|
|
// 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_++;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2026-09-29 00:37:12 +02:00
|
|
|
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
|
2026-11-17 22:48:41 +01:00
|
|
|
cal_servo = 0xFF, // CAL_NO_SERVO: never calibrates
|
2026-09-29 00:37:12 +02:00
|
|
|
};
|
|
|
|
|
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;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}
|