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>
This commit is contained in:
1 parent
6b3b2629cd
commit
2b4750f8f4
24 files changed
+677
-705
No files matched your search
+16
-10
@@ -188,8 +188,10 @@ Remote stick-frame convention: logical X + = right, Y + = forward, twist +
|
||||
`Knie.x = −(tibia + 90)`.
|
||||
- **Unity (`Assets/HexapodSim/`):**
|
||||
- `HexapodSimBootstrap` auto-adds `HexapodSimulation` in any scene and
|
||||
disables the old scripts (`Walk`, `RotationControl`, `Leg`,
|
||||
`HandleHoystick`). Those are kept for reference only.
|
||||
disables the old scripts (`Walk`, `RotationControl`, `Leg`). Those are
|
||||
kept for reference only. The old USB serial / joystick scripts
|
||||
(`Serial*`, `HandleHoystick`) were removed (owner, 2026-11-17: no USB
|
||||
links to remote or robot any more, everything goes over the radio).
|
||||
- `OpenScadModel` replaces the old scene's robot with the OpenSCAD model
|
||||
(meshes from `hardware/openscad/export_unity.py` in
|
||||
`Resources/OpenScadModel/`): it builds a new rig with the same names at
|
||||
@@ -198,9 +200,16 @@ Remote stick-frame convention: logical X + = right, Y + = forward, twist +
|
||||
- `HexapodSimNative` loads a temp copy of the DLL via
|
||||
LoadLibrary/GetProcAddress, so there's no file lock and each Play is a
|
||||
fresh power-on.
|
||||
- `SimRemote` stands in for the remote (Joystick axes "1"–"6" as in the
|
||||
old project, Keyboard, or the robot's own serial harness) and mirrors
|
||||
`remote/PacketBuilder.cpp` + `RemoteState.cpp` rules. Keep them in sync.
|
||||
- `SimRemote` has two sources. **Keyboard** stands in for the remote and
|
||||
mirrors `remote/PacketBuilder.cpp` + `RemoteState.cpp` rules (keep them
|
||||
in sync). **Real remote** takes the real remote's packets over the air
|
||||
from the USB nRF24 dongle (`DongleLink.cs`, the dongle's bridge mode,
|
||||
see `dongle/README.md`), feeds them to the firmware unchanged
|
||||
(`HexapodSimNative.RadioSend(byte[])`) and returns the robot's
|
||||
telemetry as the ack payload, so the remote's display shows the sim.
|
||||
`DongleLink` opens the port through kernel32 (no `System.IO.Ports`),
|
||||
auto-finds the CH340 (VID 1A86 PID 7523), and does all port I/O on one
|
||||
background thread.
|
||||
- `SimTestPanel` is the IMGUI panel (F1).
|
||||
- Body motion is kinematic and lives **in the DLL** (`sim/SimBody.cpp`,
|
||||
`hxsim_get_body`), so Unity shows exactly what `sim_smoke` checks.
|
||||
@@ -225,8 +234,8 @@ Remote stick-frame convention: logical X + = right, Y + = forward, twist +
|
||||
stand → walk. The harness does not exercise the right stick or remote
|
||||
tilt. Add keys if needed.
|
||||
- **Robot firmware in simulation:** `sim/build/sim_smoke.exe` (headless,
|
||||
scripted checks), or Unity Play mode with Keyboard / Joystick / Robot
|
||||
serial input.
|
||||
scripted checks), or Unity Play mode with Keyboard input or the real
|
||||
remote over the radio (USB nRF24 dongle).
|
||||
- **Remote without robot or display:** the USB serial console (see
|
||||
`remote/README.md`). `p` shows raw/mapped sticks, IMU, link and state.
|
||||
`d` dumps packets at 10 Hz. With no robot, the link shows DOWN and
|
||||
@@ -441,9 +450,6 @@ Remote (`remote/README.md` has detail):
|
||||
- USB serial at 250000. It received 18 servo bytes + checksum + `0xFE`
|
||||
from the old Unity `Serial.cs` (servo passthrough, forwarded over RF
|
||||
to the robot) and printed `i_value,...` stick lines.
|
||||
- The Unity remote stand-in's joystick axis assignment (which of axes 4/6
|
||||
is forward) couldn't be recovered from the old code. Check it with the
|
||||
panel's raw-axis readout.
|
||||
- Radio retry timing (`RADIO_RETRY_*`) is untested under real link loss.
|
||||
|
||||
## Working rules for this repo
|
||||
|
||||
@@ -0,0 +1,256 @@
|
||||
using System;
|
||||
using System.Collections.Concurrent;
|
||||
using System.Runtime.InteropServices;
|
||||
using System.Text;
|
||||
using System.Threading;
|
||||
|
||||
namespace HexapodSim
|
||||
{
|
||||
// The USB nRF24 dongle (2026-current/dongle/) in bridge mode: it stands in for
|
||||
// the robot's radio. Every packet the real remote sends comes in here; the
|
||||
// simulated robot's Telemetry goes back out as the radio's ack payload, so the
|
||||
// remote and its display see the simulation exactly like the real robot.
|
||||
//
|
||||
// Wire format both ways: A5 type len payload[len] sum (sum = type+len+payload).
|
||||
// to dongle 'L' listen (bridge on), 'X' stop, 'A' ack payload (<= 32 B)
|
||||
// from dongle 'R' received packet, 'S' status (packets u16, acks loaded u16)
|
||||
// The dongle drops the ack payload if it isn't refreshed for ~0.5 s.
|
||||
//
|
||||
// Windows only (the dev PC): the port is opened through kernel32, so there's
|
||||
// no dependency on System.IO.Ports or the project's API compatibility level.
|
||||
public sealed class DongleLink : IDisposable
|
||||
{
|
||||
public string PortName { get; private set; }
|
||||
public bool IsOpen => handle_ != IntPtr.Zero && handle_ != InvalidHandle;
|
||||
public string Error { get; private set; }
|
||||
public int DonglePacketsPerSec { get; private set; } // from the dongle's status frame
|
||||
public int DongleAcksPerSec { get; private set; }
|
||||
public int BadFrames { get; private set; }
|
||||
|
||||
readonly ConcurrentQueue<byte[]> packets_ = new ConcurrentQueue<byte[]>();
|
||||
byte[] pendingAck_; // newest telemetry frame, written by the I/O thread
|
||||
IntPtr handle_ = IntPtr.Zero;
|
||||
Thread io_;
|
||||
volatile bool running_;
|
||||
|
||||
// ---- Opening ----
|
||||
|
||||
// "auto" = the first USB-serial CH340 port Windows knows about (the dongle's
|
||||
// CH330N shows up as VID 1A86 / PID 7523); otherwise e.g. "COM7".
|
||||
public static DongleLink Open(string port)
|
||||
{
|
||||
var link = new DongleLink();
|
||||
string name = port;
|
||||
if (string.IsNullOrEmpty(name) || name.Equals("auto", StringComparison.OrdinalIgnoreCase))
|
||||
{
|
||||
name = FindCh340Port();
|
||||
if (name == null) { link.Error = "no USB nRF24 dongle (CH340, VID 1A86 PID 7523) found"; return link; }
|
||||
}
|
||||
link.PortName = name;
|
||||
link.OpenPort(name);
|
||||
if (link.IsOpen)
|
||||
{
|
||||
link.Write(Frame((byte)'L', null, 0));
|
||||
link.running_ = true;
|
||||
link.io_ = new Thread(link.IoLoop) { IsBackground = true, Name = "DongleLink" };
|
||||
link.io_.Start();
|
||||
}
|
||||
return link;
|
||||
}
|
||||
|
||||
void OpenPort(string name)
|
||||
{
|
||||
// Retry briefly: SignalRGB and similar tools grab CH340 ports on enumeration.
|
||||
for (int attempt = 0; attempt < 25; attempt++)
|
||||
{
|
||||
handle_ = CreateFile(@"\\.\" + name, GENERIC_READ | GENERIC_WRITE, 0, IntPtr.Zero, OPEN_EXISTING, 0, IntPtr.Zero);
|
||||
if (handle_ != InvalidHandle) break;
|
||||
Thread.Sleep(20);
|
||||
}
|
||||
if (handle_ == InvalidHandle)
|
||||
{
|
||||
Error = $"can't open {name} (Win32 error {Marshal.GetLastWin32Error()}; is SignalRGB or a serial monitor holding it?)";
|
||||
handle_ = IntPtr.Zero;
|
||||
return;
|
||||
}
|
||||
var dcb = new DCB { DCBlength = (uint)Marshal.SizeOf<DCB>() };
|
||||
GetCommState(handle_, ref dcb);
|
||||
dcb.BaudRate = 115200;
|
||||
dcb.ByteSize = 8;
|
||||
dcb.Parity = 0;
|
||||
dcb.StopBits = 0;
|
||||
dcb.Flags = 0x0001 | 0x0010; // fBinary, fDtrControl = DTR_CONTROL_ENABLE
|
||||
SetCommState(handle_, ref dcb);
|
||||
// Reads return within ~3 ms, so queued telemetry goes out promptly
|
||||
// (reads and writes on one handle take turns: both run on the I/O thread).
|
||||
var to = new COMMTIMEOUTS { ReadIntervalTimeout = 1, ReadTotalTimeoutConstant = 3, WriteTotalTimeoutConstant = 200 };
|
||||
SetCommTimeouts(handle_, ref to);
|
||||
PurgeComm(handle_, 0x0004 | 0x0008); // RXCLEAR | TXCLEAR
|
||||
}
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
running_ = false;
|
||||
io_?.Join(300);
|
||||
if (IsOpen)
|
||||
{
|
||||
Write(Frame((byte)'X', null, 0)); // bridge off: the remote sees the link drop
|
||||
CloseHandle(handle_);
|
||||
}
|
||||
handle_ = IntPtr.Zero;
|
||||
}
|
||||
|
||||
// ---- Traffic ----
|
||||
|
||||
public bool TryTakePacket(out byte[] packet) => packets_.TryDequeue(out packet);
|
||||
|
||||
// Queues the robot's telemetry for the ack of the remote's next packet.
|
||||
// Only the newest one matters, so an unsent older one is replaced.
|
||||
public void SendAckPayload(byte[] data, int len)
|
||||
{
|
||||
if (len <= 32) Volatile.Write(ref pendingAck_, Frame((byte)'A', data, len));
|
||||
}
|
||||
|
||||
static byte[] Frame(byte type, byte[] data, int len)
|
||||
{
|
||||
var f = new byte[4 + len];
|
||||
f[0] = 0xA5; f[1] = type; f[2] = (byte)len;
|
||||
int sum = type + len;
|
||||
for (int i = 0; i < len; i++) { f[3 + i] = data[i]; sum += data[i]; }
|
||||
f[3 + len] = (byte)sum;
|
||||
return f;
|
||||
}
|
||||
|
||||
bool Write(byte[] f)
|
||||
{
|
||||
if (WriteFile(handle_, f, (uint)f.Length, out _, IntPtr.Zero)) return true;
|
||||
Error = $"write failed (Win32 error {Marshal.GetLastWin32Error()}): dongle unplugged?";
|
||||
return false;
|
||||
}
|
||||
|
||||
void IoLoop()
|
||||
{
|
||||
var chunk = new byte[512];
|
||||
var buf = new byte[4096];
|
||||
int have = 0;
|
||||
while (running_)
|
||||
{
|
||||
byte[] ack = Interlocked.Exchange(ref pendingAck_, null);
|
||||
if (ack != null && !Write(ack)) break;
|
||||
if (!ReadFile(handle_, chunk, (uint)chunk.Length, out uint got, IntPtr.Zero))
|
||||
{
|
||||
if (running_) Error = $"read failed (Win32 error {Marshal.GetLastWin32Error()}): dongle unplugged?";
|
||||
break;
|
||||
}
|
||||
if (got == 0) continue;
|
||||
if (have + got > buf.Length) have = 0; // garbage flood: start over
|
||||
Buffer.BlockCopy(chunk, 0, buf, have, (int)got);
|
||||
have += (int)got;
|
||||
have = ParseFrames(buf, have);
|
||||
}
|
||||
}
|
||||
|
||||
// Consumes complete frames from buf[0..len), returns how many bytes remain.
|
||||
int ParseFrames(byte[] buf, int len)
|
||||
{
|
||||
int i = 0;
|
||||
while (true)
|
||||
{
|
||||
while (i < len && buf[i] != 0xA5) i++; // skip text between frames
|
||||
if (len - i < 4) break;
|
||||
byte type = buf[i + 1], n = buf[i + 2];
|
||||
if (n > 32) { i++; continue; }
|
||||
if (len - i < 4 + n) break;
|
||||
int sum = type + n;
|
||||
for (int k = 0; k < n; k++) sum += buf[i + 3 + k];
|
||||
if ((byte)sum != buf[i + 3 + n]) { BadFrames++; i++; continue; }
|
||||
var payload = new byte[n];
|
||||
Buffer.BlockCopy(buf, i + 3, payload, 0, n);
|
||||
i += 4 + n;
|
||||
if (type == 'R') packets_.Enqueue(payload);
|
||||
else if (type == 'S' && n >= 4)
|
||||
{
|
||||
DonglePacketsPerSec = payload[0] | payload[1] << 8;
|
||||
DongleAcksPerSec = payload[2] | payload[3] << 8;
|
||||
}
|
||||
}
|
||||
int rest = len - i;
|
||||
if (rest > 0 && i > 0) Buffer.BlockCopy(buf, i, buf, 0, rest);
|
||||
return rest;
|
||||
}
|
||||
|
||||
// ---- Finding the port: HKLM\SYSTEM\CurrentControlSet\Enum\USB\VID_1A86&PID_7523\<inst>\Device Parameters\PortName ----
|
||||
|
||||
static string FindCh340Port()
|
||||
{
|
||||
const string baseKey = @"SYSTEM\CurrentControlSet\Enum\USB\VID_1A86&PID_7523";
|
||||
if (RegOpenKeyEx(HKEY_LOCAL_MACHINE, baseKey, 0, KEY_READ, out IntPtr key) != 0) return null;
|
||||
try
|
||||
{
|
||||
var sb = new StringBuilder(256);
|
||||
for (uint idx = 0; ; idx++)
|
||||
{
|
||||
uint len = (uint)sb.Capacity;
|
||||
if (RegEnumKeyEx(key, idx, sb, ref len, IntPtr.Zero, IntPtr.Zero, IntPtr.Zero, IntPtr.Zero) != 0) break;
|
||||
string port = ReadString(HKEY_LOCAL_MACHINE, baseKey + "\\" + sb + "\\Device Parameters", "PortName");
|
||||
if (port != null && PortExists(port)) return port;
|
||||
}
|
||||
}
|
||||
finally { RegCloseKey(key); }
|
||||
return null;
|
||||
}
|
||||
|
||||
static string ReadString(IntPtr root, string path, string value)
|
||||
{
|
||||
var data = new StringBuilder(64);
|
||||
uint size = (uint)(data.Capacity * 2);
|
||||
return RegGetValue(root, path, value, RRF_RT_REG_SZ, IntPtr.Zero, data, ref size) == 0 ? data.ToString() : null;
|
||||
}
|
||||
|
||||
static bool PortExists(string name) => QueryDosDevice(name, new StringBuilder(256), 256) != 0;
|
||||
|
||||
// ---- Win32 ----
|
||||
|
||||
static readonly IntPtr InvalidHandle = new IntPtr(-1);
|
||||
static readonly IntPtr HKEY_LOCAL_MACHINE = new IntPtr(unchecked((int)0x80000002));
|
||||
const uint GENERIC_READ = 0x80000000, GENERIC_WRITE = 0x40000000, OPEN_EXISTING = 3;
|
||||
const int KEY_READ = 0x20019;
|
||||
const uint RRF_RT_REG_SZ = 0x00000002;
|
||||
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
struct DCB
|
||||
{
|
||||
public uint DCBlength, BaudRate, Flags;
|
||||
public ushort wReserved, XonLim, XoffLim;
|
||||
public byte ByteSize, Parity, StopBits;
|
||||
public sbyte XonChar, XoffChar, ErrorChar, EofChar, EvtChar;
|
||||
public ushort wReserved1;
|
||||
}
|
||||
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
struct COMMTIMEOUTS
|
||||
{
|
||||
public uint ReadIntervalTimeout, ReadTotalTimeoutMultiplier, ReadTotalTimeoutConstant,
|
||||
WriteTotalTimeoutMultiplier, WriteTotalTimeoutConstant;
|
||||
}
|
||||
|
||||
[DllImport("kernel32.dll", SetLastError = true, CharSet = CharSet.Unicode)]
|
||||
static extern IntPtr CreateFile(string name, uint access, uint share, IntPtr sec, uint disposition, uint flags, IntPtr template);
|
||||
[DllImport("kernel32.dll", SetLastError = true)] static extern bool CloseHandle(IntPtr h);
|
||||
[DllImport("kernel32.dll", SetLastError = true)] static extern bool GetCommState(IntPtr h, ref DCB dcb);
|
||||
[DllImport("kernel32.dll", SetLastError = true)] static extern bool SetCommState(IntPtr h, ref DCB dcb);
|
||||
[DllImport("kernel32.dll", SetLastError = true)] static extern bool SetCommTimeouts(IntPtr h, ref COMMTIMEOUTS t);
|
||||
[DllImport("kernel32.dll", SetLastError = true)] static extern bool PurgeComm(IntPtr h, uint flags);
|
||||
[DllImport("kernel32.dll", SetLastError = true)] static extern bool ReadFile(IntPtr h, byte[] buf, uint n, out uint got, IntPtr ov);
|
||||
[DllImport("kernel32.dll", SetLastError = true)] static extern bool WriteFile(IntPtr h, byte[] buf, uint n, out uint put, IntPtr ov);
|
||||
[DllImport("kernel32.dll", SetLastError = true, CharSet = CharSet.Unicode)]
|
||||
static extern uint QueryDosDevice(string device, StringBuilder target, int max);
|
||||
[DllImport("advapi32.dll", CharSet = CharSet.Unicode)]
|
||||
static extern int RegOpenKeyEx(IntPtr key, string sub, int options, int sam, out IntPtr result);
|
||||
[DllImport("advapi32.dll", CharSet = CharSet.Unicode)]
|
||||
static extern int RegEnumKeyEx(IntPtr key, uint index, StringBuilder name, ref uint nameLen, IntPtr reserved, IntPtr cls, IntPtr clsLen, IntPtr lastWrite);
|
||||
[DllImport("advapi32.dll", CharSet = CharSet.Unicode)]
|
||||
static extern int RegGetValue(IntPtr key, string sub, string value, uint flags, IntPtr type, StringBuilder data, ref uint size);
|
||||
[DllImport("advapi32.dll")] static extern int RegCloseKey(IntPtr key);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,2 @@
|
||||
fileFormatVersion: 2
|
||||
guid: d16e6c95081705a4788b93525709e796
|
||||
@@ -103,8 +103,8 @@ namespace HexapodSim
|
||||
VoidFn start_;
|
||||
UIntArgFn step_;
|
||||
PacketFn radioSend_;
|
||||
BytesInFn radioSendBytes_;
|
||||
ImuFn setImu_;
|
||||
BytesInFn serialInput_;
|
||||
BytesOutFn serialOutput_;
|
||||
ServosFn getServos_;
|
||||
FloatsFn getJoints_, getFeet_;
|
||||
@@ -142,8 +142,8 @@ namespace HexapodSim
|
||||
n.start_ = n.Fn<VoidFn>("hxsim_start");
|
||||
n.step_ = n.Fn<UIntArgFn>("hxsim_step");
|
||||
n.radioSend_ = n.Fn<PacketFn>("hxsim_radio_send");
|
||||
n.radioSendBytes_ = n.Fn<BytesInFn>("hxsim_radio_send");
|
||||
n.setImu_ = n.Fn<ImuFn>("hxsim_set_imu");
|
||||
n.serialInput_ = n.Fn<BytesInFn>("hxsim_serial_input");
|
||||
n.serialOutput_ = n.Fn<BytesOutFn>("hxsim_serial_output");
|
||||
n.getServos_ = n.Fn<ServosFn>("hxsim_get_servos");
|
||||
n.getJoints_ = n.Fn<FloatsFn>("hxsim_get_joints_deg");
|
||||
@@ -184,16 +184,24 @@ namespace HexapodSim
|
||||
public void Start() => start_();
|
||||
public void Step(uint dtMicros) => step_(dtMicros);
|
||||
public void RadioSend(CmdPacket p) => radioSend_(ref p, Marshal.SizeOf<CmdPacket>());
|
||||
// A packet exactly as it came over the air (the real remote, via the dongle).
|
||||
// Any length: the firmware itself drops ones that aren't sizeof(CmdPacket).
|
||||
public void RadioSend(byte[] bytes) => radioSendBytes_(bytes, bytes.Length);
|
||||
|
||||
// Decodes on-air bytes for display; false if the length doesn't match.
|
||||
public static bool TryDecode(byte[] bytes, out CmdPacket p)
|
||||
{
|
||||
p = default;
|
||||
if (bytes == null || bytes.Length != Marshal.SizeOf<CmdPacket>()) return false;
|
||||
var h = GCHandle.Alloc(bytes, GCHandleType.Pinned);
|
||||
try { p = Marshal.PtrToStructure<CmdPacket>(h.AddrOfPinnedObject()); }
|
||||
finally { h.Free(); }
|
||||
return true;
|
||||
}
|
||||
// Overrides the body-driven IMU with fixed readings from then on (normally unused).
|
||||
public void SetImu(Vector3 accelG, Vector3 gyroDps) =>
|
||||
setImu_(accelG.x, accelG.y, accelG.z, gyroDps.x, gyroDps.y, gyroDps.z);
|
||||
|
||||
public void SerialInput(string text)
|
||||
{
|
||||
if (string.IsNullOrEmpty(text)) return;
|
||||
byte[] b = System.Text.Encoding.ASCII.GetBytes(text);
|
||||
serialInput_(b, b.Length);
|
||||
}
|
||||
|
||||
readonly byte[] serialBuf_ = new byte[4096];
|
||||
public string SerialOutput()
|
||||
|
||||
@@ -81,7 +81,7 @@ namespace HexapodSim
|
||||
StringBuilder serialLine_ = new StringBuilder();
|
||||
|
||||
static readonly string[] LegacyMovementScripts =
|
||||
{ "Walk", "RotationControl", "Leg", "HandleHoystick", "Serial2", "Serial3" };
|
||||
{ "Walk", "RotationControl", "Leg" };
|
||||
|
||||
// Robot-frame axes, for readability (Unity's Vector3.up etc. are Unity axes).
|
||||
static readonly Vector3 AxisX = new Vector3(1, 0, 0); // forward
|
||||
|
||||
@@ -1,47 +1,28 @@
|
||||
using System;
|
||||
using UnityEngine;
|
||||
|
||||
namespace HexapodSim
|
||||
{
|
||||
// Stand-in for the handheld remote. Produces CmdPackets exactly 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.
|
||||
//
|
||||
// Input sources:
|
||||
// Joystick - the real remote as a USB game controller, read through
|
||||
// Unity input axes "1".."6" like the old HandleHoystick.
|
||||
// Keyboard - test without any remote (keys listed in SimTestPanel).
|
||||
// RobotSerialHarness - no radio at all; typed keys go to the firmware's own
|
||||
// TEST_SERIAL_INPUT harness, like the Arduino Serial Monitor.
|
||||
// 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 { Joystick, Keyboard, RobotSerialHarness }
|
||||
|
||||
[Serializable]
|
||||
public class AxisBinding
|
||||
{
|
||||
public string axis;
|
||||
public bool invert;
|
||||
public AxisBinding(string axis, bool invert) { this.axis = axis; this.invert = invert; }
|
||||
}
|
||||
public enum InputSource { Keyboard, RealRemote }
|
||||
|
||||
public InputSource source = InputSource.Keyboard;
|
||||
|
||||
// Defaults follow the old HandleHoystick: axes 4/6/5 were the walk stick
|
||||
// (b2/b1/b3), 2/1/3 the rotation stick (a1/a2/a3), with 1 and 2 inverted.
|
||||
// Which of 4/6 is forward vs sideways wasn't recoverable from the old code:
|
||||
// check the raw axis readout in the test panel and fix here.
|
||||
[Header("Joystick axes (Input Manager names)")]
|
||||
[Tooltip("Stick convention after invert: X + = right, Y + = forward, twist + = counterclockwise.")]
|
||||
public AxisBinding leftX = new AxisBinding("4", false);
|
||||
public AxisBinding leftY = new AxisBinding("6", false);
|
||||
public AxisBinding leftTwist = new AxisBinding("5", false);
|
||||
public AxisBinding rightX = new AxisBinding("2", true);
|
||||
public AxisBinding rightY = new AxisBinding("1", true);
|
||||
public AxisBinding rightTwist = new AxisBinding("3", false);
|
||||
[Range(0, 0.5f)] public float deadzone = 0.08f;
|
||||
[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;
|
||||
@@ -66,47 +47,59 @@ namespace HexapodSim
|
||||
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].
|
||||
// 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 readonly float[] rawAxes = new float[6]; // Input axes "1".."6" as read
|
||||
public CmdPacket LastPacket { get; private set; }
|
||||
public string Message { get; private set; } = "";
|
||||
public bool JoystickAxesMissing { 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_;
|
||||
string serialKeys_ = "";
|
||||
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;
|
||||
serialKeys_ = "";
|
||||
}
|
||||
|
||||
void Awake()
|
||||
{
|
||||
if (Input.GetJoystickNames().Length > 0 && !string.IsNullOrEmpty(Input.GetJoystickNames()[0]))
|
||||
source = InputSource.Joystick;
|
||||
}
|
||||
|
||||
void Update()
|
||||
{
|
||||
for (int i = 0; i < 6; i++) rawAxes[i] = ReadAxis((i + 1).ToString());
|
||||
// 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.RobotSerialHarness)
|
||||
if (source == InputSource.Keyboard)
|
||||
{
|
||||
serialKeys_ += Input.inputString; // raw keystrokes for the firmware's harness
|
||||
return;
|
||||
HandleToggleKeys();
|
||||
ReadKeyboardSticks(Time.deltaTime);
|
||||
}
|
||||
if (Time.time > messageUntil_) Message = "";
|
||||
}
|
||||
|
||||
HandleToggleKeys();
|
||||
if (source == InputSource.Joystick) ReadJoystick();
|
||||
else 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) ----
|
||||
@@ -144,32 +137,6 @@ namespace HexapodSim
|
||||
if (Input.GetKeyDown(KeyCode.PageDown)) height = Mathf.Max(-1000, height - 100);
|
||||
}
|
||||
|
||||
// ---- Sticks ----
|
||||
|
||||
float ReadAxis(string name)
|
||||
{
|
||||
try { return Input.GetAxis(name); }
|
||||
catch (ArgumentException) { JoystickAxesMissing = true; return 0; } // axis not defined in Input Manager
|
||||
}
|
||||
|
||||
float Read(AxisBinding b)
|
||||
{
|
||||
float v = ReadAxis(b.axis) * (b.invert ? -1 : 1);
|
||||
float mag = Mathf.Abs(v);
|
||||
if (mag <= deadzone) return 0;
|
||||
return Mathf.Sign(v) * Mathf.Clamp01((mag - deadzone) / (1 - deadzone));
|
||||
}
|
||||
|
||||
void ReadJoystick()
|
||||
{
|
||||
sticks[0] = Read(leftX);
|
||||
sticks[1] = Read(leftY);
|
||||
sticks[2] = Read(leftTwist);
|
||||
sticks[3] = Read(rightX);
|
||||
sticks[4] = Read(rightY);
|
||||
sticks[5] = Read(rightTwist);
|
||||
}
|
||||
|
||||
// WASD + Q/E = left stick (walk, turn), IJKL + U/O = right stick.
|
||||
void ReadKeyboardSticks(float dt)
|
||||
{
|
||||
@@ -187,17 +154,36 @@ namespace HexapodSim
|
||||
// Called by HexapodSimulation once per remote TX period.
|
||||
public void Transmit(HexapodSimNative native, float dt)
|
||||
{
|
||||
if (source == InputSource.RobotSerialHarness)
|
||||
{
|
||||
native.SerialInput(serialKeys_); // no radio: the firmware's harness injects its own packets
|
||||
serialKeys_ = "";
|
||||
return;
|
||||
}
|
||||
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.
|
||||
@@ -208,6 +194,7 @@ namespace HexapodSim
|
||||
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;
|
||||
|
||||
@@ -16,7 +16,7 @@ namespace HexapodSim
|
||||
Vector2 scroll_;
|
||||
GUIStyle mono_;
|
||||
|
||||
static readonly string[] SourceNames = { "Joystick", "Keyboard", "Robot serial" };
|
||||
static readonly string[] SourceNames = { "Keyboard", "Real remote (radio)" };
|
||||
|
||||
void Awake()
|
||||
{
|
||||
@@ -35,8 +35,10 @@ namespace HexapodSim
|
||||
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.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)
|
||||
@@ -65,18 +67,13 @@ namespace HexapodSim
|
||||
// ---- Input source ----
|
||||
GUILayout.Space(6);
|
||||
remote_.source = (SimRemote.InputSource)GUILayout.Toolbar((int)remote_.source, SourceNames);
|
||||
if (remote_.source == SimRemote.InputSource.RobotSerialHarness)
|
||||
if (remote_.source == SimRemote.InputSource.RealRemote)
|
||||
{
|
||||
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_);
|
||||
DongleSection();
|
||||
}
|
||||
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();
|
||||
@@ -116,11 +113,10 @@ namespace HexapodSim
|
||||
// ---- 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" +
|
||||
(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')}",
|
||||
@@ -136,6 +132,36 @@ namespace HexapodSim
|
||||
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);
|
||||
|
||||
@@ -1,10 +0,0 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 5f676c461da1c254990d7719d77e6250
|
||||
folderAsset: yes
|
||||
timeCreated: 1641405303
|
||||
licenseType: Free
|
||||
DefaultImporter:
|
||||
externalObjects: {}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -1,72 +0,0 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
using System.Text;
|
||||
using UnityEngine;
|
||||
|
||||
namespace Assets.JoyStick
|
||||
{
|
||||
class HandleHoystick : MonoBehaviour
|
||||
{
|
||||
public RotationControl bot;
|
||||
public Walk walk;
|
||||
|
||||
public void Update()
|
||||
{
|
||||
float a2 = Remap(Input.GetAxis("1"), 1, -127, -1, 127);
|
||||
float a1 = Remap(Input.GetAxis("2"), 1, -127, -1, 127);
|
||||
float a3 = Remap(Input.GetAxis("3"), -1, -127, 1, 127);
|
||||
|
||||
float b2 = Remap(Input.GetAxis("4"), -1, -127, 1, 127);
|
||||
float b3 = Remap(Input.GetAxis("5"), -1, -127, 1, 127);
|
||||
float b1 = Remap(Input.GetAxis("6"), -1, -127, 1, 127);
|
||||
|
||||
//Debug.Log(Input.GetAxis("4"));
|
||||
//Debug.Log(Remap(Input.GetAxis("4"), -1, -127, 1, 127));
|
||||
|
||||
bot.rotation = new Quaternion(limit(a2, -20, 20), -1 * limit(a3, -20, 20), -1 * limit(a1, -20, 20),
|
||||
1);
|
||||
|
||||
float speed = (float)(Math.Sqrt(b1 * b1 + b2 * b2) / 127) * 3;
|
||||
walk.walkspeed = speed < 0.5f ? 0 : speed;
|
||||
|
||||
float rotation = -1 * limit(b3 / 4, -5, 5);
|
||||
walk.centerRotation = rotation < 0.5f && rotation > -0.5f ? 0 : rotation;
|
||||
|
||||
walk.walkDirection = (-1 * ConvertRadiansToDegrees((float)Math.Atan2(b2, b1)));
|
||||
|
||||
float rotationabs = Math.Abs(walk.centerRotation);
|
||||
float speed2 = ((6 / 3) * walk.walkspeed) + ((6 / 5) * rotationabs);
|
||||
speed2 = speed2 < 6 ? speed2 : 6;
|
||||
walk.speed2 = (int)(10 - speed2);
|
||||
}
|
||||
|
||||
public float limit(float input, float min, float max)
|
||||
{
|
||||
input = input / 6;
|
||||
return (input < max & input > min) ? input : input < min ? min : max;
|
||||
}
|
||||
|
||||
public float ConvertRadiansToDegrees(float radians)
|
||||
{
|
||||
float degrees = (float)((180 / Math.PI) * radians);
|
||||
return (degrees);
|
||||
}
|
||||
|
||||
public float Remap(float from, float fromMin, float toMin, float fromMax, float toMax)
|
||||
{
|
||||
var fromAbs = from - fromMin;
|
||||
var fromMaxAbs = fromMax - fromMin;
|
||||
|
||||
var normal = fromAbs / fromMaxAbs;
|
||||
|
||||
var toMaxAbs = toMax - toMin;
|
||||
var toAbs = toMaxAbs * normal;
|
||||
|
||||
var to = toAbs + toMin;
|
||||
|
||||
return to;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
@@ -1,13 +0,0 @@
|
||||
fileFormatVersion: 2
|
||||
guid: c5ce8a2605d301e44a4a210aa345ed62
|
||||
timeCreated: 1641405303
|
||||
licenseType: Free
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -79,11 +79,8 @@ public class Leg : MonoBehaviour {
|
||||
// (int)Remap((float)a1, -90, 90, 2500, 500),
|
||||
// (int)Remap((float)a2, -90, 90, 2500, 500), // 0 is middel
|
||||
// (int)Remap((float)a3, 90, -90, 2500, 500)); // 0 is middel
|
||||
if(false)
|
||||
Serial.Instance.send(startServo,
|
||||
(int)Remap((float)a1, -90, 90, 0, 180),
|
||||
(int)Remap((float)a2, 90, -90, 0, 180), // 0 is middel
|
||||
(int)Remap((float)a3, -90, 90, 0, 180)); // 0 is middel
|
||||
// (servo angles used to go out over USB serial here; dropped 2026-11-17,
|
||||
// the 2026 robot runs its own firmware and takes commands over the radio)
|
||||
}
|
||||
else if (invertForLeftLeg && startServo != -1)
|
||||
{
|
||||
@@ -95,11 +92,6 @@ public class Leg : MonoBehaviour {
|
||||
// (int)Remap((float)a1, -90, 90, 500, 2500),
|
||||
// (int)Remap((float)a2, -90, 90, 500, 2500), // 0 is middel
|
||||
// (int)Remap((float)a3, 90, -90, 500, 2500)); // 0 is middel
|
||||
if (false)
|
||||
Serial.Instance.send(startServo,
|
||||
(int)Remap((float)a1, 90, -90, 0, 180),
|
||||
(int)Remap((float)a2, -90, 90, 0, 180), // 0 is middel
|
||||
(int)Remap((float)a3, 90, -90, 0, 180)); // 0 is middel
|
||||
}
|
||||
|
||||
Hip1.transform.localRotation = Quaternion.Euler(0, (float)a1, 0);
|
||||
|
||||
@@ -12875,7 +12875,6 @@ GameObject:
|
||||
serializedVersion: 6
|
||||
m_Component:
|
||||
- component: {fileID: 73925799}
|
||||
- component: {fileID: 73925800}
|
||||
m_Layer: 0
|
||||
m_Name: SerialIn
|
||||
m_TagString: Untagged
|
||||
@@ -12898,28 +12897,6 @@ Transform:
|
||||
m_Children: []
|
||||
m_Father: {fileID: 0}
|
||||
m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
|
||||
--- !u!114 &73925800
|
||||
MonoBehaviour:
|
||||
m_ObjectHideFlags: 0
|
||||
m_CorrespondingSourceObject: {fileID: 0}
|
||||
m_PrefabInstance: {fileID: 0}
|
||||
m_PrefabAsset: {fileID: 0}
|
||||
m_GameObject: {fileID: 73925798}
|
||||
m_Enabled: 0
|
||||
m_EditorHideFlags: 0
|
||||
m_Script: {fileID: 11500000, guid: 7fa025fa8dcb3a249b67fc0e2fe67b44, type: 3}
|
||||
m_Name:
|
||||
m_EditorClassIdentifier:
|
||||
bot: {fileID: 311513962}
|
||||
walk: {fileID: 311513965}
|
||||
a1: 0
|
||||
a2: 0
|
||||
a3: 0
|
||||
b1: 0
|
||||
b2: 0
|
||||
b3: 0
|
||||
enabled: 0
|
||||
enabled_send: 0
|
||||
--- !u!1 &74230589
|
||||
GameObject:
|
||||
m_ObjectHideFlags: 0
|
||||
@@ -325049,7 +325026,6 @@ GameObject:
|
||||
serializedVersion: 6
|
||||
m_Component:
|
||||
- component: {fileID: 1645933599}
|
||||
- component: {fileID: 1645933598}
|
||||
m_Layer: 0
|
||||
m_Name: Input
|
||||
m_TagString: Untagged
|
||||
@@ -325057,20 +325033,6 @@ GameObject:
|
||||
m_NavMeshLayer: 0
|
||||
m_StaticEditorFlags: 0
|
||||
m_IsActive: 1
|
||||
--- !u!114 &1645933598
|
||||
MonoBehaviour:
|
||||
m_ObjectHideFlags: 0
|
||||
m_CorrespondingSourceObject: {fileID: 0}
|
||||
m_PrefabInstance: {fileID: 0}
|
||||
m_PrefabAsset: {fileID: 0}
|
||||
m_GameObject: {fileID: 1645933597}
|
||||
m_Enabled: 1
|
||||
m_EditorHideFlags: 0
|
||||
m_Script: {fileID: 11500000, guid: c5ce8a2605d301e44a4a210aa345ed62, type: 3}
|
||||
m_Name:
|
||||
m_EditorClassIdentifier:
|
||||
bot: {fileID: 311513962}
|
||||
walk: {fileID: 311513965}
|
||||
--- !u!4 &1645933599
|
||||
Transform:
|
||||
m_ObjectHideFlags: 0
|
||||
|
||||
@@ -1,10 +0,0 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 942facdab943ceb4ca29453f4ee4d1b3
|
||||
folderAsset: yes
|
||||
timeCreated: 1641404792
|
||||
licenseType: Free
|
||||
DefaultImporter:
|
||||
externalObjects: {}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -1,119 +0,0 @@
|
||||
using System;
|
||||
using UnityEngine;
|
||||
using System.Collections;
|
||||
using System.IO.Ports;
|
||||
using Random = System.Random;
|
||||
|
||||
public class Serial
|
||||
{
|
||||
private static Serial instance;
|
||||
private int rnd = 0;
|
||||
|
||||
private Serial()
|
||||
{
|
||||
}
|
||||
|
||||
~Serial()
|
||||
{
|
||||
serialPort.Close();
|
||||
serialPort.Dispose();
|
||||
}
|
||||
|
||||
public static Serial Instance
|
||||
{
|
||||
get
|
||||
{
|
||||
if (instance == null)
|
||||
{
|
||||
instance = new Serial();
|
||||
}
|
||||
|
||||
return instance;
|
||||
}
|
||||
}
|
||||
|
||||
public bool enabled = false;
|
||||
|
||||
public SerialPort serialPort;
|
||||
|
||||
public int[] values = new int[18];
|
||||
|
||||
public static string ByteArrayToString(byte[] ba)
|
||||
{
|
||||
return BitConverter.ToString(ba).Replace("-", " ");
|
||||
}
|
||||
|
||||
public void send(int start, int a, int b, int c)
|
||||
{
|
||||
if (serialPort == null)
|
||||
{
|
||||
serialPort = new SerialPort(@"\\.\" + "COM4", 250000);
|
||||
|
||||
try
|
||||
{
|
||||
serialPort.Parity = Parity.Even;
|
||||
serialPort.StopBits = StopBits.One;
|
||||
serialPort.DataBits = 8;
|
||||
serialPort.Open();
|
||||
serialPort.DiscardOutBuffer();
|
||||
serialPort.DiscardInBuffer();
|
||||
System.Threading.Thread.Sleep(1000);
|
||||
|
||||
enabled = true;
|
||||
}
|
||||
catch
|
||||
{
|
||||
enabled = false;
|
||||
}
|
||||
}
|
||||
|
||||
if (enabled)
|
||||
{
|
||||
values[start] = a;
|
||||
values[++start] = b;
|
||||
values[++start] = c;
|
||||
|
||||
if (serialPort is SerialPort && start == 17)
|
||||
{
|
||||
// string send = "s";
|
||||
byte[] send = new byte[18+1+2];
|
||||
send[18] = 0x00;
|
||||
send[19] = 0xfe;
|
||||
send[20] = 0xff;
|
||||
// send[21] = 0xff;
|
||||
|
||||
// if (rnd == 0)
|
||||
// {
|
||||
//rnd = 1;
|
||||
for (int i = 0; i < 18; i++)
|
||||
{
|
||||
//string crc = ((i) * (values[i])) + "";
|
||||
//send = send + (i).ToString() + "_" + (values[i]).ToString() + "," + crc + "=";
|
||||
byte[] intBytes = BitConverter.GetBytes(values[i]);
|
||||
if (BitConverter.IsLittleEndian)
|
||||
Array.Reverse(intBytes);
|
||||
send[i] = intBytes[3];
|
||||
send[18] += send[i];
|
||||
}
|
||||
|
||||
// Debug.Log(ByteArrayToString(send));
|
||||
serialPort.Write(send, 0, 18+1+2);
|
||||
serialPort.BaseStream.Flush();
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// rnd = 0;
|
||||
// for (int i = 10; i < 18; i++)
|
||||
// {
|
||||
// string crc = ((i + 3) * (values[i])) + "";
|
||||
//
|
||||
// send = send + (i + 2).ToString() + "_" + (values[i]).ToString() + "," + crc + "=";
|
||||
// }
|
||||
//
|
||||
// Debug.Log(send);
|
||||
// serialPort.Write(send);
|
||||
// }
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,13 +0,0 @@
|
||||
fileFormatVersion: 2
|
||||
guid: c99b6d9374ff6d3419c7692fdf3c1791
|
||||
timeCreated: 1542057315
|
||||
licenseType: Free
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -1,70 +0,0 @@
|
||||
/*using System;
|
||||
using UnityEngine;
|
||||
using System.Collections;
|
||||
using System.IO.Ports;
|
||||
using System.Linq;
|
||||
|
||||
public class Serial2 : MonoBehaviour{
|
||||
|
||||
public float x;
|
||||
public float y;
|
||||
|
||||
public RotationControl bot;
|
||||
|
||||
~Serial2() {
|
||||
serialPort.Close();
|
||||
}
|
||||
|
||||
public bool enabled = false;
|
||||
|
||||
SerialPort serialPort;
|
||||
|
||||
public void Update()
|
||||
{
|
||||
if (serialPort == null)
|
||||
{
|
||||
serialPort = new SerialPort(@"\\.\" + "COM7", 115200);
|
||||
|
||||
try
|
||||
{
|
||||
serialPort.NewLine = "\n";
|
||||
|
||||
serialPort.Open();
|
||||
serialPort.DiscardOutBuffer();
|
||||
serialPort.DiscardInBuffer();
|
||||
|
||||
enabled = true;
|
||||
}
|
||||
catch
|
||||
{
|
||||
enabled = false;
|
||||
}
|
||||
}
|
||||
|
||||
if (enabled)
|
||||
{
|
||||
if (serialPort is SerialPort)
|
||||
{
|
||||
string received = serialPort.ReadLine();
|
||||
string[] data = received.Split(',');
|
||||
x = ConvertRadiansToDegrees(float.Parse(data.First()));
|
||||
y = ConvertRadiansToDegrees(float.Parse(data.Last()));
|
||||
|
||||
bot.rotation = new Quaternion(-1* limit(y, -30, 30), 0, -1* limit(x, -30, 30), 1);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public float limit(float input, float min, float max)
|
||||
{
|
||||
return (input < max & input > min) ? input : input < min ? min : max;
|
||||
}
|
||||
|
||||
public float ConvertRadiansToDegrees(float radians)
|
||||
{
|
||||
float degrees = (float) ((180 / Math.PI) * radians);
|
||||
return (degrees);
|
||||
}
|
||||
|
||||
}
|
||||
*/
|
||||
@@ -1,13 +0,0 @@
|
||||
fileFormatVersion: 2
|
||||
guid: ac6468bb4d2734947a923ea42d6d8192
|
||||
timeCreated: 1557262171
|
||||
licenseType: Free
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -1,170 +0,0 @@
|
||||
//using System;
|
||||
//using UnityEngine;
|
||||
//using System.Collections;
|
||||
//using System.IO.Ports;
|
||||
//using System.Linq;
|
||||
//using UnityEngine.UI;
|
||||
|
||||
//public class Serial3 : MonoBehaviour{
|
||||
|
||||
// public RotationControl bot;
|
||||
// public Walk walk;
|
||||
|
||||
// public float a1, a2, a3, b1, b2, b3;
|
||||
|
||||
// // ~Serial3()
|
||||
// // {
|
||||
// // serialPort.Close();
|
||||
// // serialPort.Dispose();
|
||||
// // }
|
||||
|
||||
// public bool enabled = false;
|
||||
// public bool enabled_send = false;
|
||||
|
||||
// SerialPort serialPort;
|
||||
|
||||
// public void Update()
|
||||
// {
|
||||
// if (serialPort == null)
|
||||
// {
|
||||
// serialPort = new SerialPort(@"\\.\" + "COM7", 230400);
|
||||
|
||||
// try
|
||||
// {
|
||||
// serialPort.NewLine = "\r\n";
|
||||
|
||||
//// serialPort.DataReceived += new SerialDataReceivedEventHandler(DataReceivedHandler);
|
||||
|
||||
// serialPort.Open();
|
||||
// serialPort.DiscardOutBuffer();
|
||||
// serialPort.DiscardInBuffer();
|
||||
|
||||
// enabled = true;
|
||||
// }
|
||||
// catch
|
||||
// {
|
||||
// enabled = false;
|
||||
// }
|
||||
// }
|
||||
|
||||
// if (enabled)
|
||||
// {
|
||||
// serialPort = Serial.Instance.serialPort;
|
||||
// Serial.Instance.enabled = enabled_send;
|
||||
// if (serialPort is SerialPort)
|
||||
// {
|
||||
// string received = serialPort.ReadLine();
|
||||
// string[] data = received.Split(',');
|
||||
|
||||
// foreach (string d in data)
|
||||
// {
|
||||
|
||||
// string[] ch = d.Split('_');
|
||||
|
||||
// int channel = int.Parse(ch.First());
|
||||
// float value = float.Parse(ch.Last());
|
||||
|
||||
// switch (channel)
|
||||
// {
|
||||
// case 0:
|
||||
// a1 = value - 127;
|
||||
// break;
|
||||
// case 1:
|
||||
// a2 = value - 127;
|
||||
// break;
|
||||
// case 2:
|
||||
// a3 = value - 127;
|
||||
// break;
|
||||
// case 3:
|
||||
// b1 = value - 127;
|
||||
// break;
|
||||
// case 4:
|
||||
// b2 = value - 127;
|
||||
// break;
|
||||
// case 5:
|
||||
// b3 = value - 127;
|
||||
// break;
|
||||
// }
|
||||
|
||||
// bot.rotation = new Quaternion(limit(a2, -20, 20), -1 * limit(a3, -20, 20), -1 * limit(a1, -20, 20),
|
||||
// 1);
|
||||
|
||||
// float speed = (float) (Math.Sqrt(b1 * b1 + b2 * b2) / 127) * 3;
|
||||
// walk.walkspeed = speed < 0.5f ? 0 : speed;
|
||||
|
||||
// float rotation = -1 * limit(b3/4, -5, 5);
|
||||
// walk.centerRotation = rotation < 0.5f && rotation > -0.5f ? 0 : rotation;
|
||||
|
||||
// walk.walkDirection = (-1 * ConvertRadiansToDegrees((float) Math.Atan2(b2, b1)));
|
||||
|
||||
// float rotationabs = Math.Abs(walk.centerRotation);
|
||||
// float speed2 = ((6 / 3) * walk.walkspeed) + ((6/5) * rotationabs);
|
||||
// speed2 = speed2 < 6 ? speed2 : 6;
|
||||
// walk.speed2 = (int)(10 - speed2);
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
|
||||
// private void DataReceivedHandler(object sender, SerialDataReceivedEventArgs e)
|
||||
// {
|
||||
// SerialPort serialPort = (SerialPort)sender;
|
||||
|
||||
// if (enabled)
|
||||
// {
|
||||
// if (serialPort is SerialPort)
|
||||
// {
|
||||
// string received = serialPort.ReadLine();
|
||||
// string[] data = received.Split(',');
|
||||
|
||||
// foreach (string d in data)
|
||||
// {
|
||||
// // if (data == "\r")
|
||||
// // continue;
|
||||
|
||||
// string[] ch = d.Split('_');
|
||||
|
||||
// int channel = int.Parse(ch.First());
|
||||
// float value = float.Parse(ch.Last());
|
||||
|
||||
// switch (channel)
|
||||
// {
|
||||
// case 0:
|
||||
// a1 = value;
|
||||
// break;
|
||||
// case 1:
|
||||
// a2 = value;
|
||||
// break;
|
||||
// case 2:
|
||||
// a3 = value;
|
||||
// break;
|
||||
// case 3:
|
||||
// b1 = value;
|
||||
// break;
|
||||
// case 4:
|
||||
// b2 = value;
|
||||
// break;
|
||||
// case 5:
|
||||
// b3 = value;
|
||||
// break;
|
||||
// }
|
||||
// }
|
||||
|
||||
// // bot.rotation = new Quaternion(-1* limit(y, -30, 30), 0, -1* limit(x, -30, 30), 1);
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
|
||||
// public float limit(float input, float min, float max)
|
||||
// {
|
||||
// input = input / 6;
|
||||
// return (input < max & input > min) ? input : input < min ? min : max;
|
||||
// }
|
||||
|
||||
// public float ConvertRadiansToDegrees(float radians)
|
||||
// {
|
||||
// float degrees = (float)((180 / Math.PI) * radians);
|
||||
// return (degrees);
|
||||
// }
|
||||
|
||||
//}
|
||||
@@ -1,13 +0,0 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 7fa025fa8dcb3a249b67fc0e2fe67b44
|
||||
timeCreated: 1558207463
|
||||
licenseType: Free
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -155,7 +155,9 @@ python tools/safeboot_flash.py firmware/build/app.bin
|
||||
```
|
||||
|
||||
This resets the running app into safeboot and rewrites only the app region. If
|
||||
the app is dead, replug the stick and re-run — it will catch the knock window.
|
||||
the app is dead or doesn't answer, start it with `--knock` and then replug the
|
||||
stick: it keeps knocking (reopening the port as it comes back) until it lands in
|
||||
safeboot's 1.5 s cold-boot window.
|
||||
|
||||
## Air settings
|
||||
|
||||
@@ -168,6 +170,36 @@ the Si24R1 over SPI (same register layout as the nRF24L01+).
|
||||
- [x] identify the hardware, map the radio + serial pins
|
||||
- [x] permanent two-stage firmware, wire-free USB flashing
|
||||
- [ ] identify the LED and AT2401C TX/RX-enable pins (app toggles the 5 free pins)
|
||||
- [ ] radio hub app: carry `CmdPacket` both ways, per-node addressing, telemetry
|
||||
- [ ] Unity serial bridge (PC as hub when online, robot as hub when not) — see
|
||||
`../README.md` "Planned"
|
||||
- [x] bridge mode: the dongle stands in for the robot's radio, Unity runs the
|
||||
robot firmware (sim) behind it — see "Bridge mode" below
|
||||
- [ ] the other direction: Unity as the remote, driving the real robot
|
||||
- [ ] AT2401C pins: at desk range and even a few rooms away with the remote at
|
||||
minimum power, every on/off combination of PA1/PB1/PF0/PF1 received
|
||||
every packet, so they couldn't be told apart; the link works without
|
||||
driving them
|
||||
|
||||
## Bridge mode (Unity ↔ real remote)
|
||||
|
||||
The app listens on the hexapod's air settings as if it were the robot
|
||||
(ch 96, 250 kbps, `HXPD1`, auto-ack, dynamic payload lengths, ack payloads)
|
||||
and passes everything to the PC. Framing both ways over the USB serial
|
||||
(115200 8N1):
|
||||
|
||||
```
|
||||
A5 type len payload[len] sum sum = type + len + payload bytes (mod 256)
|
||||
```
|
||||
|
||||
| Direction | Type | Payload |
|
||||
|-----------|------|---------|
|
||||
| PC → dongle | `L` | — : start bridging (LED on) |
|
||||
| PC → dongle | `X` | — : stop |
|
||||
| PC → dongle | `A` | ≤ 32 bytes: the robot's `Telemetry`, sent back on the auto-ack of the remote's next packet (reloaded after every packet, like `robot/Comms.cpp`). Dropped if not refreshed for ~0.5 s, so the remote never shows stale telemetry as live |
|
||||
| dongle → PC | `R` | a packet the remote sent (its `CmdPacket`, any length; the firmware checks it) |
|
||||
| dongle → PC | `S` | once a "second": packets received (u16 LE), ack payloads loaded (u16 LE). Its second runs ~30% long (blocking UART sends miss SysTick ticks); it's only for display |
|
||||
|
||||
Plain text never contains `A5`, so the PC skips anything between frames;
|
||||
while bridging the app prints no text and only takes frames (and `b` for a
|
||||
reflash). USB serial input goes through DMA into a ring buffer (no
|
||||
interrupts in this app), and the USART's overrun stall is disabled, so a
|
||||
burst can't deafen it. Unity's side is
|
||||
`Hexapod_Simulation_Unity/Assets/HexapodSim/DongleLink.cs`.
|
||||
@@ -52,11 +52,14 @@ static void nrf_write_buf(uint8_t reg, const uint8_t *d, int n)
|
||||
}
|
||||
static void nrf_cmd(uint8_t c) { CSN_LO(); spi_xfer(c); CSN_HI(); }
|
||||
|
||||
/* Receive on the hexapod protocol (ch 96, 250 kbps, addr HXPD1, 22-byte). Lets
|
||||
* us find the AT2401C RX-enable pin: packets only arrive when the LNA/antenna
|
||||
* path is on. */
|
||||
/* Receive on the hexapod protocol (ch 96, 250 kbps, addr HXPD1). Protocol v4
|
||||
* uses dynamic payload lengths (the remote's CmdPacket is 26 bytes, with ack
|
||||
* payloads), so this does too and accepts any length. Auto-ack is on, so the
|
||||
* remote's link % also shows whether our acks get back out (TX path). Lets us
|
||||
* find the AT2401C enable pins: packets only arrive when the LNA path is on. */
|
||||
static uint8_t rxmode;
|
||||
static uint16_t rx_count;
|
||||
static uint8_t rx_len;
|
||||
static void rx_setup(void)
|
||||
{
|
||||
static const uint8_t addr[5] = { 0x48, 0x58, 0x50, 0x44, 0x31 }; /* "HXPD1" */
|
||||
@@ -68,18 +71,22 @@ static void rx_setup(void)
|
||||
nrf_write(0x05, 96); /* RF_CH */
|
||||
nrf_write(0x06, 0x26); /* RF_SETUP: 250 kbps, max power */
|
||||
nrf_write_buf(0x0A, addr, 5); /* RX_ADDR_P0 */
|
||||
nrf_write(0x11, 22); /* RX_PW_P0 */
|
||||
nrf_write(0x1D, 0x04); /* FEATURE: EN_DPL */
|
||||
nrf_write(0x1C, 0x01); /* DYNPD: pipe0 */
|
||||
nrf_write(0x07, 0x70); /* clear status flags */
|
||||
nrf_cmd(0xE2); /* FLUSH_RX */
|
||||
rx_count = 0;
|
||||
rx_len = 0;
|
||||
CE_HI();
|
||||
}
|
||||
static void rx_poll(void)
|
||||
{
|
||||
if (nrf_read(0x07) & 0x40) { /* RX_DR */
|
||||
uint8_t buf[22];
|
||||
CSN_LO(); spi_xfer(0x61); for (int i = 0; i < 22; i++) buf[i] = spi_xfer(0xFF); CSN_HI();
|
||||
while (nrf_read(0x07) & 0x40) { /* RX_DR */
|
||||
CSN_LO(); spi_xfer(0x60); uint8_t n = spi_xfer(0xFF); CSN_HI(); /* R_RX_PL_WID */
|
||||
if (n > 32) { nrf_cmd(0xE2); nrf_write(0x07, 0x40); break; } /* corrupt: flush */
|
||||
CSN_LO(); spi_xfer(0x61); for (int i = 0; i < n; i++) spi_xfer(0xFF); CSN_HI();
|
||||
nrf_write(0x07, 0x40); /* clear RX_DR */
|
||||
rx_len = n;
|
||||
rx_count++;
|
||||
}
|
||||
}
|
||||
@@ -132,7 +139,7 @@ static void banner(void)
|
||||
uart_puts("\r\nhexapod dongle app v0 (@0x08001000)\r\n");
|
||||
uart_puts("nRF CONFIG=0x"); hex2(nrf_read(0)); uart_puts(" STATUS=0x"); hex2(nrf_read(7));
|
||||
uart_puts(" RF_CH=0x"); hex2(nrf_read(5)); uart_puts(" RF_SETUP=0x"); hex2(nrf_read(6));
|
||||
uart_puts("\r\ncmds: v r 0-4(PA0/PA1/PB1/PF0/PF1) b=reflash\r\n");
|
||||
uart_puts("\r\ncmds: v r 0-4(PA0/PA1/PB1/PF0/PF1) x=rx test b=reflash; bridge: frame A5 'L'\r\n");
|
||||
}
|
||||
static void regdump(void)
|
||||
{
|
||||
@@ -156,6 +163,134 @@ static void finder_rx(void)
|
||||
delay_ms(2);
|
||||
}
|
||||
|
||||
/* ---- USART1 RX through circular DMA ------------------------------------
|
||||
* The bridge moves ~100 frames/s each way at 115200 baud. Polled single-byte
|
||||
* RX loses bytes while we block on a TX frame or an SPI read, and this M0 app
|
||||
* runs without interrupts (no vector relocation), so DMA channel 3 (USART1_RX)
|
||||
* writes every byte into a ring we drain at leisure. */
|
||||
#define DMA_CCR3 REG(0x40020030)
|
||||
#define DMA_CNDTR3 REG(0x40020034)
|
||||
#define DMA_CPAR3 REG(0x40020038)
|
||||
#define DMA_CMAR3 REG(0x4002003C)
|
||||
#define USART1_CR3 REG(0x40013808)
|
||||
#define RXRING 256u
|
||||
static volatile uint8_t rxring[RXRING];
|
||||
static uint16_t rxtail;
|
||||
static void uart_dma_init(void)
|
||||
{
|
||||
RCC_AHBENR |= 1u; /* DMA1 */
|
||||
while (!(USART1_ISR & (1u << 6))) {} /* let any TX finish */
|
||||
USART1_CR1 &= ~1u; /* UE off: CR3 OVRDIS needs it */
|
||||
USART1_CR3 = (1u << 6) | (1u << 12); /* DMAR, OVRDIS (never stall on overrun) */
|
||||
USART1_CR1 |= 1u;
|
||||
DMA_CCR3 = 0;
|
||||
DMA_CPAR3 = 0x40013824u; /* USART1_RDR */
|
||||
DMA_CMAR3 = (uint32_t)rxring;
|
||||
DMA_CNDTR3 = RXRING;
|
||||
DMA_CCR3 = (1u << 7) | (1u << 5) | 1u; /* MINC, CIRC, EN; 8-bit, periph -> mem */
|
||||
}
|
||||
static int rx_getc(void)
|
||||
{
|
||||
uint16_t head = (uint16_t)(RXRING - DMA_CNDTR3) % RXRING;
|
||||
if (head == rxtail) return -1;
|
||||
uint8_t c = rxring[rxtail];
|
||||
rxtail = (uint16_t)((rxtail + 1) % RXRING);
|
||||
return c;
|
||||
}
|
||||
|
||||
/* ---- Bridge: the dongle stands in for the robot's radio ------------------
|
||||
* Framing both ways: A5 type len payload[len] sum, sum = type+len+payload (mod 256).
|
||||
* host -> dongle 'L' listen (enter bridge) 'X' leave
|
||||
* 'A' ack payload (<= 32 B): goes back on the auto-ack of the
|
||||
* next packet, like the robot's Comms::loadAckPayload.
|
||||
* Dropped if the host stops refreshing it for 500 ms, so
|
||||
* the remote doesn't see stale telemetry as live.
|
||||
* dongle -> host 'R' a received packet (the remote's CmdPacket)
|
||||
* 'S' once a second: packets received (u16 LE), acks loaded (u16 LE)
|
||||
* Plain text never contains A5, so the host can skip anything between frames. */
|
||||
static uint8_t bridge;
|
||||
static uint8_t ack_buf[32], ack_len;
|
||||
static uint32_t ack_ms;
|
||||
static uint16_t br_rx, br_acks;
|
||||
static void frame_send(uint8_t type, const uint8_t *d, uint8_t n)
|
||||
{
|
||||
uint8_t sum = (uint8_t)(type + n);
|
||||
uart_putc((char)0xA5); uart_putc((char)type); uart_putc((char)n);
|
||||
for (uint8_t i = 0; i < n; i++) { uart_putc((char)d[i]); sum = (uint8_t)(sum + d[i]); }
|
||||
uart_putc((char)sum);
|
||||
}
|
||||
static void ack_load(void)
|
||||
{
|
||||
nrf_cmd(0xE1); /* FLUSH_TX: only the newest one */
|
||||
if (!ack_len) return;
|
||||
CSN_LO(); spi_xfer(0xA8); for (uint8_t i = 0; i < ack_len; i++) spi_xfer(ack_buf[i]); CSN_HI(); /* W_ACK_PAYLOAD pipe 0 */
|
||||
br_acks++;
|
||||
}
|
||||
static void bridge_start(void)
|
||||
{
|
||||
rx_setup(); /* ch96 / 250k / HXPD1 / auto-ack / dynamic length */
|
||||
CE_LO();
|
||||
nrf_write(0x1D, 0x06); /* FEATURE: EN_DPL | EN_ACK_PAY */
|
||||
nrf_write(0x1C, 0x01); /* DYNPD pipe 0 */
|
||||
nrf_cmd(0xE1);
|
||||
ack_len = 0; br_rx = br_acks = 0;
|
||||
rxmode = 0; finder = 0; bridge = 1;
|
||||
BSRR(GPIOA) = 1u; /* LED on while bridging */
|
||||
CE_HI();
|
||||
}
|
||||
static void bridge_stop(void)
|
||||
{
|
||||
bridge = 0; ack_len = 0;
|
||||
CE_LO(); nrf_write(0, 0x0E); nrf_cmd(0xE1);
|
||||
BSRR(GPIOA) = 1u << 16;
|
||||
}
|
||||
static void bridge_poll(uint32_t now)
|
||||
{
|
||||
if (ack_len && now - ack_ms > 500) { ack_len = 0; nrf_cmd(0xE1); } /* host went quiet */
|
||||
while (nrf_read(0x07) & 0x40) { /* RX_DR */
|
||||
uint8_t buf[32];
|
||||
CSN_LO(); spi_xfer(0x60); uint8_t n = spi_xfer(0xFF); CSN_HI(); /* R_RX_PL_WID */
|
||||
if (n > 32) { nrf_cmd(0xE2); nrf_write(0x07, 0x40); break; }
|
||||
CSN_LO(); spi_xfer(0x61); for (uint8_t i = 0; i < n; i++) buf[i] = spi_xfer(0xFF); CSN_HI();
|
||||
nrf_write(0x07, 0x40);
|
||||
ack_load(); /* the ack for the NEXT packet */
|
||||
frame_send('R', buf, n);
|
||||
br_rx++;
|
||||
if ((br_rx & 31) == 0) BSRR(GPIOA) = (IDR(GPIOA) & 1u) ? (1u << 16) : 1u; /* LED flicker */
|
||||
}
|
||||
}
|
||||
/* Host frame parser; returns 1 while inside a frame (so its bytes aren't
|
||||
* taken as single-letter commands). */
|
||||
static uint8_t fr_state, fr_type, fr_len, fr_got, fr_sum, fr_buf[32];
|
||||
static int frame_rx(int c, uint32_t now)
|
||||
{
|
||||
/* if-chain, not switch: a switch becomes a jump table needing libgcc */
|
||||
if (fr_state == 0) {
|
||||
if (c != 0xA5) return 0;
|
||||
fr_state = 1;
|
||||
} else if (fr_state == 1) {
|
||||
fr_type = (uint8_t)c;
|
||||
fr_state = 2;
|
||||
} else if (fr_state == 2) {
|
||||
fr_len = (uint8_t)c; fr_got = 0; fr_sum = (uint8_t)(fr_type + fr_len);
|
||||
fr_state = (fr_len > 32) ? 0 : (fr_len ? 3 : 4);
|
||||
} else if (fr_state == 3) {
|
||||
fr_buf[fr_got++] = (uint8_t)c; fr_sum = (uint8_t)(fr_sum + c);
|
||||
if (fr_got == fr_len) fr_state = 4;
|
||||
} else {
|
||||
fr_state = 0;
|
||||
if ((uint8_t)c != fr_sum) return 1; /* corrupt: drop */
|
||||
if (fr_type == 'L') bridge_start();
|
||||
else if (fr_type == 'X') bridge_stop();
|
||||
else if (fr_type == 'A' && bridge) {
|
||||
for (uint8_t i = 0; i < fr_len; i++) ack_buf[i] = fr_buf[i];
|
||||
ack_len = fr_len; ack_ms = now;
|
||||
ack_load();
|
||||
}
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
static void reflash(void) /* ask safeboot to take over, no wire needed */
|
||||
{
|
||||
uart_puts("resetting into safeboot flash mode...\r\n");
|
||||
@@ -168,16 +303,25 @@ static void reflash(void) /* ask safeboot to take over, no wire needed */
|
||||
int main(void)
|
||||
{
|
||||
uart_init8n1();
|
||||
uart_dma_init();
|
||||
spi_init();
|
||||
free_init();
|
||||
nrf_write(0, 0x0E);
|
||||
delay_ms(5);
|
||||
banner();
|
||||
|
||||
uint32_t t = 0;
|
||||
uint32_t now = 0, t = 0;
|
||||
systick_setup();
|
||||
for (;;) {
|
||||
int c = uart_getc();
|
||||
if (c == 'v' || c == 'V') banner();
|
||||
uint32_t ctrl = STK_CTRL; /* one read: reading clears COUNTFLAG */
|
||||
if (ctrl == 0) systick_setup(); /* delay_ms() (sweep, finder) stops it */
|
||||
else if (ctrl & 0x10000) { now++; t++; } /* 1 ms ticks, polled */
|
||||
if (bridge) bridge_poll(now);
|
||||
int c = rx_getc();
|
||||
if (c >= 0 && frame_rx(c, now)) c = -1; /* consumed by a host frame */
|
||||
if (c < 0) {}
|
||||
else if (bridge && c != 'b' && c != 'B') {} /* bridging: only frames (and reflash) */
|
||||
else if (c == 'v' || c == 'V') banner();
|
||||
else if (c == 'r' || c == 'R') regdump();
|
||||
else if (c == 'b' || c == 'B') reflash();
|
||||
else if (c == 'x' || c == 'X') { rxmode = !rxmode; finder = 0; if (rxmode) rx_setup(); else { CE_LO(); nrf_write(0, 0x0E); } uart_puts(rxmode ? "RX mode on (ch96/250k/HXPD1)\r\n" : "RX off\r\n"); }
|
||||
@@ -193,10 +337,15 @@ int main(void)
|
||||
uart_puts("\r\n");
|
||||
}
|
||||
if (rxmode) rx_poll();
|
||||
delay_ms(1);
|
||||
if (++t >= 1000) {
|
||||
if (t >= 1000) {
|
||||
t = 0;
|
||||
if (rxmode) { uart_puts("RX pkts="); hex2(rx_count >> 8); hex2(rx_count & 0xFF);
|
||||
if (bridge) {
|
||||
uint8_t s[4] = { (uint8_t)br_rx, (uint8_t)(br_rx >> 8), (uint8_t)br_acks, (uint8_t)(br_acks >> 8) };
|
||||
frame_send('S', s, 4);
|
||||
br_rx = br_acks = 0;
|
||||
}
|
||||
else if (rxmode) { uart_puts("RX pkts="); hex2(rx_count >> 8); hex2(rx_count & 0xFF);
|
||||
uart_puts(" len="); hex2(rx_len);
|
||||
uart_puts(" pins="); for (int i = 0; i < NFREE; i++) uart_putc(fst[i] ? '1' : '0');
|
||||
uart_puts(" (0=PA0LED,1=PA1,2=PB1,3=PF0,4=PF1)\r\n"); rx_count = 0; }
|
||||
else if (finder) { uart_puts("finder ch=0x"); hex2(finder_ch); uart_puts(" RPD="); hex2(nrf_read(9));
|
||||
|
||||
@@ -36,6 +36,7 @@
|
||||
#define USART1_CR1 REG(0x40013800)
|
||||
#define USART1_BRR REG(0x4001380C)
|
||||
#define USART1_ISR REG(0x4001381C)
|
||||
#define USART1_ICR REG(0x40013820)
|
||||
#define USART1_RDR REG(0x40013824)
|
||||
#define USART1_TDR REG(0x40013828)
|
||||
/* SysTick */
|
||||
@@ -65,14 +66,22 @@ static inline void uart_putc(char c)
|
||||
USART1_TDR = (uint8_t)c;
|
||||
}
|
||||
static inline void uart_puts(const char *s) { while (*s) uart_putc(*s++); }
|
||||
/* Clear overrun / noise / framing / parity errors. While ORE is set the USART
|
||||
* stops moving new bytes into RDR, so one burst the app didn't read in time
|
||||
* (e.g. SignalRGB probing the COM port) used to deafen it until a replug. */
|
||||
static inline void uart_clear_errors(void)
|
||||
{
|
||||
if (USART1_ISR & 0x0Fu) USART1_ICR = 0x0Fu; /* ORECF NCF FECF PECF */
|
||||
}
|
||||
static inline int uart_getc(void) /* non-blocking, -1 if none */
|
||||
{
|
||||
uart_clear_errors();
|
||||
if (USART1_ISR & (1u << 5)) return (uint8_t)USART1_RDR;
|
||||
return -1;
|
||||
}
|
||||
static inline uint8_t uart_getc_block(void)
|
||||
{
|
||||
while (!(USART1_ISR & (1u << 5))) {}
|
||||
while (!(USART1_ISR & (1u << 5))) uart_clear_errors();
|
||||
return (uint8_t)USART1_RDR;
|
||||
}
|
||||
static inline void systick_setup(void) { STK_LOAD = 8000 - 1; STK_VAL = 0; STK_CTRL = 5; }
|
||||
|
||||
@@ -2,12 +2,14 @@
|
||||
No BOOT0 wire, no ST-Link, no ROM bootloader. Writes only the app region
|
||||
(pages 4-15); safeboot itself is never touched.
|
||||
|
||||
usage: safeboot_flash.py app.bin [--port COM16]
|
||||
usage: safeboot_flash.py app.bin [--port COM16] [--knock]
|
||||
|
||||
How it enters flash mode:
|
||||
1) if the app is running, 'b' makes it soft-reset into safeboot flash mode;
|
||||
2) otherwise (dead/absent app) replug the dongle and this spams 'S' to catch
|
||||
safeboot's cold-boot knock window.
|
||||
--knock skips 1) and keeps knocking for 2 minutes, reopening the port as it
|
||||
comes and goes: start it, then replug. Use it when the app doesn't answer.
|
||||
"""
|
||||
import sys, time, serial
|
||||
|
||||
@@ -54,6 +56,36 @@ def enter_flash(s):
|
||||
return False
|
||||
|
||||
|
||||
def knock_until_caught(port, timeout=120.0):
|
||||
"""--knock: for an app that doesn't answer 'b' (dead or deaf). Keeps the
|
||||
port open and spams 'S' for up to `timeout` s, reopening whenever the port
|
||||
vanishes, so replugging the dongle at any moment lands in safeboot's 1.5 s
|
||||
cold-boot knock window. Returns the open port in flash mode, or None."""
|
||||
t = time.time()
|
||||
s = None
|
||||
while time.time() - t < timeout:
|
||||
try:
|
||||
if s is None:
|
||||
s = serial.Serial(port, 115200, timeout=0.02)
|
||||
s.write(b"S")
|
||||
r = s.read(16)
|
||||
if b"K" in r or b"SB1" in r or b"SBR" in r:
|
||||
time.sleep(0.1); s.reset_input_buffer(); s.timeout = 0.3
|
||||
s.write(b"V"); time.sleep(0.1)
|
||||
if b"SBV" in s.read(16):
|
||||
return s
|
||||
s.timeout = 0.02
|
||||
except (serial.SerialException, OSError):
|
||||
try:
|
||||
if s is not None:
|
||||
s.close()
|
||||
except Exception:
|
||||
pass
|
||||
s = None
|
||||
time.sleep(0.02)
|
||||
return None
|
||||
|
||||
|
||||
def expect(s, ch, what):
|
||||
r = s.read(1)
|
||||
if r != ch:
|
||||
@@ -68,10 +100,17 @@ def main():
|
||||
data += b"\xFF"
|
||||
print("app %s: %d bytes -> 0x%08X" % (args[0], len(data), APP_BASE))
|
||||
|
||||
if "--knock" in sys.argv:
|
||||
print("knock mode: replug the dongle now (waiting up to 2 min)...")
|
||||
s = knock_until_caught(port)
|
||||
if s is None:
|
||||
print("no knock window caught")
|
||||
return 1
|
||||
else:
|
||||
s = open_port(port)
|
||||
print("entering safeboot flash mode...")
|
||||
if not enter_flash(s):
|
||||
print("could not enter flash mode (app not running? replug and retry)")
|
||||
print("could not enter flash mode (app not running? replug and retry with --knock)")
|
||||
return 1
|
||||
s.reset_input_buffer()
|
||||
s.write(b"V"); time.sleep(0.1)
|
||||
|
||||
+23
-16
@@ -71,25 +71,31 @@ It drives the firmware the way the remote would and checks:
|
||||
|
||||
Open `Hexapod_Simulation_Unity` and press Play in `Main.unity`. No scene
|
||||
edits are needed: `HexapodSimBootstrap` adds a `HexapodSimulation` object,
|
||||
disables the old movement scripts (`Walk`, `RotationControl`, `Leg`,
|
||||
`HandleHoystick`), and binds the existing leg rig by name (`Leg` → `hip1`
|
||||
/ `Hip2` / `Knie`). The on-screen panel (F1 toggles it) offers three input
|
||||
sources:
|
||||
disables the old movement scripts (`Walk`, `RotationControl`, `Leg`), and
|
||||
binds the existing leg rig by name (`Leg` → `hip1` / `Hip2` / `Knie`). The
|
||||
on-screen panel (F1 toggles it) offers two input sources:
|
||||
|
||||
- **Joystick:** the real remote as a USB controller, on input axes
|
||||
`"1"`–`"6"` like the old `HandleHoystick`. Axis assignment and inversion
|
||||
are set on the `SimRemote` component. The panel shows the raw axes so you
|
||||
can fix them.
|
||||
- **Keyboard:** test without a remote. WASD walks, Q/E turns, IJKL + U/O
|
||||
drive the right stick, 1–7 flip the toggles, Backspace disarms, +/-
|
||||
change speed.
|
||||
- **Robot serial:** no radio at all. Keystrokes go to `robot.ino`'s own
|
||||
`TEST_SERIAL_INPUT` harness.
|
||||
change speed. The panel has every remote toggle, the speed slider and
|
||||
simulated handset tilt (remote gyro).
|
||||
- **Real remote (radio):** the real handheld remote, over the air. The USB
|
||||
nRF24 dongle (`../dongle/`, bridge mode) receives its packets, they go
|
||||
into the firmware byte for byte, and the robot's `Telemetry` goes back as
|
||||
the radio's ack payload, exactly like the real robot's `Comms`. The
|
||||
remote's display then shows the simulated robot (state, IMU, joint
|
||||
angles, servo calibration). `DongleLink.cs` finds the dongle's CH340 port
|
||||
by itself (or set `SimRemote.donglePort`, e.g. `COM7`); quit SignalRGB
|
||||
first, it grabs CH340 ports. Don't run the real robot at the same time:
|
||||
both would answer the remote.
|
||||
|
||||
The panel also has every remote toggle, the speed slider, simulated
|
||||
handset tilt (remote gyro), a **ground tilt** slider (to test the robot
|
||||
gyro), and live firmware state, joint angles, foot contact, the packet on
|
||||
air and robot serial output.
|
||||
There's no USB link to the remote or the robot any more (the old joystick
|
||||
and serial-harness inputs and the `Serial*` / `HandleHoystick` scripts are
|
||||
gone): everything goes over the radio.
|
||||
|
||||
The panel also has a **ground tilt** slider (to test the robot gyro), and
|
||||
live firmware state, joint angles, foot contact, the packet on air and the
|
||||
robot's debug prints.
|
||||
|
||||
The body moves **kinematically**, computed in the DLL (`SimBody.cpp`), so
|
||||
Unity shows what the smoke test checks:
|
||||
@@ -119,4 +125,5 @@ component to the scene yourself.
|
||||
firmware *intends*, including pulse-limit clamping (which is how it
|
||||
caught the old ±90° coxa offsets). Whether a real servo turns that way is
|
||||
a bench check.
|
||||
- The remote firmware. `SimRemote.cs` mirrors its packet rules instead.
|
||||
- The remote firmware, in Keyboard mode: `SimRemote.cs` mirrors its packet
|
||||
rules instead. With the real remote over the radio, its own firmware runs.
|
||||
Reference in new issue
Block a user