Firmware simulation, Unity project, and sim-verified robot fixes
Simulation - sim/: the unchanged robot firmware compiled into hexapod_sim.dll against PC stand-ins (Arduino, Servo, Wire + register-level MPU6050, RF24 with matching-config radio), a kinematic body model (support plane + planted-foot odometry) that also drives the simulated IMU, and a headless smoke test (sim_smoke). build.ps1 builds and tests. - Hexapod_Simulation_Unity/ folded in (history kept in a bundle outside the repo). Assets/HexapodSim loads the DLL, drives the existing leg rig, and offers joystick / keyboard / robot-serial input plus a test panel. Robot (all found or confirmed in the simulation) - Config.h geometry taken from the Unity model. - IK femur elevation sign, gait stance direction, body pose rotation direction, yaw lever arm. - Stance feet integrated from the current command; gait restarts at mid-stance; heading integrates the applied turn rate, only while walking. - Leg clearance and reach checks (Limits::feetFeasible); walk command and body pose scaled as a whole, pose scale rate-limited, planted feet guarded so the body never snaps or legs collide. - Robot-gyro self-leveling as an integrator. - Heading mode: steering relative to the body; S walks backward. - Standing height from the remote (STANCE_HEIGHT_RANGE_MM). Protocol v3 (reflash both boards) - CmdPacket gains body_height (22 bytes). Remote - Height slider (Nextion hHeight/nHeight, serial [ ]). - Fixed unspecified I2C read order in RemoteIMU. LEGACY/: owner's old Nextion HMI and remote hardware assets. Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
This commit is contained in:
mangavalkandClaude Opus 5.5 committed
1 parent
91dac16fca
commit
935526eab5
145 files changed
+431258
-114
No files matched your search
@@ -0,0 +1,18 @@
|
||||
using UnityEngine;
|
||||
|
||||
namespace HexapodSim
|
||||
{
|
||||
// Adds the firmware simulation to whatever scene is playing, so Main.unity
|
||||
// works without editing the scene. To configure it in the Inspector
|
||||
// instead, add a HexapodSimulation component to any GameObject yourself;
|
||||
// the bootstrap then does nothing.
|
||||
static class HexapodSimBootstrap
|
||||
{
|
||||
[RuntimeInitializeOnLoadMethod(RuntimeInitializeLoadType.AfterSceneLoad)]
|
||||
static void AddSimulationIfMissing()
|
||||
{
|
||||
if (Object.FindAnyObjectByType<HexapodSimulation>() != null) return;
|
||||
new GameObject("HexapodSimulation").AddComponent<HexapodSimulation>();
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,2 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 0c650988c59fd78469143f99ff54f18d
|
||||
@@ -0,0 +1,232 @@
|
||||
using System;
|
||||
using System.IO;
|
||||
using System.Runtime.InteropServices;
|
||||
using UnityEngine;
|
||||
|
||||
namespace HexapodSim
|
||||
{
|
||||
// Wire format, byte-for-byte identical to shared/HexapodProtocol/HexapodProtocol.h.
|
||||
// Checked against the DLL's sizeof(CmdPacket) at load.
|
||||
[StructLayout(LayoutKind.Sequential, Pack = 2)]
|
||||
public struct CmdPacket
|
||||
{
|
||||
public short vx, vy, yaw_rate;
|
||||
public short rx, ry, rz;
|
||||
public short remote_roll, remote_pitch;
|
||||
public short body_height; // -1000 lowest .. 0 default .. 1000 highest
|
||||
public byte speed_scale, flags, seq;
|
||||
|
||||
public const byte FLAG_ARMED = 1 << 0;
|
||||
public const byte FLAG_STAND = 1 << 1;
|
||||
public const byte FLAG_WALK = 1 << 2;
|
||||
public const byte FLAG_MODE_B = 1 << 3;
|
||||
public const byte FLAG_RSTICK_XLATE = 1 << 4;
|
||||
public const byte FLAG_GAIT_WAVE = 1 << 5;
|
||||
public const byte FLAG_REMOTE_GYRO = 1 << 6;
|
||||
public const byte FLAG_ROBOT_GYRO = 1 << 7;
|
||||
}
|
||||
|
||||
// Mirrors HxGeometry in sim/HexapodSimApi.h.
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct HxGeometry
|
||||
{
|
||||
public int numLegs;
|
||||
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 6)] public float[] mountXmm;
|
||||
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 6)] public float[] mountYmm;
|
||||
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 6)] public float[] mountAngleDeg;
|
||||
public float coxaMm, femurMm, tibiaMm;
|
||||
public float stanceRadiusMm, stanceHeightMm;
|
||||
public float coxaMinDeg, coxaMaxDeg, femurMinDeg, femurMaxDeg, tibiaMinDeg, tibiaMaxDeg;
|
||||
public float controlHz;
|
||||
public int servoUsAt0Deg, servoUsPerDeg;
|
||||
public float legClearanceMm;
|
||||
public float stanceHeightRangeMm;
|
||||
}
|
||||
|
||||
// Mirrors HxStatus in sim/HexapodSimApi.h.
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct HxStatus
|
||||
{
|
||||
public int robotState; // 0 DISARMED, 1 SIT, 2 STAND, 3 WALKING
|
||||
public float sitStandBlend;
|
||||
public int linkOk;
|
||||
public float headingDeg;
|
||||
public float imuRollDeg, imuPitchDeg;
|
||||
public int imuEnabled;
|
||||
public uint simTimeMs;
|
||||
public uint controlTicks;
|
||||
public int stanceMask;
|
||||
public float poseReachScale; // 1 = full commanded pose; less = scaled to what the legs can reach
|
||||
public float twistScale; // 1 = walk command as sent; less = slowed to a stride the legs can take
|
||||
public float stanceHeightMm; // current standing height (feet below hips)
|
||||
|
||||
public string StateName => robotState switch
|
||||
{
|
||||
0 => "DISARMED", 1 => "SIT", 2 => "STAND", 3 => "WALKING", _ => "?"
|
||||
};
|
||||
}
|
||||
|
||||
// Loads sim/build/hexapod_sim.dll - the unchanged robot firmware compiled for
|
||||
// the PC. The DLL is copied to a temp file and loaded by hand (not DllImport)
|
||||
// so Unity never locks the build output: rebuild with CMake, press Play or
|
||||
// "Reload firmware" and the new code runs. Each load is a fresh power-on.
|
||||
public sealed class HexapodSimNative : IDisposable
|
||||
{
|
||||
public const int ExpectedApiVersion = 5; // HXSIM_API_VERSION
|
||||
|
||||
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate int IntFn();
|
||||
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void VoidFn();
|
||||
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void IntArgFn(int v);
|
||||
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void UIntArgFn(uint v);
|
||||
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void GeometryFn(out HxGeometry g);
|
||||
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void StatusFn(out HxStatus s);
|
||||
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void PacketFn(ref CmdPacket p, int len);
|
||||
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void ImuFn(float ax, float ay, float az, float gx, float gy, float gz);
|
||||
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void BytesInFn(byte[] bytes, int len);
|
||||
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate int BytesOutFn(byte[] bytes, int cap);
|
||||
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void ServosFn(int[] us, int[] attached, uint[] writes);
|
||||
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void FloatsFn(float[] values);
|
||||
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void TwoFloatsFn(float a, float b);
|
||||
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void BodyFn(float[] pos3, float[] quatWxyz4, out float yawDeg, out int contactMask);
|
||||
|
||||
IntPtr handle_;
|
||||
string tempCopy_;
|
||||
|
||||
IntFn apiVersion_, packetSize_;
|
||||
GeometryFn getGeometry_;
|
||||
IntArgFn setRadioPresent_, setImuPresent_;
|
||||
VoidFn start_;
|
||||
UIntArgFn step_;
|
||||
PacketFn radioSend_;
|
||||
ImuFn setImu_;
|
||||
BytesInFn serialInput_;
|
||||
BytesOutFn serialOutput_;
|
||||
ServosFn getServos_;
|
||||
FloatsFn getJoints_, getFeet_;
|
||||
StatusFn getStatus_;
|
||||
TwoFloatsFn setGroundTilt_;
|
||||
BodyFn getBody_;
|
||||
|
||||
public string LoadedFrom { get; private set; }
|
||||
|
||||
public static HexapodSimNative Load(string dllPath)
|
||||
{
|
||||
if (!File.Exists(dllPath))
|
||||
throw new FileNotFoundException(
|
||||
"Simulation DLL not found. Build it first: run .\\sim\\build.ps1 from the repo root in PowerShell.",
|
||||
dllPath);
|
||||
|
||||
var n = new HexapodSimNative();
|
||||
n.tempCopy_ = Path.Combine(Path.GetTempPath(), $"hexapod_sim_{Guid.NewGuid():N}.dll");
|
||||
File.Copy(dllPath, n.tempCopy_, true);
|
||||
n.handle_ = LoadLibraryW(n.tempCopy_);
|
||||
if (n.handle_ == IntPtr.Zero)
|
||||
{
|
||||
int err = Marshal.GetLastWin32Error();
|
||||
n.Dispose();
|
||||
throw new Exception($"LoadLibrary failed for {dllPath} (Win32 error {err}). Is it a 64-bit build?");
|
||||
}
|
||||
n.LoadedFrom = dllPath;
|
||||
|
||||
n.apiVersion_ = n.Fn<IntFn>("hxsim_api_version");
|
||||
n.packetSize_ = n.Fn<IntFn>("hxsim_packet_size");
|
||||
n.getGeometry_ = n.Fn<GeometryFn>("hxsim_get_geometry");
|
||||
n.setRadioPresent_ = n.Fn<IntArgFn>("hxsim_set_radio_present");
|
||||
n.setImuPresent_ = n.Fn<IntArgFn>("hxsim_set_imu_present");
|
||||
n.start_ = n.Fn<VoidFn>("hxsim_start");
|
||||
n.step_ = n.Fn<UIntArgFn>("hxsim_step");
|
||||
n.radioSend_ = n.Fn<PacketFn>("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");
|
||||
n.getFeet_ = n.Fn<FloatsFn>("hxsim_get_feet_mm");
|
||||
n.getStatus_ = n.Fn<StatusFn>("hxsim_get_status");
|
||||
n.setGroundTilt_ = n.Fn<TwoFloatsFn>("hxsim_set_ground_tilt");
|
||||
n.getBody_ = n.Fn<BodyFn>("hxsim_get_body");
|
||||
|
||||
int version = n.apiVersion_();
|
||||
if (version != ExpectedApiVersion)
|
||||
{
|
||||
n.Dispose();
|
||||
throw new Exception($"hexapod_sim.dll API version {version}, Unity scripts expect {ExpectedApiVersion}. Rebuild the DLL or update the scripts.");
|
||||
}
|
||||
int size = n.packetSize_();
|
||||
if (size != Marshal.SizeOf<CmdPacket>())
|
||||
{
|
||||
n.Dispose();
|
||||
throw new Exception($"CmdPacket is {size} bytes in the firmware but {Marshal.SizeOf<CmdPacket>()} in C#. Update HexapodSimNative.CmdPacket to match HexapodProtocol.h.");
|
||||
}
|
||||
return n;
|
||||
}
|
||||
|
||||
T Fn<T>(string name) where T : Delegate
|
||||
{
|
||||
IntPtr p = GetProcAddress(handle_, name);
|
||||
if (p == IntPtr.Zero) throw new Exception($"hexapod_sim.dll is missing export '{name}' - rebuild it.");
|
||||
return Marshal.GetDelegateForFunctionPointer<T>(p);
|
||||
}
|
||||
|
||||
public int PacketSize => packetSize_();
|
||||
public HxGeometry Geometry { get { getGeometry_(out var g); return g; } }
|
||||
public HxStatus Status { get { getStatus_(out var s); return s; } }
|
||||
|
||||
public void SetRadioPresent(bool present) => setRadioPresent_(present ? 1 : 0);
|
||||
public void SetImuPresent(bool present) => setImuPresent_(present ? 1 : 0);
|
||||
public void Start() => start_();
|
||||
public void Step(uint dtMicros) => step_(dtMicros);
|
||||
public void RadioSend(CmdPacket p) => radioSend_(ref p, Marshal.SizeOf<CmdPacket>());
|
||||
// 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()
|
||||
{
|
||||
int n = serialOutput_(serialBuf_, serialBuf_.Length);
|
||||
return n > 0 ? System.Text.Encoding.ASCII.GetString(serialBuf_, 0, n) : "";
|
||||
}
|
||||
|
||||
public void GetServos(int[] us, int[] attached, uint[] writes) => getServos_(us, attached, writes);
|
||||
public void GetJointsDeg(float[] deg18) => getJoints_(deg18);
|
||||
public void GetFeetMm(float[] xyz18) => getFeet_(xyz18);
|
||||
|
||||
public void SetGroundTilt(float rollDeg, float pitchDeg) => setGroundTilt_(rollDeg, pitchDeg);
|
||||
|
||||
readonly float[] bodyPos_ = new float[3], bodyQuat_ = new float[4];
|
||||
// Kinematic body from the DLL, robot-world frame. The rotation is a
|
||||
// robot-frame quaternion held in a Unity Quaternion for its math only.
|
||||
public void GetBody(out Vector3 posMm, out Quaternion rotRobot, out float yawDeg, out int contactMask)
|
||||
{
|
||||
getBody_(bodyPos_, bodyQuat_, out yawDeg, out contactMask);
|
||||
posMm = new Vector3(bodyPos_[0], bodyPos_[1], bodyPos_[2]);
|
||||
rotRobot = new Quaternion(bodyQuat_[1], bodyQuat_[2], bodyQuat_[3], bodyQuat_[0]);
|
||||
}
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
if (handle_ != IntPtr.Zero) { FreeLibrary(handle_); handle_ = IntPtr.Zero; }
|
||||
if (tempCopy_ != null)
|
||||
{
|
||||
try { File.Delete(tempCopy_); } catch { /* still mapped on some systems; temp dir cleans up */ }
|
||||
tempCopy_ = null;
|
||||
}
|
||||
}
|
||||
|
||||
[DllImport("kernel32", SetLastError = true, CharSet = CharSet.Unicode)]
|
||||
static extern IntPtr LoadLibraryW(string path);
|
||||
[DllImport("kernel32", SetLastError = true)]
|
||||
[return: MarshalAs(UnmanagedType.Bool)]
|
||||
static extern bool FreeLibrary(IntPtr handle);
|
||||
[DllImport("kernel32", SetLastError = true, CharSet = CharSet.Ansi, ExactSpelling = true)]
|
||||
static extern IntPtr GetProcAddress(IntPtr handle, string name);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,2 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 5fd045740d26ccf4f98e1153df25add4
|
||||
@@ -0,0 +1,326 @@
|
||||
using System.Collections.Generic;
|
||||
using System.IO;
|
||||
using System.Text;
|
||||
using UnityEngine;
|
||||
|
||||
namespace HexapodSim
|
||||
{
|
||||
// Runs the real robot firmware (robot/, compiled into sim/build/hexapod_sim.dll)
|
||||
// against the existing Unity model:
|
||||
// input (SimRemote) -> CmdPacket over the simulated radio -> firmware loop()
|
||||
// -> 18 servo pulses -> joint rotations on the rig.
|
||||
// The body's motion (kinematic: it rests on its feet, grounded feet don't
|
||||
// slide) and the simulated MPU6050 readings are computed inside the DLL
|
||||
// (sim/SimBody.cpp), so the headless smoke test checks the same thing this
|
||||
// shows. Nothing here decides how the robot moves.
|
||||
[DefaultExecutionOrder(-100)]
|
||||
public class HexapodSimulation : MonoBehaviour
|
||||
{
|
||||
[Header("Firmware")]
|
||||
[Tooltip("Empty = <repo>/sim/build/hexapod_sim.dll")]
|
||||
public string dllPath = "";
|
||||
public bool radioPresent = true;
|
||||
public bool imuPresent = true;
|
||||
[Tooltip("Remote transmit rate. The real remote sends at 100 Hz.")]
|
||||
public float remoteTxHz = 100f;
|
||||
|
||||
[Header("Rig (auto-detected from the scene if empty)")]
|
||||
public Transform chassis;
|
||||
public Transform ground;
|
||||
public ForwardAxis forwardAxis = ForwardAxis.NegativeX;
|
||||
[Tooltip("Unity units per millimetre. The model is built in cm.")]
|
||||
public float unitsPerMm = 0.1f;
|
||||
[Tooltip("Firmware tibia angle when the rig's Knie rotation is 0 (tibia straight down from a horizontal femur).")]
|
||||
public float kneeRestDeg = -90f;
|
||||
|
||||
[Header("World")]
|
||||
[Tooltip("Tilts the ground (deg about robot X = roll, Y = pitch) to exercise the robot gyro.")]
|
||||
public Vector2 groundTiltDeg;
|
||||
public bool followCamera = true;
|
||||
|
||||
// ---- Runtime state, read by SimTestPanel ----
|
||||
public HexapodSimNative Native { get; private set; }
|
||||
public HxGeometry Geometry { get; private set; }
|
||||
public HxStatus Status { get; private set; }
|
||||
public string LoadError { get; private set; }
|
||||
public readonly List<string> SerialLog = new List<string>();
|
||||
public readonly List<string> RigWarnings = new List<string>();
|
||||
public float[] JointsDeg { get; } = new float[18];
|
||||
public float[] FeetMm { get; } = new float[18];
|
||||
public bool[] Contact { get; } = new bool[6];
|
||||
public int[] ServoUs { get; } = new int[18];
|
||||
public int[] ServoAttached { get; } = new int[18];
|
||||
public uint[] ServoWrites { get; } = new uint[18];
|
||||
public Vector3 BodyPosMm => bodyPos_;
|
||||
public float BodyYawDeg => yaw_;
|
||||
|
||||
SimRemote remote_;
|
||||
|
||||
class LegRig
|
||||
{
|
||||
public Transform root, hip1, hip2, knie, footMarker;
|
||||
public float coxaOffsetDeg;
|
||||
}
|
||||
readonly LegRig[] legs_ = new LegRig[6];
|
||||
|
||||
// Body pose from the DLL, robot-world frame (mm, Z up, ground z = 0 before tilt).
|
||||
float yaw_;
|
||||
Vector3 bodyPos_;
|
||||
Quaternion bodyRot_ = Quaternion.identity; // robot-frame rotation used as a math object
|
||||
|
||||
// Unity-side anchors captured at start.
|
||||
Vector3 originUnity_;
|
||||
Quaternion chassisRestRot_;
|
||||
Vector3 groundRestPos_;
|
||||
Quaternion groundRestRot_;
|
||||
Transform camera_;
|
||||
Vector3 cameraOffset_;
|
||||
StringBuilder serialLine_ = new StringBuilder();
|
||||
|
||||
static readonly string[] LegacyMovementScripts =
|
||||
{ "Walk", "RotationControl", "Leg", "HandleHoystick", "Serial2", "Serial3" };
|
||||
|
||||
// Robot-frame axes, for readability (Unity's Vector3.up etc. are Unity axes).
|
||||
static readonly Vector3 AxisX = new Vector3(1, 0, 0); // forward
|
||||
static readonly Vector3 AxisY = new Vector3(0, 1, 0); // left
|
||||
|
||||
void Awake()
|
||||
{
|
||||
DisableLegacyScripts();
|
||||
remote_ = GetComponent<SimRemote>();
|
||||
if (remote_ == null) remote_ = gameObject.AddComponent<SimRemote>();
|
||||
if (GetComponent<SimTestPanel>() == null) gameObject.AddComponent<SimTestPanel>();
|
||||
|
||||
if (chassis == null) chassis = FindChassis();
|
||||
if (ground == null) { var g = GameObject.Find("Plane"); if (g) ground = g.transform; }
|
||||
if (chassis == null) { LoadError = "No leg rig found (expected 6 objects named 'Leg' with hip1/Hip2/Knie)."; return; }
|
||||
|
||||
chassisRestRot_ = chassis.rotation;
|
||||
float groundY = ground ? ground.position.y : chassis.position.y - 150f * unitsPerMm;
|
||||
originUnity_ = new Vector3(chassis.position.x, groundY, chassis.position.z);
|
||||
if (ground) { groundRestPos_ = ground.position; groundRestRot_ = ground.rotation; }
|
||||
if (Camera.main) { camera_ = Camera.main.transform; cameraOffset_ = camera_.position - chassis.position; }
|
||||
|
||||
ReloadFirmware();
|
||||
}
|
||||
|
||||
void OnDestroy() => Native?.Dispose();
|
||||
|
||||
public string ResolvedDllPath =>
|
||||
!string.IsNullOrEmpty(dllPath)
|
||||
? dllPath
|
||||
: Path.GetFullPath(Path.Combine(Application.dataPath, "..", "..", "sim", "build", "hexapod_sim.dll"));
|
||||
|
||||
// Fresh power-on: loads the current DLL build and resets the body.
|
||||
public void ReloadFirmware()
|
||||
{
|
||||
Native?.Dispose();
|
||||
Native = null;
|
||||
LoadError = null;
|
||||
try
|
||||
{
|
||||
Native = HexapodSimNative.Load(ResolvedDllPath);
|
||||
Native.SetRadioPresent(radioPresent);
|
||||
Native.SetImuPresent(imuPresent);
|
||||
Geometry = Native.Geometry;
|
||||
BindRig();
|
||||
Native.SetGroundTilt(groundTiltDeg.x, groundTiltDeg.y);
|
||||
Native.Start();
|
||||
remote_.ResetState();
|
||||
SerialLog.Add($"--- firmware loaded: {ResolvedDllPath}");
|
||||
}
|
||||
catch (System.Exception e)
|
||||
{
|
||||
LoadError = e.Message;
|
||||
Debug.LogError("[HexapodSim] " + e.Message);
|
||||
Native?.Dispose();
|
||||
Native = null;
|
||||
}
|
||||
}
|
||||
|
||||
void FixedUpdate()
|
||||
{
|
||||
if (Native == null) return;
|
||||
float dt = Time.fixedDeltaTime;
|
||||
|
||||
Native.SetGroundTilt(groundTiltDeg.x, groundTiltDeg.y);
|
||||
|
||||
// Remote packets at the remote's rate, firmware time advanced in step.
|
||||
int sub = Mathf.Max(1, Mathf.RoundToInt(dt * remoteTxHz));
|
||||
float subDt = dt / sub;
|
||||
for (int n = 0; n < sub; n++)
|
||||
{
|
||||
remote_.Transmit(Native, subDt);
|
||||
Native.Step((uint)Mathf.RoundToInt(subDt * 1e6f));
|
||||
}
|
||||
|
||||
Status = Native.Status;
|
||||
Native.GetJointsDeg(JointsDeg);
|
||||
Native.GetFeetMm(FeetMm);
|
||||
Native.GetServos(ServoUs, ServoAttached, ServoWrites);
|
||||
Native.GetBody(out bodyPos_, out bodyRot_, out yaw_, out int contactMask);
|
||||
for (int i = 0; i < 6; i++) Contact[i] = (contactMask >> i & 1) != 0;
|
||||
DrainSerial();
|
||||
|
||||
ApplyBodyPose();
|
||||
ApplyJoints();
|
||||
}
|
||||
|
||||
void LateUpdate()
|
||||
{
|
||||
if (followCamera && camera_ && chassis) camera_.position = chassis.position + cameraOffset_;
|
||||
}
|
||||
|
||||
// ------------------------------------------------------------------
|
||||
// Rig
|
||||
// ------------------------------------------------------------------
|
||||
|
||||
void DisableLegacyScripts()
|
||||
{
|
||||
foreach (var mb in FindObjectsByType<MonoBehaviour>(FindObjectsInactive.Include))
|
||||
if (System.Array.IndexOf(LegacyMovementScripts, mb.GetType().Name) >= 0)
|
||||
mb.enabled = false;
|
||||
}
|
||||
|
||||
static List<Transform> FindLegRoots()
|
||||
{
|
||||
var result = new List<Transform>();
|
||||
foreach (var t in FindObjectsByType<Transform>())
|
||||
if (t.name == "Leg" && FindDeep(t, "hip1") != null) result.Add(t);
|
||||
return result;
|
||||
}
|
||||
|
||||
static Transform FindChassis()
|
||||
{
|
||||
var roots = FindLegRoots();
|
||||
return roots.Count > 0 ? roots[0].parent : null;
|
||||
}
|
||||
|
||||
static Transform FindDeep(Transform parent, string name)
|
||||
{
|
||||
foreach (Transform c in parent)
|
||||
{
|
||||
if (string.Equals(c.name, name, System.StringComparison.OrdinalIgnoreCase)) return c;
|
||||
var d = FindDeep(c, name);
|
||||
if (d) return d;
|
||||
}
|
||||
return null;
|
||||
}
|
||||
|
||||
// Matches each firmware leg (Config.h order) to the rig leg at its mount
|
||||
// position, and checks the rig against Config.h so a mismatch is loud.
|
||||
void BindRig()
|
||||
{
|
||||
RigWarnings.Clear();
|
||||
var roots = FindLegRoots();
|
||||
var g = Geometry;
|
||||
for (int i = 0; i < 6; i++)
|
||||
{
|
||||
Vector3 mountLocal = RobotFrame.ToUnity(new Vector3(g.mountXmm[i], g.mountYmm[i], 0), forwardAxis) * unitsPerMm;
|
||||
Transform best = null;
|
||||
float bestDist = float.MaxValue;
|
||||
foreach (var r in roots)
|
||||
{
|
||||
float dist = Vector3.Distance(chassis.InverseTransformPoint(r.position), mountLocal);
|
||||
if (dist < bestDist) { bestDist = dist; best = r; }
|
||||
}
|
||||
if (best == null) { legs_[i] = null; continue; }
|
||||
roots.Remove(best);
|
||||
|
||||
var leg = new LegRig
|
||||
{
|
||||
root = best,
|
||||
hip1 = FindDeep(best, "hip1"),
|
||||
hip2 = FindDeep(best, "Hip2"),
|
||||
knie = FindDeep(best, "Knie"),
|
||||
footMarker = FindDeep(best, "Point"),
|
||||
};
|
||||
if (bestDist / unitsPerMm > 5f)
|
||||
RigWarnings.Add($"leg {i}: rig hip is {bestDist / unitsPerMm:F0} mm from Config.h mount ({g.mountXmm[i]}, {g.mountYmm[i]})");
|
||||
|
||||
// Direction the rig's coxa points at rotation 0, in robot-frame degrees.
|
||||
Vector3 outward = RobotFrame.ToRobot(chassis.InverseTransformDirection(best.forward), forwardAxis);
|
||||
float rigMountDeg = Mathf.Atan2(outward.y, outward.x) * Mathf.Rad2Deg;
|
||||
leg.coxaOffsetDeg = RobotFrame.WrapDeg(g.mountAngleDeg[i] - rigMountDeg);
|
||||
if (Mathf.Abs(leg.coxaOffsetDeg) > 1f)
|
||||
RigWarnings.Add($"leg {i}: Config mount angle {g.mountAngleDeg[i]:F0} vs rig {rigMountDeg:F0} deg (compensated)");
|
||||
|
||||
if (leg.hip1 && leg.hip2 && leg.knie)
|
||||
{
|
||||
float rigCoxa = Vector3.Distance(leg.hip1.position, leg.hip2.position) / unitsPerMm;
|
||||
float rigFemur = Vector3.Distance(leg.hip2.position, leg.knie.position) / unitsPerMm;
|
||||
if (Mathf.Abs(rigCoxa - g.coxaMm) > 2f || Mathf.Abs(rigFemur - g.femurMm) > 2f)
|
||||
RigWarnings.Add($"leg {i}: rig coxa/femur {rigCoxa:F0}/{rigFemur:F0} mm vs Config {g.coxaMm:F0}/{g.femurMm:F0} mm");
|
||||
}
|
||||
else RigWarnings.Add($"leg {i}: missing hip1/Hip2/Knie under '{best.name}'");
|
||||
legs_[i] = leg;
|
||||
}
|
||||
foreach (var w in RigWarnings) Debug.LogWarning("[HexapodSim] " + w);
|
||||
}
|
||||
|
||||
// Firmware joint convention -> rig rotations (see sim/HexapodSimApi.cpp FK):
|
||||
// coxa + = CCW from above; Unity +Y rotation is clockwise from above.
|
||||
// femur/tibia + = up; Unity +X rotation tips the leg's +Z down.
|
||||
void ApplyJoints()
|
||||
{
|
||||
for (int i = 0; i < 6; i++)
|
||||
{
|
||||
var leg = legs_[i];
|
||||
if (leg == null || leg.hip1 == null) continue;
|
||||
float coxa = JointsDeg[i * 3], femur = JointsDeg[i * 3 + 1], tibia = JointsDeg[i * 3 + 2];
|
||||
leg.hip1.localRotation = Quaternion.Euler(0, -(coxa + leg.coxaOffsetDeg), 0);
|
||||
if (leg.hip2) leg.hip2.localRotation = Quaternion.Euler(-femur, 0, 0);
|
||||
if (leg.knie) leg.knie.localRotation = Quaternion.Euler(-(tibia - kneeRestDeg), 0, 0);
|
||||
|
||||
// The sphere marks where the firmware-side FK puts the foot. If it
|
||||
// doesn't sit on the tibia tip, the rig and Config.h disagree.
|
||||
if (leg.footMarker)
|
||||
leg.footMarker.position = chassis.TransformPoint(FootLocal(i));
|
||||
}
|
||||
}
|
||||
|
||||
Vector3 FootLocal(int i) =>
|
||||
RobotFrame.ToUnity(new Vector3(FeetMm[i * 3], FeetMm[i * 3 + 1], FeetMm[i * 3 + 2]), forwardAxis) * unitsPerMm;
|
||||
|
||||
// ------------------------------------------------------------------
|
||||
// World
|
||||
// ------------------------------------------------------------------
|
||||
|
||||
// Robot frame: X forward, Y left (Unity's Vector3.right/up are Unity axes).
|
||||
Quaternion GroundRotation() =>
|
||||
Quaternion.AngleAxis(groundTiltDeg.x, AxisX) * Quaternion.AngleAxis(groundTiltDeg.y, AxisY);
|
||||
|
||||
// The robot-world frame is the chassis's starting orientation, so the
|
||||
// rig's local axes (which the Config.h mapping is defined against) and
|
||||
// the world stay consistent even if the chassis starts rotated.
|
||||
void ApplyBodyPose()
|
||||
{
|
||||
if (!chassis) return;
|
||||
chassis.position = originUnity_ + chassisRestRot_ * RobotFrame.ToUnity(bodyPos_, forwardAxis) * unitsPerMm;
|
||||
chassis.rotation = chassisRestRot_ * RobotFrame.ToUnity(bodyRot_, forwardAxis);
|
||||
if (ground)
|
||||
{
|
||||
Quaternion g = chassisRestRot_ * RobotFrame.ToUnity(GroundRotation(), forwardAxis) * Quaternion.Inverse(chassisRestRot_);
|
||||
ground.rotation = g * groundRestRot_;
|
||||
ground.position = originUnity_ + g * (groundRestPos_ - originUnity_);
|
||||
}
|
||||
}
|
||||
|
||||
void DrainSerial()
|
||||
{
|
||||
string s = Native.SerialOutput();
|
||||
foreach (char ch in s)
|
||||
{
|
||||
if (ch == '\n')
|
||||
{
|
||||
string line = serialLine_.ToString().TrimEnd('\r');
|
||||
serialLine_.Clear();
|
||||
SerialLog.Add(line);
|
||||
Debug.Log("[robot] " + line);
|
||||
}
|
||||
else serialLine_.Append(ch);
|
||||
}
|
||||
if (SerialLog.Count > 200) SerialLog.RemoveRange(0, SerialLog.Count - 200);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,2 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 5e27b78fcb1d2624d9e437ac5c6f8853
|
||||
@@ -0,0 +1,38 @@
|
||||
using UnityEngine;
|
||||
|
||||
namespace HexapodSim
|
||||
{
|
||||
// Which Unity axis the robot's forward (+X) maps to. The old scene marks
|
||||
// the legs on Unity -Z as "invertForLeftLeg", which makes -X forward.
|
||||
public enum ForwardAxis { NegativeX, PositiveX }
|
||||
|
||||
// Conversions between the firmware's body frame (right-handed, mm:
|
||||
// +X forward, +Y left, +Z up) and Unity (left-handed, Y up). Everything
|
||||
// physical in the simulation is computed in the robot frame and only
|
||||
// converted here, in one place.
|
||||
public static class RobotFrame
|
||||
{
|
||||
public static Vector3 ToUnity(Vector3 r, ForwardAxis f) =>
|
||||
f == ForwardAxis.NegativeX ? new Vector3(-r.x, r.z, -r.y) : new Vector3(r.x, r.z, r.y);
|
||||
|
||||
public static Vector3 ToRobot(Vector3 u, ForwardAxis f) =>
|
||||
f == ForwardAxis.NegativeX ? new Vector3(-u.x, -u.z, u.y) : new Vector3(u.x, u.z, u.y);
|
||||
|
||||
// A rotation expressed in the robot frame, as the equivalent Unity rotation.
|
||||
// (The mapping is a reflection, so this is M * R * M^-1, built from two axes.)
|
||||
public static Quaternion ToUnity(Quaternion robotRot, ForwardAxis f)
|
||||
{
|
||||
Vector3 fwd = ToUnity(robotRot * ToRobot(Vector3.forward, f), f);
|
||||
Vector3 up = ToUnity(robotRot * ToRobot(Vector3.up, f), f);
|
||||
return Quaternion.LookRotation(fwd, up);
|
||||
}
|
||||
|
||||
public static float WrapDeg(float d)
|
||||
{
|
||||
d %= 360f;
|
||||
if (d > 180f) d -= 360f;
|
||||
if (d < -180f) d += 360f;
|
||||
return d;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,2 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 1ce33c911afdf8f4a90bcdfe72c26093
|
||||
@@ -0,0 +1,255 @@
|
||||
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.
|
||||
// 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 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("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].
|
||||
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; }
|
||||
|
||||
float speedCurrent_ = 128;
|
||||
float lastWalkInputTime_ = -999f;
|
||||
byte seq_;
|
||||
string serialKeys_ = "";
|
||||
float messageUntil_;
|
||||
|
||||
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());
|
||||
|
||||
if (source == InputSource.RobotSerialHarness)
|
||||
{
|
||||
serialKeys_ += Input.inputString; // raw keystrokes for the firmware's harness
|
||||
return;
|
||||
}
|
||||
|
||||
HandleToggleKeys();
|
||||
if (source == InputSource.Joystick) ReadJoystick();
|
||||
else ReadKeyboardSticks(Time.deltaTime);
|
||||
if (Time.time > messageUntil_) Message = "";
|
||||
}
|
||||
|
||||
// ---- 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);
|
||||
}
|
||||
|
||||
// ---- 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)
|
||||
{
|
||||
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.RobotSerialHarness)
|
||||
{
|
||||
native.SerialInput(serialKeys_); // no radio: the firmware's harness injects its own packets
|
||||
serialKeys_ = "";
|
||||
return;
|
||||
}
|
||||
speedCurrent_ = Mathf.MoveTowards(speedCurrent_, speedTarget, speedRatePerSec * dt);
|
||||
LastPacket = BuildPacket();
|
||||
native.RadioSend(LastPacket);
|
||||
}
|
||||
|
||||
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
|
||||
};
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,2 @@
|
||||
fileFormatVersion: 2
|
||||
guid: d1e34fa033f570c48aba85f690477c41
|
||||
@@ -0,0 +1,160 @@
|
||||
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();
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,2 @@
|
||||
fileFormatVersion: 2
|
||||
guid: dc5806c61edfd2c4e8ef048e706a10b2
|
||||
Reference in new issue
Block a user