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,67 @@
|
||||
# Builds the unchanged robot firmware (robot/) into a PC library for the
|
||||
# Unity simulation, against the hardware stand-ins in shim/.
|
||||
#
|
||||
# cmake -S sim -B sim/build -G Ninja -DCMAKE_BUILD_TYPE=Release
|
||||
# cmake --build sim/build
|
||||
# sim/build/sim_smoke.exe headless smoke test
|
||||
#
|
||||
# Output: sim/build/hexapod_sim.dll (Unity loads it from there; see sim/README.md).
|
||||
cmake_minimum_required(VERSION 3.16)
|
||||
project(hexapod_sim CXX)
|
||||
|
||||
set(CMAKE_CXX_STANDARD 17)
|
||||
set(CMAKE_CXX_STANDARD_REQUIRED ON)
|
||||
if(NOT CMAKE_BUILD_TYPE)
|
||||
set(CMAKE_BUILD_TYPE Release)
|
||||
endif()
|
||||
|
||||
set(REPO_ROOT ${CMAKE_CURRENT_SOURCE_DIR}/..)
|
||||
file(GLOB ROBOT_SOURCES CONFIGURE_DEPENDS ${REPO_ROOT}/robot/*.cpp)
|
||||
|
||||
# Each robot/*.cpp is compiled through a one-line wrapper in the build dir.
|
||||
# Compiling it in place would make robot/ the main source directory, and on
|
||||
# Windows' case-insensitive filesystem GCC's own <limits.h> chain
|
||||
# (syslimits.h -> #include_next) then picks up robot/Limits.h.
|
||||
set(ROBOT_WRAPPERS)
|
||||
foreach(src ${ROBOT_SOURCES})
|
||||
get_filename_component(name ${src} NAME_WE)
|
||||
set(wrapper ${CMAKE_CURRENT_BINARY_DIR}/robot_src/${name}.cpp)
|
||||
file(CONFIGURE OUTPUT ${wrapper} CONTENT "// generated by sim/CMakeLists.txt\n#include \"${src}\"\n")
|
||||
list(APPEND ROBOT_WRAPPERS ${wrapper})
|
||||
endforeach()
|
||||
|
||||
# Everything shared by the DLL and the test exe (robot.ino via robot_ino.cpp).
|
||||
add_library(robot_core OBJECT
|
||||
${ROBOT_WRAPPERS}
|
||||
robot_ino.cpp
|
||||
SimHardware.cpp
|
||||
SimBody.cpp
|
||||
HexapodSimApi.cpp
|
||||
)
|
||||
# shim/ first so <Arduino.h>, <Servo.h>, <RF24.h>... resolve to the stand-ins.
|
||||
target_include_directories(robot_core PUBLIC
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/shim
|
||||
${CMAKE_CURRENT_SOURCE_DIR}
|
||||
${REPO_ROOT}/shared/HexapodProtocol
|
||||
)# robot/ is deliberately NOT an include directory (not even -iquote: GCC's
|
||||
# #include_next searches those too). Sim sources include "../robot/..." directly.
|
||||
target_compile_definitions(robot_core PUBLIC HEXAPOD_SIM=1)
|
||||
set_target_properties(robot_core PROPERTIES POSITION_INDEPENDENT_CODE ON)
|
||||
if(CMAKE_CXX_COMPILER_ID MATCHES "GNU|Clang")
|
||||
target_compile_options(robot_core PRIVATE -Wall -Wextra -Wno-unused-parameter)
|
||||
endif()
|
||||
set_source_files_properties(robot_ino.cpp PROPERTIES OBJECT_DEPENDS ${REPO_ROOT}/robot/robot.ino)
|
||||
|
||||
add_library(hexapod_sim SHARED $<TARGET_OBJECTS:robot_core>)
|
||||
set_target_properties(hexapod_sim PROPERTIES PREFIX "") # hexapod_sim.dll, not libhexapod_sim.dll
|
||||
|
||||
add_executable(sim_smoke tests/sim_smoke.cpp $<TARGET_OBJECTS:robot_core>)
|
||||
target_include_directories(sim_smoke PRIVATE ${CMAKE_CURRENT_SOURCE_DIR} ${REPO_ROOT}/shared/HexapodProtocol
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/shim)
|
||||
|
||||
if(MINGW)
|
||||
# Self-contained binaries: no libstdc++/libgcc/winpthread DLLs needed next to them.
|
||||
foreach(t hexapod_sim sim_smoke)
|
||||
target_link_options(${t} PRIVATE -static -static-libgcc -static-libstdc++)
|
||||
endforeach()
|
||||
endif()
|
||||
@@ -0,0 +1,161 @@
|
||||
#include "HexapodSimApi.h"
|
||||
#include "SimHardware.h"
|
||||
#include "SimRobotAccess.h"
|
||||
#include "SimBody.h"
|
||||
#include "../robot/Config.h" // geometry and pins as the firmware sees them
|
||||
#include <HexapodProtocol.h>
|
||||
|
||||
static_assert(NUM_LEGS == HXSIM_NUM_LEGS, "HxGeometry arrays are sized for 6 legs");
|
||||
|
||||
// The real Teensy spins loop() far faster than this; 250 us keeps the
|
||||
// control-rate jitter under 2% at 60 Hz while staying cheap.
|
||||
static constexpr uint32_t HXSIM_LOOP_PERIOD_US = 250;
|
||||
|
||||
static bool g_started = false;
|
||||
static uint32_t g_control_ticks = 0;
|
||||
static bool g_manual_imu = false; // hxsim_set_imu() called: stop feeding the IMU from the body
|
||||
static uint64_t g_last_body_us = 0;
|
||||
|
||||
// After each control tick: move the kinematic body to the new feet, and feed
|
||||
// the simulated MPU6050 what it would measure on that body.
|
||||
static void bodyStep() {
|
||||
float feet[18];
|
||||
hxsim_get_feet_mm(feet);
|
||||
uint64_t now = simhw::nowMicros();
|
||||
simbody::update(feet, (now - g_last_body_us) / 1e6f);
|
||||
g_last_body_us = now;
|
||||
if (!g_manual_imu) {
|
||||
float a[3], g[3];
|
||||
simbody::imu(a, g);
|
||||
simhw::imuSet(a[0], a[1], a[2], g[0], g[1], g[2]);
|
||||
}
|
||||
}
|
||||
|
||||
HXSIM_API int32_t hxsim_api_version(void) { return HXSIM_API_VERSION; }
|
||||
HXSIM_API int32_t hxsim_packet_size(void) { return (int32_t)sizeof(CmdPacket); }
|
||||
|
||||
HXSIM_API void hxsim_get_geometry(HxGeometry* g) {
|
||||
g->num_legs = NUM_LEGS;
|
||||
for (int i = 0; i < NUM_LEGS; i++) {
|
||||
g->mount_x_mm[i] = LEG_MOUNT_X_MM[i];
|
||||
g->mount_y_mm[i] = LEG_MOUNT_Y_MM[i];
|
||||
g->mount_angle_deg[i] = LEG_MOUNT_ANGLE_DEG[i];
|
||||
}
|
||||
g->coxa_mm = COXA_LEN_MM;
|
||||
g->femur_mm = FEMUR_LEN_MM;
|
||||
g->tibia_mm = TIBIA_LEN_MM;
|
||||
g->stance_radius_mm = STANCE_RADIUS_MM;
|
||||
g->stance_height_mm = STANCE_HEIGHT_MM;
|
||||
g->coxa_min_deg = COXA_MIN_DEG; g->coxa_max_deg = COXA_MAX_DEG;
|
||||
g->femur_min_deg = FEMUR_MIN_DEG; g->femur_max_deg = FEMUR_MAX_DEG;
|
||||
g->tibia_min_deg = TIBIA_MIN_DEG; g->tibia_max_deg = TIBIA_MAX_DEG;
|
||||
g->control_hz = CONTROL_HZ;
|
||||
g->servo_us_at_0deg = SERVO_US_AT_0DEG;
|
||||
g->servo_us_per_deg = SERVO_US_PER_DEG;
|
||||
g->leg_clearance_mm = LEG_CLEARANCE_MM;
|
||||
g->stance_height_range_mm = STANCE_HEIGHT_RANGE_MM;
|
||||
}
|
||||
|
||||
HXSIM_API void hxsim_set_radio_present(int32_t present) { simhw::radioSetPresent(present != 0); }
|
||||
HXSIM_API void hxsim_set_imu_present(int32_t present) { simhw::imuSetPresent(present != 0); }
|
||||
|
||||
HXSIM_API void hxsim_start(void) {
|
||||
if (g_started) return;
|
||||
g_started = true;
|
||||
simhw::imuSet(0, 0, 1, 0, 0, 0); // level and still until the body model says otherwise
|
||||
simbody::reset();
|
||||
simrobot::setupOnce();
|
||||
bodyStep();
|
||||
}
|
||||
|
||||
HXSIM_API void hxsim_step(uint32_t dt_us) {
|
||||
if (!g_started) hxsim_start();
|
||||
while (dt_us > 0) {
|
||||
uint32_t chunk = dt_us < HXSIM_LOOP_PERIOD_US ? dt_us : HXSIM_LOOP_PERIOD_US;
|
||||
simhw::advanceMicros(chunk);
|
||||
dt_us -= chunk;
|
||||
uint32_t before = simrobot::lastLoopMicros();
|
||||
simrobot::loopOnce();
|
||||
if (simrobot::lastLoopMicros() != before) {
|
||||
g_control_ticks++;
|
||||
bodyStep();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
HXSIM_API void hxsim_radio_send(const uint8_t* bytes, int32_t len) { simhw::radioSend(bytes, len); }
|
||||
|
||||
HXSIM_API void hxsim_set_imu(float ax, float ay, float az, float gx, float gy, float gz) {
|
||||
g_manual_imu = true;
|
||||
simhw::imuSet(ax, ay, az, gx, gy, gz);
|
||||
}
|
||||
|
||||
HXSIM_API void hxsim_set_ground_tilt(float roll_deg, float pitch_deg) { simbody::setGroundTilt(roll_deg, pitch_deg); }
|
||||
|
||||
HXSIM_API void hxsim_get_body(float* pos_mm, float* quat_wxyz, float* yaw_deg, int32_t* contact_mask) {
|
||||
if (pos_mm) simbody::position(pos_mm);
|
||||
if (quat_wxyz) simbody::orientation(quat_wxyz);
|
||||
if (yaw_deg) *yaw_deg = simbody::yawDeg();
|
||||
if (contact_mask) *contact_mask = simbody::contactMask();
|
||||
}
|
||||
|
||||
HXSIM_API void hxsim_serial_input(const char* bytes, int32_t len) { simhw::serialFeed(bytes, len); }
|
||||
HXSIM_API int32_t hxsim_serial_output(char* out, int32_t cap) { return simhw::serialDrain(out, cap); }
|
||||
|
||||
HXSIM_API void hxsim_get_servos(int32_t* us, int32_t* attached, uint32_t* writes) {
|
||||
for (int i = 0; i < NUM_LEGS; i++)
|
||||
for (int j = 0; j < 3; j++) {
|
||||
const simhw::ServoPin& p = simhw::servoPin(SERVO_PINS[i][j]);
|
||||
if (us) us[i * 3 + j] = p.us;
|
||||
if (attached) attached[i * 3 + j] = p.attached ? 1 : 0;
|
||||
if (writes) writes[i * 3 + j] = p.writes;
|
||||
}
|
||||
}
|
||||
|
||||
// Inverse of ServoDriver::degToMicros - what angle the firmware meant.
|
||||
static float pulseToDeg(int us) {
|
||||
return (us - SERVO_US_AT_0DEG) / (float)SERVO_US_PER_DEG;
|
||||
}
|
||||
|
||||
HXSIM_API void hxsim_get_joints_deg(float* deg) {
|
||||
for (int i = 0; i < NUM_LEGS; i++)
|
||||
for (int j = 0; j < 3; j++)
|
||||
deg[i * 3 + j] = pulseToDeg(simhw::servoPin(SERVO_PINS[i][j]).us);
|
||||
}
|
||||
|
||||
// Forward kinematics, written independently of robot/Kinematics.cpp so it
|
||||
// can catch IK mistakes. Joint convention (the one Config.h's limits imply):
|
||||
// coxa - CCW from the leg's mount angle, viewed from above
|
||||
// femur - elevation above horizontal, + = up
|
||||
// tibia - relative to the femur line, + = up, so a knee-down leg is negative
|
||||
HXSIM_API void hxsim_get_feet_mm(float* xyz) {
|
||||
float deg[NUM_LEGS * 3];
|
||||
hxsim_get_joints_deg(deg);
|
||||
for (int i = 0; i < NUM_LEGS; i++) {
|
||||
float a = radians(LEG_MOUNT_ANGLE_DEG[i] + deg[i * 3 + 0]);
|
||||
float f = radians(deg[i * 3 + 1]);
|
||||
float t = radians(deg[i * 3 + 1] + deg[i * 3 + 2]);
|
||||
float horiz = COXA_LEN_MM + FEMUR_LEN_MM * cosf(f) + TIBIA_LEN_MM * cosf(t);
|
||||
xyz[i * 3 + 0] = LEG_MOUNT_X_MM[i] + horiz * cosf(a);
|
||||
xyz[i * 3 + 1] = LEG_MOUNT_Y_MM[i] + horiz * sinf(a);
|
||||
xyz[i * 3 + 2] = FEMUR_LEN_MM * sinf(f) + TIBIA_LEN_MM * sinf(t);
|
||||
}
|
||||
}
|
||||
|
||||
HXSIM_API void hxsim_get_status(HxStatus* s) {
|
||||
s->robot_state = simrobot::state();
|
||||
s->sit_stand_blend = simrobot::sitStandBlend();
|
||||
s->link_ok = simrobot::linkOk() ? 1 : 0;
|
||||
s->heading_deg = simrobot::headingDeg();
|
||||
s->imu_roll_deg = simrobot::imuRollDeg();
|
||||
s->imu_pitch_deg = simrobot::imuPitchDeg();
|
||||
s->imu_enabled = simrobot::imuEnabled() ? 1 : 0;
|
||||
s->sim_time_ms = (uint32_t)(simhw::nowMicros() / 1000);
|
||||
s->control_ticks = g_control_ticks;
|
||||
s->pose_reach_scale = simrobot::poseReachScale();
|
||||
s->twist_scale = simrobot::twistScale();
|
||||
s->stance_height_mm = simrobot::stanceHeightMm();
|
||||
s->stance_mask = 0;
|
||||
for (int i = 0; i < NUM_LEGS; i++)
|
||||
if (simrobot::legInStance(i)) s->stance_mask |= 1 << i;
|
||||
}
|
||||
@@ -0,0 +1,90 @@
|
||||
#pragma once
|
||||
// C API of hexapod_sim.dll: the unchanged robot firmware running against
|
||||
// simulated hardware. Consumed by Unity (Assets/HexapodSim/HexapodSimNative.cs)
|
||||
// and by the headless test (tests/sim_smoke.cpp).
|
||||
//
|
||||
// Conventions: robot body frame, mm and degrees - +X forward, +Y left, +Z up
|
||||
// (same as shared/HexapodProtocol). Per-leg arrays are [leg][joint or xyz]
|
||||
// flattened, leg order as in robot/Config.h, joints coxa/femur/tibia.
|
||||
//
|
||||
// One robot per loaded DLL: its globals are the firmware's globals. To reset
|
||||
// the robot, unload and reload the DLL (Unity does this on every Play).
|
||||
#include <stdint.h>
|
||||
|
||||
#ifdef _WIN32
|
||||
#define HXSIM_API extern "C" __declspec(dllexport)
|
||||
#else
|
||||
#define HXSIM_API extern "C" __attribute__((visibility("default")))
|
||||
#endif
|
||||
|
||||
// Bump whenever a signature or struct below changes; the C# side checks it.
|
||||
#define HXSIM_API_VERSION 5
|
||||
|
||||
#define HXSIM_NUM_LEGS 6
|
||||
|
||||
// All fields 4 bytes wide so C# can mirror this with LayoutKind.Sequential.
|
||||
typedef struct {
|
||||
int32_t num_legs;
|
||||
float mount_x_mm[HXSIM_NUM_LEGS];
|
||||
float mount_y_mm[HXSIM_NUM_LEGS];
|
||||
float mount_angle_deg[HXSIM_NUM_LEGS];
|
||||
float coxa_mm, femur_mm, tibia_mm;
|
||||
float stance_radius_mm, stance_height_mm;
|
||||
float coxa_min_deg, coxa_max_deg;
|
||||
float femur_min_deg, femur_max_deg;
|
||||
float tibia_min_deg, tibia_max_deg;
|
||||
float control_hz;
|
||||
int32_t servo_us_at_0deg;
|
||||
int32_t servo_us_per_deg;
|
||||
float leg_clearance_mm; // LEG_CLEARANCE_MM: minimum gap between neighbouring legs' segments
|
||||
float stance_height_range_mm; // CmdPacket.body_height +-1000 moves the height by this much
|
||||
} HxGeometry;
|
||||
|
||||
typedef struct {
|
||||
int32_t robot_state; // 0 DISARMED, 1 SIT, 2 STAND, 3 WALKING
|
||||
float sit_stand_blend; // 0 sit .. 1 stand
|
||||
int32_t link_ok; // Comms::linkOk()
|
||||
float heading_deg; // robot.ino's open-loop heading estimate
|
||||
float imu_roll_deg; // RobotIMU filter output
|
||||
float imu_pitch_deg;
|
||||
int32_t imu_enabled; // robot-gyro toggle as the robot sees it
|
||||
uint32_t sim_time_ms;
|
||||
uint32_t control_ticks; // loop() passes that ran the control step
|
||||
int32_t stance_mask; // bit i = leg i in stance per the gait engine
|
||||
float pose_reach_scale; // 1 = full commanded body pose; less = scaled down to what the legs can reach
|
||||
float twist_scale; // 1 = walk command as sent; less = slowed to a stride the legs can take
|
||||
float stance_height_mm; // current (rate-limited) standing height: feet below hips
|
||||
} HxStatus;
|
||||
|
||||
HXSIM_API int32_t hxsim_api_version(void);
|
||||
HXSIM_API int32_t hxsim_packet_size(void); // sizeof(CmdPacket) as compiled
|
||||
HXSIM_API void hxsim_get_geometry(HxGeometry* out);
|
||||
|
||||
// Hardware present at power-on. Call before hxsim_start().
|
||||
HXSIM_API void hxsim_set_radio_present(int32_t present);
|
||||
HXSIM_API void hxsim_set_imu_present(int32_t present);
|
||||
|
||||
HXSIM_API void hxsim_start(void); // runs setup() once
|
||||
// Advances simulated time by dt_us, calling loop() every HXSIM_LOOP_PERIOD_US
|
||||
// like the real busy loop (loop() itself holds the CONTROL_HZ rate).
|
||||
HXSIM_API void hxsim_step(uint32_t dt_us);
|
||||
|
||||
// Inputs
|
||||
HXSIM_API void hxsim_radio_send(const uint8_t* bytes, int32_t len); // one packet on air
|
||||
HXSIM_API void hxsim_set_ground_tilt(float roll_deg, float pitch_deg); // tilt the world (robot-gyro tests)
|
||||
// The MPU6050 is normally fed from the kinematic body model (SimBody.cpp).
|
||||
// Calling this overrides it with fixed readings from then on.
|
||||
HXSIM_API void hxsim_set_imu(float ax_g, float ay_g, float az_g,
|
||||
float gx_dps, float gy_dps, float gz_dps);
|
||||
HXSIM_API void hxsim_serial_input(const char* bytes, int32_t len); // TEST_SERIAL_INPUT keys
|
||||
|
||||
// Outputs
|
||||
HXSIM_API int32_t hxsim_serial_output(char* out, int32_t cap); // drains robot Serial prints
|
||||
HXSIM_API void hxsim_get_servos(int32_t* us_18, int32_t* attached_18, uint32_t* writes_18);
|
||||
HXSIM_API void hxsim_get_joints_deg(float* deg_18); // servo pulses decoded with Config.h constants
|
||||
HXSIM_API void hxsim_get_feet_mm(float* xyz_18); // forward kinematics of those angles, body frame
|
||||
HXSIM_API void hxsim_get_status(HxStatus* out);
|
||||
// Kinematic body (SimBody.cpp): the body rests on the support plane of its
|
||||
// feet and grounded feet don't slide. World frame = robot frame at power-on,
|
||||
// ground at z = 0. Any pointer may be null.
|
||||
HXSIM_API void hxsim_get_body(float* pos_mm_3, float* quat_wxyz_4, float* yaw_deg, int32_t* contact_mask);
|
||||
@@ -0,0 +1,120 @@
|
||||
# Firmware simulation (software-in-the-loop)
|
||||
|
||||
This folder compiles the **unchanged** robot firmware (`robot/*.cpp` plus
|
||||
`robot.ino`) into a Windows DLL. The Unity project
|
||||
(`Hexapod_Simulation_Unity/`) runs that DLL, so the code you test in the
|
||||
simulation is exactly the code you flash to the Teensy. `robot/` has no
|
||||
`#ifdef SIM` anywhere.
|
||||
|
||||
```
|
||||
robot/ (unchanged) ──► PlatformIO + Teensy core ──► Teensy 4.0 firmware
|
||||
└─► CMake + g++ + sim/shim/ ──► sim/build/hexapod_sim.dll ──► Unity
|
||||
```
|
||||
|
||||
## How it works
|
||||
|
||||
`shim/` provides PC stand-ins for the only hardware the firmware touches:
|
||||
|
||||
| Firmware uses | Simulated as |
|
||||
|---|---|
|
||||
| `Servo::writeMicroseconds()` | the pulse stored per pin, decoded back to angles with `Config.h`'s `SERVO_US_*` |
|
||||
| `Wire` + MPU6050 registers | a register-level fake MPU6050 at 0x68, fed from the simulated body's tilt and rotation |
|
||||
| `RF24` | a simulated radio: packets are received only if channel, address, data rate and **payload size** match, and the RX FIFO holds 3 |
|
||||
| `millis()` / `micros()` / `Serial` | a simulated clock that Unity advances, and captured serial I/O |
|
||||
|
||||
`robot_ino.cpp` compiles `robot.ino` itself and exposes read-only views of
|
||||
its globals (state machine, heading, IMU filter, gait phase) for status
|
||||
display. `HexapodSimApi.h` is the C API. Its forward kinematics is written
|
||||
separately from `robot/Kinematics.cpp` on purpose, so it can catch IK
|
||||
mistakes.
|
||||
|
||||
## Build
|
||||
|
||||
Needs a 64-bit g++ and CMake. Strawberry Perl's `C:\Strawberry\c\bin` has
|
||||
g++, cmake and ninja, and the script falls back to it automatically. From
|
||||
the repo root in PowerShell:
|
||||
|
||||
```
|
||||
.\sim\build.ps1 # configure (first time) + build + smoke test
|
||||
.\sim\build.ps1 -NoTest # build only
|
||||
.\sim\build.ps1 -Clean # rebuild from scratch
|
||||
```
|
||||
|
||||
To run the steps by hand, use separate lines. Windows PowerShell 5.1 has
|
||||
no `&&`:
|
||||
|
||||
```
|
||||
cmake -S sim -B sim\build -G Ninja
|
||||
cmake --build sim\build
|
||||
.\sim\build\sim_smoke.exe
|
||||
```
|
||||
|
||||
Rebuild after any change in `robot/` or `shared/`. In Unity, press **Play**
|
||||
(or **Reload firmware** in the panel) to pick up the new DLL. Unity copies
|
||||
the DLL before loading it, so it never locks the build output and you
|
||||
don't need to restart the editor.
|
||||
|
||||
## Headless smoke test (`tests/sim_smoke.cpp`)
|
||||
|
||||
It drives the firmware the way the remote would and checks:
|
||||
- disarmed at power-on,
|
||||
- arm lands in SIT,
|
||||
- stand reaches STAND,
|
||||
- **IK gives valid solutions and the feet end up at `STANCE_HEIGHT`**,
|
||||
- `FLAG_WALK` leads to WALKING,
|
||||
- **`+vx` moves the body forward**,
|
||||
- link loss disarms,
|
||||
- a wrong-size packet is ignored,
|
||||
- the serial harness still works.
|
||||
|
||||
## Using it in Unity
|
||||
|
||||
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:
|
||||
|
||||
- **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.
|
||||
|
||||
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.
|
||||
|
||||
The body moves **kinematically**, computed in the DLL (`SimBody.cpp`), so
|
||||
Unity shows what the smoke test checks:
|
||||
- The body rests on the support plane of its feet, the plane all feet are
|
||||
on or above.
|
||||
- Grounded feet don't slide.
|
||||
- The simulated MPU6050 reads that body's tilt and rotation rate.
|
||||
|
||||
The sphere at each foot (`Point`) is placed by the DLL's forward
|
||||
kinematics. If it doesn't sit on the tibia tip, the rig and `Config.h`
|
||||
disagree.
|
||||
|
||||
Setup that lives in the Inspector on `HexapodSimulation`:
|
||||
- `forwardAxis`: robot forward = Unity −X by default, taken from the old
|
||||
scene's `invertForLeftLeg` naming.
|
||||
- `unitsPerMm`: 0.1, since the model is in cm.
|
||||
- `kneeRestDeg`: −90, the rig's knee pose at rotation 0.
|
||||
|
||||
For a custom setup instead of the bootstrap, add a `HexapodSimulation`
|
||||
component to the scene yourself.
|
||||
|
||||
## What is not simulated
|
||||
|
||||
- Physics (mass, falling, slipping, servo torque and speed).
|
||||
- Servo calibration. The sim decodes pulses with `Config.h`'s single
|
||||
`SERVO_US_AT_0DEG` / `SERVO_US_PER_DEG`, i.e. what the firmware *intends*,
|
||||
not per-servo offsets or directions.
|
||||
- The remote firmware. `SimRemote.cs` mirrors its packet rules instead.
|
||||
@@ -0,0 +1,196 @@
|
||||
#include "SimBody.h"
|
||||
#include <math.h>
|
||||
|
||||
namespace {
|
||||
|
||||
struct V { float x = 0, y = 0, z = 0; };
|
||||
struct Q { float w = 1, x = 0, y = 0, z = 0; };
|
||||
|
||||
V operator+(V a, V b) { return { a.x + b.x, a.y + b.y, a.z + b.z }; }
|
||||
V operator-(V a, V b) { return { a.x - b.x, a.y - b.y, a.z - b.z }; }
|
||||
V operator*(V a, float s) { return { a.x * s, a.y * s, a.z * s }; }
|
||||
float dot(V a, V b) { return a.x * b.x + a.y * b.y + a.z * b.z; }
|
||||
V cross(V a, V b) { return { a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x }; }
|
||||
float len(V a) { return sqrtf(dot(a, a)); }
|
||||
|
||||
Q mul(Q a, Q b) {
|
||||
return { a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z,
|
||||
a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y,
|
||||
a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x,
|
||||
a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w };
|
||||
}
|
||||
Q conj(Q q) { return { q.w, -q.x, -q.y, -q.z }; }
|
||||
Q normalized(Q q) {
|
||||
float n = sqrtf(q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z);
|
||||
return { q.w / n, q.x / n, q.y / n, q.z / n };
|
||||
}
|
||||
V rotate(Q q, V v) {
|
||||
Q p = mul(mul(q, Q{ 0, v.x, v.y, v.z }), conj(q));
|
||||
return { p.x, p.y, p.z };
|
||||
}
|
||||
Q axisAngle(V axis, float rad) {
|
||||
float s = sinf(rad * 0.5f);
|
||||
return { cosf(rad * 0.5f), axis.x * s, axis.y * s, axis.z * s };
|
||||
}
|
||||
// Shortest rotation taking unit vector a onto unit vector b.
|
||||
Q fromTo(V a, V b) {
|
||||
float d = dot(a, b);
|
||||
if (d < -0.9999f) { // opposite: any perpendicular axis
|
||||
V axis = fabsf(a.x) < 0.9f ? cross(a, V{ 1, 0, 0 }) : cross(a, V{ 0, 1, 0 });
|
||||
return axisAngle(axis * (1.0f / len(axis)), (float)M_PI);
|
||||
}
|
||||
V c = cross(a, b);
|
||||
return normalized(Q{ 1 + d, c.x, c.y, c.z });
|
||||
}
|
||||
|
||||
const V UP = { 0, 0, 1 };
|
||||
constexpr float DEG = 180.0f / (float)M_PI;
|
||||
constexpr float CONTACT_EPS_MM = 3.0f;
|
||||
constexpr float MAX_GROUND_TILT_COS = 0.5f; // body can't be tilted more than 60 deg against its ground
|
||||
|
||||
// ---- State ----
|
||||
bool g_init = false;
|
||||
V g_normal_prev = UP; // ground normal in the body frame, last update
|
||||
V g_ground_feet_prev[6]; // feet in the ground-aligned (un-yawed) frame, last update
|
||||
int g_contact = 0, g_contact_prev = 0;
|
||||
float g_px = 0, g_py = 0, g_yaw = 0; // planar body position (mm) and heading (rad)
|
||||
Q g_rot, g_rot_prev;
|
||||
V g_pos;
|
||||
V g_gyro_dps;
|
||||
float g_tilt_roll = 0, g_tilt_pitch = 0;
|
||||
|
||||
Q groundRotation() {
|
||||
return mul(axisAngle(V{ 1, 0, 0 }, g_tilt_roll / DEG), axisAngle(V{ 0, 1, 0 }, g_tilt_pitch / DEG));
|
||||
}
|
||||
|
||||
// The support plane: the plane the body rests on is one that every foot is
|
||||
// on or above (a bottom face of the feet's convex hull). Of those, take the
|
||||
// one most feet touch, then the one closest to last update's ground (the
|
||||
// body doesn't flip onto a different face without reason).
|
||||
bool supportPlane(const V feet[6], V& n_out, float& d_out) {
|
||||
int best_count = -1;
|
||||
float best_align = -2;
|
||||
for (int i = 0; i < 6; i++)
|
||||
for (int j = i + 1; j < 6; j++)
|
||||
for (int k = j + 1; k < 6; k++) {
|
||||
V c = cross(feet[j] - feet[i], feet[k] - feet[i]);
|
||||
float l = len(c);
|
||||
if (l < 1e-3f * 100 * 100) continue; // feet (nearly) in a line: no plane
|
||||
V n = c * (1.0f / l);
|
||||
if (dot(n, g_normal_prev) < 0) n = n * -1.0f; // ground normal points up toward the body
|
||||
float align = dot(n, g_normal_prev);
|
||||
if (align < MAX_GROUND_TILT_COS) continue;
|
||||
float d = dot(n, feet[i]);
|
||||
bool all_above = true;
|
||||
int count = 0;
|
||||
for (int m = 0; m < 6; m++) {
|
||||
float h = dot(n, feet[m]) - d;
|
||||
if (h < -CONTACT_EPS_MM) { all_above = false; break; }
|
||||
if (h < CONTACT_EPS_MM) count++;
|
||||
}
|
||||
if (!all_above) continue;
|
||||
if (count > best_count || (count == best_count && align > best_align)) {
|
||||
best_count = count;
|
||||
best_align = align;
|
||||
n_out = n;
|
||||
d_out = d;
|
||||
}
|
||||
}
|
||||
return best_count >= 3;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
namespace simbody {
|
||||
|
||||
void reset() {
|
||||
g_init = false;
|
||||
g_normal_prev = UP;
|
||||
g_contact = g_contact_prev = 0;
|
||||
g_px = g_py = g_yaw = 0;
|
||||
g_rot = g_rot_prev = Q{};
|
||||
g_pos = V{};
|
||||
g_gyro_dps = V{};
|
||||
}
|
||||
|
||||
void setGroundTilt(float roll_deg, float pitch_deg) { g_tilt_roll = roll_deg; g_tilt_pitch = pitch_deg; }
|
||||
|
||||
void update(const float feet_mm[18], float dt) {
|
||||
V feet[6];
|
||||
for (int i = 0; i < 6; i++) feet[i] = { feet_mm[i * 3], feet_mm[i * 3 + 1], feet_mm[i * 3 + 2] };
|
||||
|
||||
V n;
|
||||
float d;
|
||||
if (!supportPlane(feet, n, d)) { // degenerate (e.g. limp legs in a line): keep last ground
|
||||
n = g_normal_prev;
|
||||
float lowest = 1e9f;
|
||||
for (auto& f : feet) lowest = fminf(lowest, dot(n, f));
|
||||
d = lowest;
|
||||
}
|
||||
g_contact = 0;
|
||||
for (int i = 0; i < 6; i++)
|
||||
if (dot(n, feet[i]) - d < CONTACT_EPS_MM) g_contact |= 1 << i;
|
||||
|
||||
float height = -d; // body origin's distance above the ground
|
||||
Q tilt = fromTo(n, UP); // body frame -> ground-aligned frame
|
||||
|
||||
// Planar motion: the rigid 2D move (Kabsch) that best carries the feet
|
||||
// that stayed grounded from their last positions to their new ones; the
|
||||
// body moved by the inverse of it.
|
||||
V ground_feet[6];
|
||||
for (int i = 0; i < 6; i++) ground_feet[i] = rotate(tilt, feet[i]);
|
||||
if (g_init) {
|
||||
int both = g_contact & g_contact_prev, m = 0;
|
||||
V c0, c1;
|
||||
for (int i = 0; i < 6; i++)
|
||||
if (both >> i & 1) { c0 = c0 + g_ground_feet_prev[i]; c1 = c1 + ground_feet[i]; m++; }
|
||||
if (m >= 2) {
|
||||
c0 = c0 * (1.0f / m);
|
||||
c1 = c1 * (1.0f / m);
|
||||
float s_cross = 0, s_dot = 0;
|
||||
for (int i = 0; i < 6; i++) {
|
||||
if (!(both >> i & 1)) continue;
|
||||
V a = g_ground_feet_prev[i] - c0, b = ground_feet[i] - c1;
|
||||
s_cross += a.x * b.y - a.y * b.x;
|
||||
s_dot += a.x * b.x + a.y * b.y;
|
||||
}
|
||||
float theta = atan2f(s_cross, s_dot); // feet turned by theta under the body...
|
||||
float tx = c1.x - (c0.x * cosf(theta) - c0.y * sinf(theta));
|
||||
float ty = c1.y - (c0.x * sinf(theta) + c0.y * cosf(theta));
|
||||
g_yaw -= theta; // ...so the body turned by -theta
|
||||
g_px -= tx * cosf(g_yaw) - ty * sinf(g_yaw);
|
||||
g_py -= tx * sinf(g_yaw) + ty * cosf(g_yaw);
|
||||
}
|
||||
}
|
||||
for (int i = 0; i < 6; i++) g_ground_feet_prev[i] = ground_feet[i];
|
||||
g_contact_prev = g_contact;
|
||||
g_normal_prev = n;
|
||||
|
||||
Q ground = groundRotation();
|
||||
Q rot = normalized(mul(mul(ground, axisAngle(UP, g_yaw)), tilt));
|
||||
g_pos = rotate(ground, V{ g_px, g_py, height });
|
||||
|
||||
if (g_init && dt > 0) {
|
||||
Q dq = mul(conj(g_rot), rot); // body-frame rotation over dt
|
||||
if (dq.w < 0) dq = Q{ -dq.w, -dq.x, -dq.y, -dq.z };
|
||||
float angle = 2.0f * acosf(fminf(1.0f, dq.w));
|
||||
float s = sqrtf(fmaxf(0.0f, 1.0f - dq.w * dq.w));
|
||||
g_gyro_dps = s > 1e-6f ? V{ dq.x / s, dq.y / s, dq.z / s } * (angle / dt * DEG) : V{};
|
||||
}
|
||||
g_rot_prev = g_rot;
|
||||
g_rot = rot;
|
||||
g_init = true;
|
||||
}
|
||||
|
||||
void position(float out[3]) { out[0] = g_pos.x; out[1] = g_pos.y; out[2] = g_pos.z; }
|
||||
void orientation(float out[4]) { out[0] = g_rot.w; out[1] = g_rot.x; out[2] = g_rot.y; out[3] = g_rot.z; }
|
||||
float yawDeg() { return g_yaw * DEG; }
|
||||
int contactMask() { return g_contact; }
|
||||
|
||||
void imu(float accel[3], float gyro[3]) {
|
||||
V a = rotate(conj(g_rot), UP); // gravity reaction, seen from the body
|
||||
accel[0] = a.x; accel[1] = a.y; accel[2] = a.z;
|
||||
gyro[0] = g_gyro_dps.x; gyro[1] = g_gyro_dps.y; gyro[2] = g_gyro_dps.z;
|
||||
}
|
||||
|
||||
} // namespace simbody
|
||||
@@ -0,0 +1,27 @@
|
||||
#pragma once
|
||||
// Kinematic body model: where the robot's body is in the world, given only
|
||||
// where the firmware puts its feet. No dynamics - the body rests on the
|
||||
// ground plane its feet define, and grounded feet don't slide.
|
||||
//
|
||||
// Robot/world frame: right-handed, mm, +X forward, +Y left, +Z up; world
|
||||
// ground at z = 0 before ground tilt.
|
||||
#include <stdint.h>
|
||||
|
||||
namespace simbody {
|
||||
|
||||
void reset();
|
||||
void setGroundTilt(float roll_deg, float pitch_deg); // tilts the ground (and so the robot) - robot-gyro tests
|
||||
|
||||
// Advances the body to match new body-frame foot positions (18 floats, mm).
|
||||
void update(const float feet_mm[18], float dt);
|
||||
|
||||
// Results.
|
||||
void position(float out_mm[3]); // body origin, world
|
||||
void orientation(float out_wxyz[4]); // body -> world rotation
|
||||
float yawDeg(); // accumulated heading (not wrapped)
|
||||
int contactMask(); // bit i = foot i on the ground
|
||||
|
||||
// What an IMU fixed to the body reads: accel in g (+1 g up at rest), gyro deg/s.
|
||||
void imu(float accel_g[3], float gyro_dps[3]);
|
||||
|
||||
} // namespace simbody
|
||||
@@ -0,0 +1,222 @@
|
||||
// Implementations of the Arduino/Teensy stand-ins declared in shim/.
|
||||
#include "SimHardware.h"
|
||||
#include <Arduino.h>
|
||||
#include <Servo.h>
|
||||
#include <Wire.h>
|
||||
#include <RF24.h>
|
||||
#include <EEPROM.h>
|
||||
#include <HexapodProtocol.h>
|
||||
#include <stdarg.h>
|
||||
#include <deque>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
SimSerial Serial;
|
||||
TwoWire Wire;
|
||||
SimEEPROM EEPROM;
|
||||
|
||||
// =====================================================================
|
||||
// Clock
|
||||
// =====================================================================
|
||||
static uint64_t g_now_us = 0;
|
||||
|
||||
namespace simhw {
|
||||
void advanceMicros(uint32_t us) { g_now_us += us; }
|
||||
uint64_t nowMicros() { return g_now_us; }
|
||||
} // namespace simhw
|
||||
|
||||
uint32_t micros() { return (uint32_t)g_now_us; }
|
||||
uint32_t millis() { return (uint32_t)(g_now_us / 1000); }
|
||||
// The robot loop never blocks; if it ever does, time just moves on.
|
||||
void delay(uint32_t ms) { g_now_us += (uint64_t)ms * 1000; }
|
||||
void delayMicroseconds(uint32_t us) { g_now_us += us; }
|
||||
|
||||
// =====================================================================
|
||||
// Serial
|
||||
// =====================================================================
|
||||
static std::deque<char> g_serial_in;
|
||||
static std::string g_serial_out;
|
||||
|
||||
namespace simhw {
|
||||
void serialFeed(const char* bytes, int len) {
|
||||
for (int i = 0; i < len; i++) g_serial_in.push_back(bytes[i]);
|
||||
}
|
||||
int serialDrain(char* out, int cap) {
|
||||
int n = (int)g_serial_out.size() < cap ? (int)g_serial_out.size() : cap;
|
||||
memcpy(out, g_serial_out.data(), n);
|
||||
g_serial_out.erase(0, n);
|
||||
return n;
|
||||
}
|
||||
} // namespace simhw
|
||||
|
||||
int SimSerial::available() { return (int)g_serial_in.size(); }
|
||||
int SimSerial::read() {
|
||||
if (g_serial_in.empty()) return -1;
|
||||
char c = g_serial_in.front();
|
||||
g_serial_in.pop_front();
|
||||
return (uint8_t)c;
|
||||
}
|
||||
size_t SimSerial::write(uint8_t c) {
|
||||
if (g_serial_out.size() < 65536) g_serial_out.push_back((char)c); // cap if nobody drains
|
||||
return 1;
|
||||
}
|
||||
size_t SimSerial::print(const char* s) {
|
||||
size_t n = 0;
|
||||
while (*s) n += write((uint8_t)*s++);
|
||||
return n;
|
||||
}
|
||||
size_t SimSerial::printf(const char* fmt, ...) {
|
||||
char buf[256];
|
||||
va_list ap;
|
||||
va_start(ap, fmt);
|
||||
vsnprintf(buf, sizeof(buf), fmt, ap);
|
||||
va_end(ap);
|
||||
return print(buf);
|
||||
}
|
||||
|
||||
// =====================================================================
|
||||
// Servos
|
||||
// =====================================================================
|
||||
static simhw::ServoPin g_pins[simhw::MAX_PINS];
|
||||
|
||||
namespace simhw {
|
||||
const ServoPin& servoPin(int pin) {
|
||||
static const ServoPin none;
|
||||
return (pin >= 0 && pin < MAX_PINS) ? g_pins[pin] : none;
|
||||
}
|
||||
} // namespace simhw
|
||||
|
||||
uint8_t Servo::attach(int pin) {
|
||||
if (pin < 0 || pin >= simhw::MAX_PINS) return 0;
|
||||
pin_ = pin;
|
||||
if (!g_pins[pin].attached) g_pins[pin].us = 1500;
|
||||
g_pins[pin].attached = true;
|
||||
return 1;
|
||||
}
|
||||
void Servo::detach() {
|
||||
if (pin_ >= 0) g_pins[pin_].attached = false;
|
||||
pin_ = -1;
|
||||
}
|
||||
void Servo::writeMicroseconds(int us) {
|
||||
if (pin_ < 0) return;
|
||||
g_pins[pin_].us = us;
|
||||
g_pins[pin_].writes++;
|
||||
}
|
||||
|
||||
// =====================================================================
|
||||
// I2C + MPU6050 (register-level, address 0x68)
|
||||
// =====================================================================
|
||||
static constexpr int MPU_ADDR = 0x68;
|
||||
static bool g_imu_present = true;
|
||||
static uint8_t g_mpu_regs[128];
|
||||
static uint8_t g_mpu_ptr = 0;
|
||||
static int g_tx_addr = -1;
|
||||
static std::vector<uint8_t> g_tx_bytes;
|
||||
static std::deque<uint8_t> g_rx;
|
||||
|
||||
namespace simhw {
|
||||
void imuSetPresent(bool present) { g_imu_present = present; }
|
||||
|
||||
static void putBE(uint8_t reg, float value, float scale) {
|
||||
float raw = value * scale;
|
||||
if (raw > 32767) raw = 32767;
|
||||
if (raw < -32768) raw = -32768;
|
||||
int16_t v = (int16_t)lroundf(raw);
|
||||
g_mpu_regs[reg] = (uint8_t)((uint16_t)v >> 8);
|
||||
g_mpu_regs[reg + 1] = (uint8_t)(v & 0xFF);
|
||||
}
|
||||
|
||||
void imuSet(float ax, float ay, float az, float gx, float gy, float gz) {
|
||||
// Default +/-2 g and +/-250 deg/s ranges, same scale factors RobotIMU uses.
|
||||
putBE(0x3B, ax, 16384.0f);
|
||||
putBE(0x3D, ay, 16384.0f);
|
||||
putBE(0x3F, az, 16384.0f);
|
||||
putBE(0x41, 0, 1); // temperature, unused
|
||||
putBE(0x43, gx, 131.0f);
|
||||
putBE(0x45, gy, 131.0f);
|
||||
putBE(0x47, gz, 131.0f);
|
||||
}
|
||||
} // namespace simhw
|
||||
|
||||
void TwoWire::beginTransmission(int address) {
|
||||
g_tx_addr = address;
|
||||
g_tx_bytes.clear();
|
||||
}
|
||||
size_t TwoWire::write(uint8_t b) {
|
||||
g_tx_bytes.push_back(b);
|
||||
return 1;
|
||||
}
|
||||
uint8_t TwoWire::endTransmission(bool) {
|
||||
if (g_tx_addr != MPU_ADDR || !g_imu_present) return 2; // address NACK
|
||||
if (!g_tx_bytes.empty()) {
|
||||
g_mpu_ptr = g_tx_bytes[0] & 0x7F; // first byte = register pointer
|
||||
for (size_t i = 1; i < g_tx_bytes.size(); i++) g_mpu_regs[(g_mpu_ptr++) & 0x7F] = g_tx_bytes[i];
|
||||
if (g_tx_bytes.size() > 1) g_mpu_ptr = g_tx_bytes[0] & 0x7F;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
uint8_t TwoWire::requestFrom(int address, int quantity) {
|
||||
g_rx.clear();
|
||||
if (address != MPU_ADDR || !g_imu_present) return 0;
|
||||
for (int i = 0; i < quantity; i++) g_rx.push_back(g_mpu_regs[(g_mpu_ptr + i) & 0x7F]);
|
||||
return (uint8_t)quantity;
|
||||
}
|
||||
int TwoWire::available() { return (int)g_rx.size(); }
|
||||
int TwoWire::read() {
|
||||
if (g_rx.empty()) return -1;
|
||||
uint8_t b = g_rx.front();
|
||||
g_rx.pop_front();
|
||||
return b;
|
||||
}
|
||||
|
||||
// =====================================================================
|
||||
// nRF24L01 "air"
|
||||
// =====================================================================
|
||||
struct AirPacket {
|
||||
uint8_t channel;
|
||||
rf24_datarate_e rate;
|
||||
uint8_t address[5];
|
||||
std::vector<uint8_t> payload;
|
||||
};
|
||||
static bool g_radio_present = true;
|
||||
static std::deque<AirPacket> g_air; // packets waiting at the receiver
|
||||
static constexpr size_t RX_FIFO_DEPTH = 3;
|
||||
|
||||
namespace simhw {
|
||||
void radioSetPresent(bool present) { g_radio_present = present; }
|
||||
void radioSend(const uint8_t* bytes, int len) {
|
||||
if (len < 0 || len > 32) return;
|
||||
if (g_air.size() >= RX_FIFO_DEPTH) return; // FIFO full: the real chip drops it too
|
||||
AirPacket p;
|
||||
p.channel = HXP_RADIO_CHANNEL;
|
||||
p.rate = (rf24_datarate_e)HXP_RADIO_DATA_RATE;
|
||||
memcpy(p.address, HXP_RADIO_ADDRESS, 5);
|
||||
p.payload.assign(bytes, bytes + len);
|
||||
g_air.push_back(p);
|
||||
}
|
||||
} // namespace simhw
|
||||
|
||||
bool RF24::begin() { return g_radio_present; }
|
||||
bool RF24::isChipConnected() { return g_radio_present; }
|
||||
void RF24::openReadingPipe(uint8_t, const uint8_t* address) {
|
||||
memcpy(address_, address, 5);
|
||||
pipe_open_ = true;
|
||||
}
|
||||
bool RF24::available() {
|
||||
if (!g_radio_present || !listening_ || !pipe_open_) return false;
|
||||
// Drop anything this radio's configuration wouldn't have decoded.
|
||||
while (!g_air.empty()) {
|
||||
const AirPacket& p = g_air.front();
|
||||
bool match = p.channel == channel_ && p.rate == data_rate_ &&
|
||||
memcmp(p.address, address_, 5) == 0 && p.payload.size() == payload_size_;
|
||||
if (match) return true;
|
||||
g_air.pop_front();
|
||||
}
|
||||
return false;
|
||||
}
|
||||
void RF24::read(void* buf, uint8_t len) {
|
||||
if (g_air.empty()) return;
|
||||
const AirPacket& p = g_air.front();
|
||||
memcpy(buf, p.payload.data(), len < p.payload.size() ? len : p.payload.size());
|
||||
g_air.pop_front();
|
||||
}
|
||||
@@ -0,0 +1,36 @@
|
||||
#pragma once
|
||||
// Internal interface between the hardware stand-ins (shim/, SimHardware.cpp)
|
||||
// and the exported API (HexapodSimApi.cpp). Not visible to robot/ code.
|
||||
#include <stdint.h>
|
||||
|
||||
namespace simhw {
|
||||
|
||||
// ---- Clock ----
|
||||
void advanceMicros(uint32_t us);
|
||||
uint64_t nowMicros();
|
||||
|
||||
// ---- Serial ----
|
||||
void serialFeed(const char* bytes, int len); // Unity -> robot Serial.read()
|
||||
int serialDrain(char* out, int cap); // robot Serial.print() -> Unity
|
||||
|
||||
// ---- Servos (indexed by pin) ----
|
||||
constexpr int MAX_PINS = 64;
|
||||
struct ServoPin {
|
||||
bool attached = false;
|
||||
int us = 1500; // Teensy Servo's default pulse after attach()
|
||||
uint32_t writes = 0; // writeMicroseconds() calls since start
|
||||
};
|
||||
const ServoPin& servoPin(int pin);
|
||||
|
||||
// ---- MPU6050 ----
|
||||
void imuSetPresent(bool present);
|
||||
// Body-frame values as the chip would measure them: accel in g (reads +1 g
|
||||
// on the axis pointing up when at rest), gyro in deg/s.
|
||||
void imuSet(float ax, float ay, float az, float gx, float gy, float gz);
|
||||
|
||||
// ---- Radio ----
|
||||
void radioSetPresent(bool present);
|
||||
// Puts one packet "on air" using the protocol's channel/address/data rate.
|
||||
void radioSend(const uint8_t* bytes, int len);
|
||||
|
||||
} // namespace simhw
|
||||
@@ -0,0 +1,21 @@
|
||||
#pragma once
|
||||
#include <stdint.h>
|
||||
|
||||
// Read-only access to robot.ino's globals, implemented in robot_ino.cpp
|
||||
// (the translation unit that compiles robot.ino).
|
||||
namespace simrobot {
|
||||
void setupOnce();
|
||||
void loopOnce();
|
||||
int state(); // RobotState: 0 DISARMED, 1 SIT, 2 STAND, 3 WALKING
|
||||
float sitStandBlend();
|
||||
bool linkOk();
|
||||
float headingDeg();
|
||||
float imuRollDeg();
|
||||
float imuPitchDeg();
|
||||
bool imuEnabled();
|
||||
uint32_t lastLoopMicros();
|
||||
bool legInStance(int leg); // gait phase; only meaningful while WALKING
|
||||
float poseReachScale(); // BodyPose::reachScale(): 1 = full commanded pose
|
||||
float twistScale(); // GaitEngine::twistScale(): 1 = walk command as sent
|
||||
float stanceHeightMm(); // Kinematics::stanceHeight()
|
||||
} // namespace simrobot
|
||||
@@ -0,0 +1,42 @@
|
||||
# Builds the firmware simulation DLL and runs the headless smoke test.
|
||||
# Windows PowerShell 5.1 compatible. From the repo root:
|
||||
#
|
||||
# .\sim\build.ps1 configure (first time) + build + smoke test
|
||||
# .\sim\build.ps1 -NoTest build only
|
||||
# .\sim\build.ps1 -Clean wipe sim\build first
|
||||
#
|
||||
# Output: sim\build\hexapod_sim.dll (loaded by the Unity project) and
|
||||
# sim\build\sim_smoke.exe. Exit code = build failure (1) or failed checks.
|
||||
param(
|
||||
[switch]$NoTest,
|
||||
[switch]$Clean
|
||||
)
|
||||
$ErrorActionPreference = 'Stop'
|
||||
|
||||
$simDir = $PSScriptRoot
|
||||
$buildDir = Join-Path $simDir 'build'
|
||||
|
||||
# g++/cmake/ninja: use what's on PATH, else Strawberry Perl's bundled toolchain.
|
||||
if (-not (Get-Command cmake -ErrorAction SilentlyContinue) -or -not (Get-Command g++ -ErrorAction SilentlyContinue)) {
|
||||
$strawberry = 'C:\Strawberry\c\bin'
|
||||
if (Test-Path $strawberry) {
|
||||
$env:PATH = "$strawberry;$env:PATH"
|
||||
} else {
|
||||
Write-Error 'cmake and g++ not found. Install Strawberry Perl (bundles g++, cmake, ninja) or add a MinGW-w64 toolchain to PATH.'
|
||||
}
|
||||
}
|
||||
|
||||
if ($Clean -and (Test-Path $buildDir)) { Remove-Item $buildDir -Recurse -Force }
|
||||
|
||||
if (-not (Test-Path (Join-Path $buildDir 'build.ninja'))) {
|
||||
cmake -S $simDir -B $buildDir -G Ninja -DCMAKE_BUILD_TYPE=Release
|
||||
if ($LASTEXITCODE -ne 0) { exit 1 }
|
||||
}
|
||||
|
||||
cmake --build $buildDir
|
||||
if ($LASTEXITCODE -ne 0) { exit 1 }
|
||||
|
||||
if (-not $NoTest) {
|
||||
& (Join-Path $buildDir 'sim_smoke.exe')
|
||||
exit $LASTEXITCODE
|
||||
}
|
||||
@@ -0,0 +1,22 @@
|
||||
// Compiles robot/robot.ino unchanged as part of the simulation, then exposes
|
||||
// read-only views of its globals so the simulation can report robot state
|
||||
// without the robot code having to know it's being simulated.
|
||||
#include "../robot/robot.ino"
|
||||
#include "SimRobotAccess.h"
|
||||
|
||||
namespace simrobot {
|
||||
void setupOnce() { setup(); }
|
||||
void loopOnce() { loop(); }
|
||||
int state() { return (int)state_machine.state(); }
|
||||
float sitStandBlend() { return state_machine.sitStandBlend(); }
|
||||
bool linkOk() { return comms.linkOk(); }
|
||||
float headingDeg() { return current_heading_deg; }
|
||||
float imuRollDeg() { return imu.rollDeg(); }
|
||||
float imuPitchDeg() { return imu.pitchDeg(); }
|
||||
bool imuEnabled() { return imu.enabled; }
|
||||
uint32_t lastLoopMicros() { return last_loop_us; }
|
||||
bool legInStance(int leg) { return gait.legInStance(leg); }
|
||||
float poseReachScale() { return pose.reachScale(); }
|
||||
float twistScale() { return gait.twistScale(); }
|
||||
float stanceHeightMm() { return Kinematics::stanceHeight(); }
|
||||
} // namespace simrobot
|
||||
@@ -0,0 +1,56 @@
|
||||
#pragma once
|
||||
// PC stand-in for the Arduino core, just enough for robot/ to compile
|
||||
// unchanged into the simulation DLL. Only on the sim's include path - the
|
||||
// Teensy build never sees this file.
|
||||
#include <stdint.h>
|
||||
#include <stddef.h>
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
#include <type_traits>
|
||||
|
||||
#ifndef PI
|
||||
#define PI 3.1415926535897932384626433832795
|
||||
#endif
|
||||
|
||||
// Arduino defines these as macros; templates behave the same for every use
|
||||
// in robot/ without the double-evaluation surprises.
|
||||
template <class T, class L, class H>
|
||||
constexpr T constrain(T amt, L low, H high) {
|
||||
return amt < low ? (T)low : (amt > high ? (T)high : amt);
|
||||
}
|
||||
template <class A, class B>
|
||||
constexpr typename std::common_type<A, B>::type min(A a, B b) { return a < b ? a : b; }
|
||||
template <class A, class B>
|
||||
constexpr typename std::common_type<A, B>::type max(A a, B b) { return a > b ? a : b; }
|
||||
|
||||
inline double radians(double deg) { return deg * (PI / 180.0); }
|
||||
inline double degrees(double rad) { return rad * (180.0 / PI); }
|
||||
|
||||
// Simulated clock, advanced by the simulation (see SimHardware.cpp).
|
||||
uint32_t millis();
|
||||
uint32_t micros();
|
||||
void delay(uint32_t ms);
|
||||
void delayMicroseconds(uint32_t us);
|
||||
|
||||
// USB serial: output is captured for Unity, input is fed from Unity.
|
||||
class SimSerial {
|
||||
public:
|
||||
void begin(unsigned long) {}
|
||||
int available();
|
||||
int read();
|
||||
size_t write(uint8_t c);
|
||||
size_t print(const char* s);
|
||||
size_t print(char c) { return write((uint8_t)c); }
|
||||
size_t print(int v) { return printf("%d", v); }
|
||||
size_t print(unsigned v) { return printf("%u", v); }
|
||||
size_t print(long v) { return printf("%ld", v); }
|
||||
size_t print(unsigned long v) { return printf("%lu", v); }
|
||||
size_t print(double v) { return printf("%.2f", v); }
|
||||
template <class T> size_t println(T v) { size_t n = print(v); return n + print("\n"); }
|
||||
size_t println() { return print("\n"); }
|
||||
size_t printf(const char* fmt, ...);
|
||||
operator bool() const { return true; }
|
||||
};
|
||||
extern SimSerial Serial;
|
||||
@@ -0,0 +1,17 @@
|
||||
#pragma once
|
||||
// Not used by robot/ today; here so a future robot-side EEPROM use doesn't
|
||||
// break the simulation build silently. Backed by RAM, starts erased (0xFF).
|
||||
#include <Arduino.h>
|
||||
|
||||
class SimEEPROM {
|
||||
public:
|
||||
template <class T> T& get(int addr, T& t) { memcpy(&t, mem_ + addr, sizeof(T)); return t; }
|
||||
template <class T> const T& put(int addr, const T& t) { memcpy(mem_ + addr, &t, sizeof(T)); return t; }
|
||||
uint8_t read(int addr) { return mem_[addr]; }
|
||||
void write(int addr, uint8_t v) { mem_[addr] = v; }
|
||||
SimEEPROM() { memset(mem_, 0xFF, sizeof(mem_)); }
|
||||
|
||||
private:
|
||||
uint8_t mem_[4284];
|
||||
};
|
||||
extern SimEEPROM EEPROM;
|
||||
@@ -0,0 +1,38 @@
|
||||
#pragma once
|
||||
#include <Arduino.h>
|
||||
|
||||
// Stand-in for the TMRh20 RF24 library. Enum values match RF24 1.x so the
|
||||
// static_asserts in Comms.cpp still check HexapodProtocol.h.
|
||||
enum rf24_pa_dbm_e { RF24_PA_MIN = 0, RF24_PA_LOW, RF24_PA_HIGH, RF24_PA_MAX, RF24_PA_ERROR };
|
||||
enum rf24_datarate_e { RF24_1MBPS = 0, RF24_2MBPS, RF24_250KBPS };
|
||||
|
||||
// A simulated radio. Packets the simulation puts "on air" (hxsim_radio_send)
|
||||
// are only received if this radio's channel, data rate, address and static
|
||||
// payload size match the sender's, just like the real nRF24L01. The RX FIFO
|
||||
// holds 3 packets; extra packets are dropped.
|
||||
class RF24 {
|
||||
public:
|
||||
RF24(uint16_t ce_pin, uint16_t csn_pin) { (void)ce_pin; (void)csn_pin; }
|
||||
bool begin();
|
||||
bool isChipConnected();
|
||||
void setPayloadSize(uint8_t size) { payload_size_ = size > 32 ? 32 : size; }
|
||||
void openReadingPipe(uint8_t pipe, const uint8_t* address);
|
||||
void openWritingPipe(const uint8_t*) {}
|
||||
void setPALevel(rf24_pa_dbm_e, bool = true) {}
|
||||
bool setDataRate(rf24_datarate_e rate) { data_rate_ = rate; return true; }
|
||||
void setChannel(uint8_t ch) { channel_ = ch > 125 ? 125 : ch; }
|
||||
void setRetries(uint8_t, uint8_t) {}
|
||||
void startListening() { listening_ = true; }
|
||||
void stopListening() { listening_ = false; }
|
||||
bool available();
|
||||
void read(void* buf, uint8_t len);
|
||||
bool write(const void*, uint8_t) { return false; } // the robot never transmits
|
||||
|
||||
private:
|
||||
uint8_t payload_size_ = 32;
|
||||
uint8_t channel_ = 76;
|
||||
rf24_datarate_e data_rate_ = RF24_1MBPS;
|
||||
uint8_t address_[5] = {};
|
||||
bool pipe_open_ = false;
|
||||
bool listening_ = false;
|
||||
};
|
||||
@@ -0,0 +1,2 @@
|
||||
#pragma once
|
||||
// Nothing needed: the simulated RF24 doesn't talk SPI.
|
||||
@@ -0,0 +1,16 @@
|
||||
#pragma once
|
||||
#include <Arduino.h>
|
||||
|
||||
// Stand-in for the Teensy Servo library. Every pulse the robot code writes is
|
||||
// recorded per pin, so the simulation sees exactly what real servos would.
|
||||
class Servo {
|
||||
public:
|
||||
uint8_t attach(int pin);
|
||||
void detach();
|
||||
void writeMicroseconds(int us);
|
||||
void write(int deg) { writeMicroseconds(544 + deg * (2400 - 544) / 180); }
|
||||
bool attached() const { return pin_ >= 0; }
|
||||
|
||||
private:
|
||||
int pin_ = -1;
|
||||
};
|
||||
@@ -0,0 +1,18 @@
|
||||
#pragma once
|
||||
#include <Arduino.h>
|
||||
|
||||
// Stand-in for the I2C bus with one simulated MPU6050 on it (address 0x68).
|
||||
// The simulation fills its accel/gyro registers from the simulated body
|
||||
// motion, so robot/RobotIMU.cpp runs unchanged, register map and all.
|
||||
class TwoWire {
|
||||
public:
|
||||
void begin() {}
|
||||
void setClock(uint32_t) {}
|
||||
void beginTransmission(int address);
|
||||
size_t write(uint8_t b);
|
||||
uint8_t endTransmission(bool stop = true);
|
||||
uint8_t requestFrom(int address, int quantity);
|
||||
int available();
|
||||
int read();
|
||||
};
|
||||
extern TwoWire Wire;
|
||||
@@ -0,0 +1,573 @@
|
||||
// Headless check of the robot firmware running in the simulation - no Unity,
|
||||
// no remote. Drives it with CmdPackets the way the remote would and checks
|
||||
// the observable behavior. Exit code = number of failed checks.
|
||||
//
|
||||
// sim/build/sim_smoke.exe
|
||||
#include "HexapodSimApi.h"
|
||||
#include <HexapodProtocol.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
#include <initializer_list>
|
||||
|
||||
static int g_failures = 0;
|
||||
static void check(bool ok, const char* what, const char* detail = "") {
|
||||
printf("[%s] %s%s%s\n", ok ? "PASS" : "FAIL", what, detail[0] ? " - " : "", detail);
|
||||
if (!ok) g_failures++;
|
||||
}
|
||||
|
||||
static const char* stateName(int s) {
|
||||
static const char* names[] = { "DISARMED", "SIT", "STAND", "WALKING" };
|
||||
return (s >= 0 && s < 4) ? names[s] : "?";
|
||||
}
|
||||
|
||||
// Body yaw as actually achieved by the feet, integrated over every step:
|
||||
// the lowest feet are grounded, and the 2D rotation that best maps their
|
||||
// previous body-frame positions onto the new ones is minus the body's turn.
|
||||
// (Same method as the Unity simulation's kinematic body.)
|
||||
static float g_actual_yaw_deg = 0;
|
||||
// Set HXSIM_VERBOSE=1 to print the mode B timeline and a per-tick trace of
|
||||
// its first half second (stance/grounded feet, foot heights, yaw steps).
|
||||
static bool g_verbose = getenv("HXSIM_VERBOSE") != nullptr;
|
||||
static bool g_trace = false;
|
||||
|
||||
static void groundedMask(const float* feet, bool* grounded) {
|
||||
float min_z = 1e9f;
|
||||
for (int i = 0; i < 6; i++) min_z = fminf(min_z, feet[i * 3 + 2]);
|
||||
for (int i = 0; i < 6; i++) grounded[i] = feet[i * 3 + 2] <= min_z + 5.0f;
|
||||
}
|
||||
|
||||
static void trackYaw(const float* before, const float* after) {
|
||||
bool g0[6], g1[6];
|
||||
groundedMask(before, g0);
|
||||
groundedMask(after, g1);
|
||||
float c0x = 0, c0y = 0, c1x = 0, c1y = 0; int m = 0;
|
||||
for (int i = 0; i < 6; i++)
|
||||
if (g0[i] && g1[i]) { c0x += before[i*3]; c0y += before[i*3+1]; c1x += after[i*3]; c1y += after[i*3+1]; m++; }
|
||||
if (m < 2) return;
|
||||
c0x /= m; c0y /= m; c1x /= m; c1y /= m;
|
||||
float s_cross = 0, s_dot = 0;
|
||||
for (int i = 0; i < 6; i++) {
|
||||
if (!(g0[i] && g1[i])) continue;
|
||||
float ax = before[i*3] - c0x, ay = before[i*3+1] - c0y, bx = after[i*3] - c1x, by = after[i*3+1] - c1y;
|
||||
s_cross += ax * by - ay * bx;
|
||||
s_dot += ax * bx + ay * by;
|
||||
}
|
||||
g_actual_yaw_deg -= atan2f(s_cross, s_dot) * 57.29578f;
|
||||
}
|
||||
|
||||
// ---- Physical-plausibility monitor, enabled per test section ----
|
||||
// Leg clearance is measured independently of the firmware: joint positions
|
||||
// by forward kinematics from the decoded servo angles, then the closest
|
||||
// approach between points sampled along neighbouring legs' segments.
|
||||
static HxGeometry g_geo;
|
||||
static bool g_monitor = false;
|
||||
static float g_min_clearance, g_max_rigid_err, g_min_pose_scale, g_max_pose_scale;
|
||||
static float g_min_roll, g_max_roll, g_min_pitch, g_max_pitch, g_max_body_step_deg;
|
||||
static void resetMonitor() {
|
||||
g_min_clearance = 1e9f; g_max_rigid_err = 0; g_min_pose_scale = 1e9f; g_max_pose_scale = -1e9f;
|
||||
g_min_roll = g_min_pitch = 1e9f; g_max_roll = g_max_pitch = -1e9f; g_max_body_step_deg = 0;
|
||||
}
|
||||
|
||||
// Kinematic body (what Unity shows): world roll/pitch in degrees.
|
||||
static void bodyRollPitch(float& roll, float& pitch, float q_out[4] = nullptr) {
|
||||
float q[4]; hxsim_get_body(nullptr, q, nullptr, nullptr);
|
||||
float w = q[0], x = q[1], y = q[2], z = q[3];
|
||||
roll = atan2f(2 * (w * x + y * z), 1 - 2 * (x * x + y * y)) * 57.29578f;
|
||||
pitch = asinf(fmaxf(-1.0f, fminf(1.0f, 2 * (w * y - z * x)))) * 57.29578f;
|
||||
if (q_out) for (int k = 0; k < 4; k++) q_out[k] = q[k];
|
||||
}
|
||||
|
||||
struct P3 { float x, y, z; };
|
||||
static void legJoints(const float* deg, int i, P3 out[4]) {
|
||||
float yaw = (g_geo.mount_angle_deg[i] + deg[i*3]) * 0.0174533f;
|
||||
float f = deg[i*3+1] * 0.0174533f, t = (deg[i*3+1] + deg[i*3+2]) * 0.0174533f;
|
||||
float r1 = g_geo.coxa_mm, r2 = r1 + g_geo.femur_mm * cosf(f), r3 = r2 + g_geo.tibia_mm * cosf(t);
|
||||
float z2 = g_geo.femur_mm * sinf(f), z3 = z2 + g_geo.tibia_mm * sinf(t);
|
||||
float mx = g_geo.mount_x_mm[i], my = g_geo.mount_y_mm[i], c = cosf(yaw), s = sinf(yaw);
|
||||
out[0] = { mx, my, 0 };
|
||||
out[1] = { mx + c * r1, my + s * r1, 0 };
|
||||
out[2] = { mx + c * r2, my + s * r2, z2 };
|
||||
out[3] = { mx + c * r3, my + s * r3, z3 };
|
||||
}
|
||||
static float clearanceNow() {
|
||||
static const int pairs[][2] = { {0,1}, {1,2}, {3,4}, {4,5}, {0,5}, {2,3} };
|
||||
float deg[18]; hxsim_get_joints_deg(deg);
|
||||
P3 j[6][4];
|
||||
for (int i = 0; i < 6; i++) legJoints(deg, i, j[i]);
|
||||
float best = 1e9f;
|
||||
constexpr int N = 16;
|
||||
for (auto& pr : pairs)
|
||||
for (int sa = 0; sa < 3; sa++)
|
||||
for (int sb = 0; sb < 3; sb++)
|
||||
for (int u = 0; u <= N; u++)
|
||||
for (int v = 0; v <= N; v++) {
|
||||
const P3 &a0 = j[pr[0]][sa], &a1 = j[pr[0]][sa+1], &b0 = j[pr[1]][sb], &b1 = j[pr[1]][sb+1];
|
||||
float ua = (float)u / N, vb = (float)v / N;
|
||||
float dx = a0.x + (a1.x - a0.x) * ua - b0.x - (b1.x - b0.x) * vb;
|
||||
float dy = a0.y + (a1.y - a0.y) * ua - b0.y - (b1.y - b0.y) * vb;
|
||||
float dz = a0.z + (a1.z - a0.z) * ua - b0.z - (b1.z - b0.z) * vb;
|
||||
best = fminf(best, sqrtf(dx*dx + dy*dy + dz*dz));
|
||||
}
|
||||
return best;
|
||||
}
|
||||
|
||||
static float footDist(const float* f, int a, int b) {
|
||||
return hypotf(hypotf(f[a*3] - f[b*3], f[a*3+1] - f[b*3+1]), f[a*3+2] - f[b*3+2]);
|
||||
}
|
||||
|
||||
// Sends `p` at 100 Hz (like the remote) for `seconds` of simulated time.
|
||||
static void run(CmdPacket p, float seconds) {
|
||||
int steps = (int)(seconds * 100.0f + 0.5f);
|
||||
for (int n = 0; n < steps; n++) {
|
||||
float before[18]; hxsim_get_feet_mm(before);
|
||||
HxStatus s0; hxsim_get_status(&s0);
|
||||
float r0, p0, q0[4]; bodyRollPitch(r0, p0, q0);
|
||||
hxsim_radio_send(reinterpret_cast<const uint8_t*>(&p), sizeof(p));
|
||||
p.seq++;
|
||||
hxsim_step(10000);
|
||||
float after[18]; hxsim_get_feet_mm(after);
|
||||
float yaw_before = g_actual_yaw_deg;
|
||||
trackYaw(before, after);
|
||||
if (g_monitor) {
|
||||
HxStatus s1; hxsim_get_status(&s1);
|
||||
g_min_clearance = fminf(g_min_clearance, clearanceNow());
|
||||
g_min_pose_scale = fminf(g_min_pose_scale, s1.pose_reach_scale);
|
||||
g_max_pose_scale = fmaxf(g_max_pose_scale, s1.pose_reach_scale);
|
||||
float r1, p1, q1[4]; bodyRollPitch(r1, p1, q1);
|
||||
g_min_roll = fminf(g_min_roll, r1); g_max_roll = fmaxf(g_max_roll, r1);
|
||||
g_min_pitch = fminf(g_min_pitch, p1); g_max_pitch = fmaxf(g_max_pitch, p1);
|
||||
// How far the body rotated in this 10 ms step (a flip or snap shows up here).
|
||||
float qd = fabsf(q0[0]*q1[0] + q0[1]*q1[1] + q0[2]*q1[2] + q0[3]*q1[3]);
|
||||
g_max_body_step_deg = fmaxf(g_max_body_step_deg, 2 * acosf(fminf(1.0f, qd)) * 57.29578f);
|
||||
// Feet planted through this step must keep their distances (the body
|
||||
// moves rigidly over them); a clamp bending one foot shows up here.
|
||||
int both = s0.stance_mask & s1.stance_mask;
|
||||
if (s1.robot_state != 3) both = 0x3F; // standing: all six planted
|
||||
for (int a = 0; a < 6; a++)
|
||||
for (int b = a + 1; b < 6; b++)
|
||||
if ((both >> a & 1) && (both >> b & 1))
|
||||
g_max_rigid_err = fmaxf(g_max_rigid_err, fabsf(footDist(after, a, b) - footDist(before, a, b)));
|
||||
}
|
||||
if (g_trace) {
|
||||
bool g1[6]; groundedMask(after, g1);
|
||||
HxStatus s; hxsim_get_status(&s);
|
||||
printf(" %s stance ", stateName(s.robot_state));
|
||||
for (int i = 0; i < 6; i++) putchar((s.stance_mask >> i) & 1 ? '#' : '.');
|
||||
printf(" grounded ");
|
||||
for (int i = 0; i < 6; i++) putchar(g1[i] ? '#' : '.');
|
||||
printf(" z");
|
||||
for (int i = 0; i < 6; i++) printf(" %6.1f", after[i * 3 + 2]);
|
||||
printf(" dyaw %+.2f est %.1f\n", g_actual_yaw_deg - yaw_before, s.heading_deg);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static float wrap180(float d) { while (d > 180) d -= 360; while (d < -180) d += 360; return d; }
|
||||
|
||||
// Mean foot z over a set of legs (body frame).
|
||||
static float meanZ(const float* feet, std::initializer_list<int> legs) {
|
||||
float s = 0; for (int i : legs) s += feet[i * 3 + 2]; return s / legs.size();
|
||||
}
|
||||
static void runSilent(float seconds) { hxsim_step((uint32_t)(seconds * 1e6f)); }
|
||||
|
||||
static HxStatus status() { HxStatus s; hxsim_get_status(&s); return s; }
|
||||
|
||||
static uint32_t totalWrites() {
|
||||
uint32_t w[18]; hxsim_get_servos(nullptr, nullptr, w);
|
||||
uint32_t sum = 0; for (uint32_t v : w) sum += v;
|
||||
return sum;
|
||||
}
|
||||
|
||||
static void printLegs(const char* label) {
|
||||
float deg[18], feet[18];
|
||||
hxsim_get_joints_deg(deg);
|
||||
hxsim_get_feet_mm(feet);
|
||||
printf(" %s\n", label);
|
||||
for (int i = 0; i < 6; i++)
|
||||
printf(" leg %d coxa %7.1f femur %7.1f tibia %7.1f | foot %7.1f %7.1f %7.1f mm\n", i,
|
||||
deg[i * 3], deg[i * 3 + 1], deg[i * 3 + 2], feet[i * 3], feet[i * 3 + 1], feet[i * 3 + 2]);
|
||||
}
|
||||
|
||||
int main() {
|
||||
char buf[512];
|
||||
check(hxsim_api_version() == HXSIM_API_VERSION, "API version matches header");
|
||||
snprintf(buf, sizeof(buf), "%d bytes", hxsim_packet_size());
|
||||
check(hxsim_packet_size() == (int)sizeof(CmdPacket), "CmdPacket size", buf);
|
||||
|
||||
HxGeometry g; hxsim_get_geometry(&g);
|
||||
g_geo = g;
|
||||
printf(" geometry: coxa %.0f femur %.0f tibia %.0f mm, stance r %.0f h %.0f mm, %.0f Hz\n",
|
||||
g.coxa_mm, g.femur_mm, g.tibia_mm, g.stance_radius_mm, g.stance_height_mm, g.control_hz);
|
||||
|
||||
hxsim_start();
|
||||
|
||||
// ---- Power-on, no packets: disarmed ----
|
||||
runSilent(0.5f);
|
||||
check(status().robot_state == 0, "starts DISARMED with no link");
|
||||
|
||||
// ---- Arm -> SIT ----
|
||||
CmdPacket p;
|
||||
p.flags = FLAG_ARMED;
|
||||
run(p, 0.5f);
|
||||
HxStatus s = status();
|
||||
check(s.link_ok, "link comes up from radio packets");
|
||||
snprintf(buf, sizeof(buf), "state %s", stateName(s.robot_state));
|
||||
check(s.robot_state == 1, "arming lands in SIT", buf);
|
||||
|
||||
// ---- Stand ----
|
||||
uint32_t writes_before = totalWrites();
|
||||
p.flags = FLAG_ARMED | FLAG_STAND;
|
||||
run(p, 1.5f);
|
||||
s = status();
|
||||
snprintf(buf, sizeof(buf), "state %s, blend %.2f", stateName(s.robot_state), s.sit_stand_blend);
|
||||
check(s.robot_state == 2 && s.sit_stand_blend > 0.99f, "stand request reaches STAND", buf);
|
||||
uint32_t writes = totalWrites() - writes_before;
|
||||
snprintf(buf, sizeof(buf), "%u servo writes in 1.5 s (0 = every IK solution was rejected as invalid)", writes);
|
||||
check(writes > 0, "IK produces valid joint solutions at the stance pose", buf);
|
||||
printLegs("standing:");
|
||||
|
||||
float feet[18]; hxsim_get_feet_mm(feet);
|
||||
float worst = 0;
|
||||
for (int i = 0; i < 6; i++) worst = fmaxf(worst, fabsf(feet[i * 3 + 2] + g.stance_height_mm));
|
||||
snprintf(buf, sizeof(buf), "worst foot height error %.1f mm vs -%.0f", worst, g.stance_height_mm);
|
||||
check(worst < 10.0f, "standing feet are at STANCE_HEIGHT below the hips (FK of servo output)", buf);
|
||||
|
||||
// ---- Walk forward: grounded feet must move backward relative to the body ----
|
||||
p.vx = 600;
|
||||
p.speed_scale = 255;
|
||||
p.flags = FLAG_ARMED | FLAG_STAND | FLAG_WALK;
|
||||
run(p, 0.5f);
|
||||
s = status();
|
||||
snprintf(buf, sizeof(buf), "state %s", stateName(s.robot_state));
|
||||
check(s.robot_state == 3, "FLAG_WALK while standing -> WALKING", buf);
|
||||
|
||||
float dx_sum = 0; int samples = 0;
|
||||
for (int n = 0; n < 200; n++) {
|
||||
float before[18]; hxsim_get_feet_mm(before);
|
||||
int mask = status().stance_mask;
|
||||
run(p, 0.01f);
|
||||
float after[18]; hxsim_get_feet_mm(after);
|
||||
for (int i = 0; i < 6; i++)
|
||||
if ((mask & (1 << i)) && (status().stance_mask & (1 << i))) { dx_sum += after[i * 3] - before[i * 3]; samples++; }
|
||||
}
|
||||
snprintf(buf, sizeof(buf), "mean stance-foot dx %.3f mm/step over %d samples (must be < 0)", samples ? dx_sum / samples : 0.0f, samples);
|
||||
check(samples > 0 && dx_sum < 0, "+vx pushes the body forward (stance feet sweep backward)", buf);
|
||||
|
||||
// ---- Turn left: grounded feet must rotate clockwise around the body ----
|
||||
p.vx = 0;
|
||||
p.yaw_rate = 600;
|
||||
run(p, 0.5f);
|
||||
float turn_sum = 0; samples = 0;
|
||||
for (int n = 0; n < 200; n++) {
|
||||
float before[18]; hxsim_get_feet_mm(before);
|
||||
int mask = status().stance_mask;
|
||||
run(p, 0.01f);
|
||||
float after[18]; hxsim_get_feet_mm(after);
|
||||
for (int i = 0; i < 6; i++)
|
||||
if ((mask & (1 << i)) && (status().stance_mask & (1 << i))) {
|
||||
// z of (position x displacement): > 0 means CCW about the body center.
|
||||
float dx = after[i * 3] - before[i * 3], dy = after[i * 3 + 1] - before[i * 3 + 1];
|
||||
turn_sum += before[i * 3] * dy - before[i * 3 + 1] * dx;
|
||||
samples++;
|
||||
}
|
||||
}
|
||||
snprintf(buf, sizeof(buf), "mean stance-foot angular sweep %.1f (must be < 0 = clockwise)", samples ? turn_sum / samples : 0.0f);
|
||||
check(samples > 0 && turn_sum < 0, "+yaw_rate turns the body left (stance feet sweep clockwise)", buf);
|
||||
|
||||
// ---- Mode A: the body actually turns at the commanded rate ----
|
||||
// (robot.ino's heading estimate integrates the *commanded* omega; if the
|
||||
// gait turns slower or faster, mode B's heading hold is off by that ratio.)
|
||||
p.yaw_rate = 1000; // = MAX_TURN_RATE_DEG_S
|
||||
float est0 = status().heading_deg, act0 = g_actual_yaw_deg;
|
||||
resetMonitor(); g_monitor = true;
|
||||
run(p, 3.0f);
|
||||
g_monitor = false;
|
||||
float est_turn = status().heading_deg - est0, act_turn = g_actual_yaw_deg - act0;
|
||||
snprintf(buf, sizeof(buf), "estimated %.0f deg (walk scaled to %.0f%%), body actually turned %.0f deg in 3 s",
|
||||
est_turn, status().twist_scale * 100, act_turn);
|
||||
check(est_turn > 0 && act_turn / est_turn > 0.8f && act_turn / est_turn < 1.25f,
|
||||
"mode A: actual turn rate matches the heading estimate (within 20%)", buf);
|
||||
snprintf(buf, sizeof(buf), "closest neighbouring legs %.1f mm (limit %.0f)", g_min_clearance, g.leg_clearance_mm);
|
||||
check(g_min_clearance >= g.leg_clearance_mm - 2, "turning at full rate keeps neighbouring legs apart", buf);
|
||||
p.yaw_rate = 0;
|
||||
|
||||
// ---- Heading estimate doesn't run on while not walking ----
|
||||
p.flags = FLAG_ARMED | FLAG_STAND | FLAG_MODE_B;
|
||||
p.vy = 1000; // stick left: target heading +90
|
||||
run(p, 0.05f);
|
||||
p.vx = 0; p.vy = 0;
|
||||
p.flags = FLAG_ARMED | FLAG_STAND | FLAG_MODE_B; // no FLAG_WALK -> STAND, feet don't move
|
||||
est0 = status().heading_deg; act0 = g_actual_yaw_deg;
|
||||
run(p, 1.0f);
|
||||
snprintf(buf, sizeof(buf), "estimate moved %.1f deg, body moved %.1f deg while standing",
|
||||
status().heading_deg - est0, g_actual_yaw_deg - act0);
|
||||
check(fabsf(status().heading_deg - est0) < 1.0f, "heading estimate holds while standing still", buf);
|
||||
|
||||
// ---- Mode B: steering, relative to where the body faces right now ----
|
||||
auto bodyPlanar = [](float& x, float& y) { float pos[3]; hxsim_get_body(pos, nullptr, nullptr, nullptr); x = pos[0]; y = pos[1]; };
|
||||
p.flags = FLAG_ARMED | FLAG_STAND | FLAG_WALK | FLAG_MODE_B;
|
||||
|
||||
// Forward (W): straight on, no turning.
|
||||
p.vx = 1000; p.vy = 0;
|
||||
run(p, 1.0f);
|
||||
est0 = status().heading_deg; act0 = g_actual_yaw_deg;
|
||||
run(p, 2.0f);
|
||||
snprintf(buf, sizeof(buf), "estimate changed %.1f deg, body turned %.1f deg",
|
||||
wrap180(status().heading_deg - est0), g_actual_yaw_deg - act0);
|
||||
check(fabsf(wrap180(status().heading_deg - est0)) < 3.0f && fabsf(g_actual_yaw_deg - act0) < 5.0f,
|
||||
"mode B: forward walks straight on", buf);
|
||||
|
||||
// Forward + left (W+A), held: keeps arcing left while still moving.
|
||||
p.vx = 707; p.vy = 707;
|
||||
est0 = status().heading_deg; act0 = g_actual_yaw_deg;
|
||||
float bx0, by0, bx1, by1; bodyPlanar(bx0, by0);
|
||||
run(p, 2.0f);
|
||||
bodyPlanar(bx1, by1);
|
||||
float moved = hypotf(bx1 - bx0, by1 - by0);
|
||||
snprintf(buf, sizeof(buf), "turned %.0f deg (estimate %.0f) and moved %.0f mm in 2 s",
|
||||
g_actual_yaw_deg - act0, status().heading_deg - est0, moved);
|
||||
check(g_actual_yaw_deg - act0 > 30.0f && moved > 30.0f, "mode B: forward+left held steers left while walking", buf);
|
||||
|
||||
// Left alone (A), held: keeps turning on the spot - not stopping at 90.
|
||||
p.vx = 0; p.vy = 1000;
|
||||
est0 = status().heading_deg; act0 = g_actual_yaw_deg;
|
||||
for (int k = 0; k < 6; k++) {
|
||||
g_trace = g_verbose && k == 0;
|
||||
run(p, 0.5f);
|
||||
g_trace = false;
|
||||
if (g_verbose)
|
||||
printf(" t %.1f state %s estimate %+6.1f actual %+6.1f\n", (k + 1) * 0.5f,
|
||||
stateName(status().robot_state), status().heading_deg - est0, g_actual_yaw_deg - act0);
|
||||
}
|
||||
float est_turn_b = status().heading_deg - est0, act_turn_b = g_actual_yaw_deg - act0;
|
||||
snprintf(buf, sizeof(buf), "3 s held: estimate %.0f deg, body actually turned %.0f deg", est_turn_b, act_turn_b);
|
||||
check(act_turn_b > 120.0f && act_turn_b / est_turn_b > 0.8f && act_turn_b / est_turn_b < 1.25f,
|
||||
"mode B: left held keeps turning left (estimate tracks the body)", buf);
|
||||
|
||||
// Let go: the heading holds.
|
||||
p.vx = 0; p.vy = 0;
|
||||
run(p, 0.2f);
|
||||
est0 = status().heading_deg; act0 = g_actual_yaw_deg;
|
||||
run(p, 1.0f);
|
||||
snprintf(buf, sizeof(buf), "estimate moved %.1f deg, body %.1f deg after release",
|
||||
wrap180(status().heading_deg - est0), g_actual_yaw_deg - act0);
|
||||
check(fabsf(g_actual_yaw_deg - act0) < 3.0f, "mode B: releasing the stick holds the heading", buf);
|
||||
|
||||
// Back (S): walks straight backward - no turning around.
|
||||
p.vx = -1000; p.vy = 0;
|
||||
est0 = status().heading_deg; act0 = g_actual_yaw_deg;
|
||||
float yaw_b; hxsim_get_body(nullptr, nullptr, &yaw_b, nullptr);
|
||||
bodyPlanar(bx0, by0);
|
||||
run(p, 2.0f);
|
||||
bodyPlanar(bx1, by1);
|
||||
float along = (bx1 - bx0) * cosf(yaw_b * 0.0174533f) + (by1 - by0) * sinf(yaw_b * 0.0174533f);
|
||||
snprintf(buf, sizeof(buf), "moved %.0f mm along its facing (must be < 0), turned %.1f deg", along, g_actual_yaw_deg - act0);
|
||||
check(along < -30.0f && fabsf(g_actual_yaw_deg - act0) < 5.0f, "mode B: back walks straight backward", buf);
|
||||
|
||||
// Forward again: straight on in the new facing.
|
||||
p.vx = 1000;
|
||||
est0 = status().heading_deg; act0 = g_actual_yaw_deg;
|
||||
run(p, 3.0f);
|
||||
snprintf(buf, sizeof(buf), "estimate changed %.1f deg, body turned %.1f deg while walking forward",
|
||||
wrap180(status().heading_deg - est0), g_actual_yaw_deg - act0);
|
||||
check(fabsf(wrap180(status().heading_deg - est0)) < 3.0f && fabsf(g_actual_yaw_deg - act0) < 5.0f,
|
||||
"mode B: forward after a turn walks straight on in the body's new facing", buf);
|
||||
p.vx = 0; p.vy = 0;
|
||||
|
||||
// ---- Body pose from the right stick, standing still ----
|
||||
// Protocol: +roll = right side down, +pitch = nose down, +yaw = CCW, and
|
||||
// translation +X = body forward. In body frame that means: roll -> right
|
||||
// feet higher than left, pitch -> front feet higher than rear, yaw -> feet
|
||||
// rotate clockwise, +X offset -> feet move backward.
|
||||
p.flags = FLAG_ARMED | FLAG_STAND;
|
||||
run(p, 1.0f);
|
||||
float neutral[18]; hxsim_get_feet_mm(neutral);
|
||||
float f[18];
|
||||
const auto right = { 0, 1, 2 }, left = { 3, 4, 5 };
|
||||
|
||||
p.rx = 500; run(p, 1.0f); hxsim_get_feet_mm(f); p.rx = 0;
|
||||
snprintf(buf, sizeof(buf), "right feet z %.1f, left feet z %.1f mm", meanZ(f, right), meanZ(f, left));
|
||||
check(meanZ(f, right) > meanZ(f, left) + 5, "right stick +roll lowers the body's right side", buf);
|
||||
|
||||
p.ry = 500; run(p, 1.0f); hxsim_get_feet_mm(f); p.ry = 0;
|
||||
snprintf(buf, sizeof(buf), "front feet z %.1f, rear feet z %.1f mm", meanZ(f, { 0, 5 }), meanZ(f, { 2, 3 }));
|
||||
check(meanZ(f, { 0, 5 }) > meanZ(f, { 2, 3 }) + 5, "right stick +pitch lowers the nose", buf);
|
||||
|
||||
p.rz = 500; run(p, 1.0f); hxsim_get_feet_mm(f); p.rz = 0;
|
||||
float sweep = 0;
|
||||
for (int i = 0; i < 6; i++)
|
||||
sweep += neutral[i*3] * (f[i*3+1] - neutral[i*3+1]) - neutral[i*3+1] * (f[i*3] - neutral[i*3]);
|
||||
snprintf(buf, sizeof(buf), "feet angular sweep %.0f (must be < 0 = clockwise)", sweep);
|
||||
check(sweep < 0, "right stick +yaw turns the body left (feet rotate clockwise under it)", buf);
|
||||
|
||||
p.flags = FLAG_ARMED | FLAG_STAND | FLAG_RSTICK_XLATE;
|
||||
p.rx = 500; run(p, 1.0f); hxsim_get_feet_mm(f); p.rx = 0;
|
||||
float dx = 0; for (int i = 0; i < 6; i++) dx += (f[i*3] - neutral[i*3]) / 6;
|
||||
snprintf(buf, sizeof(buf), "mean foot dx %.1f mm (must be < 0)", dx);
|
||||
check(dx < -5, "right stick +X translation moves the body forward", buf);
|
||||
// ---- Height slider: standing height across its whole range ----
|
||||
for (int16_t h : { (int16_t)1000, (int16_t)-1000 }) {
|
||||
p.flags = FLAG_ARMED | FLAG_STAND;
|
||||
p.body_height = h;
|
||||
run(p, 3.0f); // rate-limited: range/rate is < 1 s, plus margin
|
||||
hxsim_get_feet_mm(f);
|
||||
float want = -(g.stance_height_mm + h / 1000.0f * g.stance_height_range_mm);
|
||||
float worst_h = 0;
|
||||
for (int i = 0; i < 6; i++) worst_h = fmaxf(worst_h, fabsf(f[i*3+2] - want));
|
||||
uint32_t w0[18]; hxsim_get_servos(nullptr, nullptr, w0);
|
||||
run(p, 0.2f);
|
||||
uint32_t w1[18]; hxsim_get_servos(nullptr, nullptr, w1);
|
||||
int frozen = 0; for (int k = 0; k < 18; k++) if (w1[k] == w0[k]) frozen++;
|
||||
snprintf(buf, sizeof(buf), "body_height %d: feet %.1f mm off the %.0f mm target, %d/18 servos frozen",
|
||||
h, worst_h, -want, frozen);
|
||||
check(worst_h < 2.0f && frozen == 0, "height slider sets the standing height", buf);
|
||||
}
|
||||
p.body_height = 0;
|
||||
run(p, 3.0f);
|
||||
hxsim_get_feet_mm(neutral); // back at default height for the pose tests
|
||||
|
||||
// ---- Combined full-deflection poses stay physically possible ----
|
||||
// All six feet are planted while standing, so whatever the body does, the
|
||||
// feet must keep their distances to each other (a rigid shape on the
|
||||
// ground) and every leg must keep getting valid servo commands.
|
||||
struct Combo { const char* name; uint8_t flags; int16_t rx, ry, rz; };
|
||||
const Combo combos[] = {
|
||||
{ "roll+pitch+yaw", FLAG_ARMED | FLAG_STAND, 1000, 1000, 1000 },
|
||||
{ "roll-pitch+yaw", FLAG_ARMED | FLAG_STAND, -1000, 1000, 1000 },
|
||||
{ "x+y+z offset", FLAG_ARMED | FLAG_STAND | FLAG_RSTICK_XLATE, 1000, 1000, 1000 },
|
||||
{ "x-y-z offset", FLAG_ARMED | FLAG_STAND | FLAG_RSTICK_XLATE, 1000, -1000, -1000 },
|
||||
};
|
||||
for (const Combo& c : combos) {
|
||||
p.flags = c.flags; p.rx = c.rx; p.ry = c.ry; p.rz = c.rz;
|
||||
run(p, 2.0f); // let the pose smoothers get all the way there
|
||||
uint32_t w0[18]; hxsim_get_servos(nullptr, nullptr, w0);
|
||||
run(p, 0.2f);
|
||||
uint32_t w1[18]; hxsim_get_servos(nullptr, nullptr, w1);
|
||||
int frozen = 0; for (int k = 0; k < 18; k++) if (w1[k] == w0[k]) frozen++;
|
||||
hxsim_get_feet_mm(f);
|
||||
float worst = 0;
|
||||
for (int a = 0; a < 6; a++)
|
||||
for (int b = a + 1; b < 6; b++) {
|
||||
float d0 = hypotf(hypotf(neutral[a*3] - neutral[b*3], neutral[a*3+1] - neutral[b*3+1]), neutral[a*3+2] - neutral[b*3+2]);
|
||||
float d1 = hypotf(hypotf(f[a*3] - f[b*3], f[a*3+1] - f[b*3+1]), f[a*3+2] - f[b*3+2]);
|
||||
worst = fmaxf(worst, fabsf(d1 - d0));
|
||||
}
|
||||
float clear = clearanceNow();
|
||||
snprintf(buf, sizeof(buf), "%s: feet distorted by up to %.1f mm, %d/18 servos frozen, legs %.0f mm apart",
|
||||
c.name, worst, frozen, clear);
|
||||
check(worst < 3.0f && frozen == 0 && clear >= g.leg_clearance_mm - 2,
|
||||
"full-deflection pose keeps the planted feet rigid and the legs apart", buf);
|
||||
}
|
||||
p.rx = p.ry = p.rz = 0;
|
||||
|
||||
// ---- Walking (WASD) while posing (UIOJKL) ----
|
||||
// Must stay physical (planted feet rigid, legs apart) and steady: the pose
|
||||
// scale must not saw up and down with the step cycle.
|
||||
const Combo walk_combos[] = {
|
||||
{ "walk fwd + roll/pitch/yaw", FLAG_ARMED | FLAG_STAND | FLAG_WALK, 1000, 1000, 1000 },
|
||||
{ "walk fwd + x/y/z offset", FLAG_ARMED | FLAG_STAND | FLAG_WALK | FLAG_RSTICK_XLATE, 1000, -1000, 1000 },
|
||||
};
|
||||
for (const Combo& c : walk_combos) {
|
||||
p.flags = c.flags; p.rx = c.rx; p.ry = c.ry; p.rz = c.rz;
|
||||
p.vx = 1000; p.vy = 500; p.yaw_rate = 500;
|
||||
run(p, 3.0f); // pose smoothers settle
|
||||
resetMonitor(); g_monitor = true;
|
||||
run(p, 2.0f);
|
||||
g_monitor = false;
|
||||
snprintf(buf, sizeof(buf), "%s: planted feet drift %.1f mm, legs >= %.0f mm apart, pose %.0f-%.0f%%, walk %.0f%%, "
|
||||
"body roll %.1f..%.1f pitch %.1f..%.1f, max step %.1f deg",
|
||||
c.name, g_max_rigid_err, g_min_clearance, g_min_pose_scale * 100, g_max_pose_scale * 100,
|
||||
status().twist_scale * 100, g_min_roll, g_max_roll, g_min_pitch, g_max_pitch, g_max_body_step_deg);
|
||||
check(g_max_rigid_err < 1.0f && g_min_clearance >= g.leg_clearance_mm - 2 &&
|
||||
g_max_pose_scale - g_min_pose_scale < 0.05f &&
|
||||
g_max_roll - g_min_roll < 3.0f && g_max_pitch - g_min_pitch < 3.0f && g_max_body_step_deg < 3.0f,
|
||||
"walking while posing stays physical and steady (feet, legs, body)", buf);
|
||||
}
|
||||
|
||||
// ---- Like a person on the keyboard: walk and rotate keys going on/off ----
|
||||
// Sticks ramp like SimRemote's keyboard sticks (4/s), keys change every
|
||||
// 0.3-0.7 s. The body must never snap or tip beyond the tilt limit.
|
||||
{
|
||||
p.flags = FLAG_ARMED | FLAG_STAND | FLAG_WALK;
|
||||
resetMonitor(); g_monitor = true;
|
||||
float sx = 0, sr = 0, sp = 0, sy = 0;
|
||||
for (int n = 0; n < 500; n++) { // 5 s at 100 Hz
|
||||
float t = n * 0.01f;
|
||||
float tx = fmodf(t, 1.4f) < 0.7f ? 1.0f : 0.0f; // W on/off
|
||||
float tr = fmodf(t, 0.6f) < 0.3f ? 1.0f : -1.0f; // L / J alternating
|
||||
float tp = fmodf(t, 1.0f) < 0.5f ? 1.0f : 0.0f; // I on/off
|
||||
float ty = fmodf(t, 0.8f) < 0.4f ? 1.0f : 0.0f; // U on/off
|
||||
auto ramp = [](float v, float target) { float d = target - v; return v + fmaxf(-0.04f, fminf(0.04f, d)); };
|
||||
sx = ramp(sx, tx); sr = ramp(sr, tr); sp = ramp(sp, tp); sy = ramp(sy, ty);
|
||||
p.vx = (int16_t)(sx * 1000); p.rx = (int16_t)(sr * 1000); p.ry = (int16_t)(sp * 1000); p.rz = (int16_t)(sy * 1000);
|
||||
run(p, 0.01f);
|
||||
}
|
||||
g_monitor = false;
|
||||
float tilt = fmaxf(fmaxf(fabsf(g_min_roll), fabsf(g_max_roll)), fmaxf(fabsf(g_min_pitch), fabsf(g_max_pitch)));
|
||||
snprintf(buf, sizeof(buf), "planted feet drift %.1f mm, legs >= %.0f mm apart, body tilt up to %.1f deg, "
|
||||
"max body step %.1f deg / 10 ms", g_max_rigid_err, g_min_clearance, tilt, g_max_body_step_deg);
|
||||
check(g_max_rigid_err < 1.5f && g_min_clearance >= g.leg_clearance_mm - 2 && tilt < 23.0f &&
|
||||
g_max_body_step_deg < 3.0f, "walking while toggling rotate keys stays smooth", buf);
|
||||
}
|
||||
p.vx = p.vy = p.yaw_rate = 0;
|
||||
p.rx = p.ry = p.rz = 0;
|
||||
|
||||
// ---- Standing: the body really tilts as commanded ----
|
||||
p.flags = FLAG_ARMED | FLAG_STAND;
|
||||
p.rx = 500; // +10 deg roll = right side down
|
||||
run(p, 2.0f);
|
||||
float br, bp; bodyRollPitch(br, bp);
|
||||
snprintf(buf, sizeof(buf), "body roll %.1f deg (want +10), pitch %.1f", br, bp);
|
||||
check(fabsf(br - 10.0f) < 1.0f && fabsf(bp) < 1.0f, "standing +roll tilts the simulated body right side down", buf);
|
||||
p.rx = 0;
|
||||
run(p, 2.0f);
|
||||
|
||||
// ---- Robot gyro: on tilted ground the body levels itself ----
|
||||
// (Total tilt, since the robot's heading turns a ground roll into body pitch.)
|
||||
auto tiltNow = []() {
|
||||
float q[4]; hxsim_get_body(nullptr, q, nullptr, nullptr);
|
||||
return acosf(fmaxf(-1.0f, fminf(1.0f, 1 - 2 * (q[1] * q[1] + q[2] * q[2])))) * 57.29578f;
|
||||
};
|
||||
hxsim_set_ground_tilt(8.0f, 0.0f);
|
||||
p.flags = FLAG_ARMED | FLAG_STAND;
|
||||
run(p, 2.0f);
|
||||
float uncorrected = tiltNow();
|
||||
p.flags = FLAG_ARMED | FLAG_STAND | FLAG_ROBOT_GYRO;
|
||||
run(p, 2.0f);
|
||||
float t_min = 1e9f, t_max = -1e9f;
|
||||
for (int n = 0; n < 200; n++) { run(p, 0.01f); float t = tiltNow(); t_min = fminf(t_min, t); t_max = fmaxf(t_max, t); }
|
||||
snprintf(buf, sizeof(buf), "ground tilted 8 deg: body %.1f deg without, %.1f..%.1f deg with robot gyro",
|
||||
uncorrected, t_min, t_max);
|
||||
check(t_max < 1.5f, "robot gyro levels the body on tilted ground, without oscillating", buf);
|
||||
hxsim_set_ground_tilt(0, 0);
|
||||
p.flags = FLAG_ARMED | FLAG_STAND;
|
||||
run(p, 2.0f);
|
||||
|
||||
p.flags = FLAG_ARMED | FLAG_STAND | FLAG_WALK;
|
||||
run(p, 1.0f);
|
||||
|
||||
// ---- Link loss -> disarm ----
|
||||
runSilent(0.5f);
|
||||
s = status();
|
||||
check(!s.link_ok && s.robot_state == 0, "link loss (>0.3 s silence) disarms the robot");
|
||||
|
||||
// ---- Protocol mismatch fails closed ----
|
||||
uint8_t short_payload[16] = {};
|
||||
short_payload[10] = FLAG_ARMED;
|
||||
for (int n = 0; n < 20; n++) { hxsim_radio_send(short_payload, sizeof(short_payload)); hxsim_step(10000); }
|
||||
check(!status().link_ok, "a wrong-size (old protocol) packet is never received");
|
||||
|
||||
// ---- Serial test harness path ----
|
||||
hxsim_serial_input("a", 1);
|
||||
runSilent(0.1f);
|
||||
check(status().robot_state == 1, "serial harness 'a' arms the robot (TEST_SERIAL_INPUT)");
|
||||
|
||||
int n = hxsim_serial_output(buf, sizeof(buf) - 1);
|
||||
buf[n] = 0;
|
||||
if (n) printf(" robot serial output:\n%s\n", buf);
|
||||
|
||||
printf("%d check(s) failed\n", g_failures);
|
||||
return g_failures;
|
||||
}
|
||||
Reference in new issue
Block a user