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,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