mangavalkandClaude Opus 5.5
935526eab5
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>
71 lines
2.9 KiB
C++
71 lines
2.9 KiB
C++
#include "PacketBuilder.h"
|
|
|
|
// Handset tilt (deg) -> -1000..1000 with a small deadzone for hand tremor,
|
|
// scaled by the fade blend so toggling remote gyro ramps instead of steps.
|
|
int16_t PacketBuilder::tiltToWire(float deg, float blend) {
|
|
float mag = fabsf(deg) - REMOTE_TILT_DEADZONE_DEG;
|
|
if (mag <= 0 || blend <= 0) return 0;
|
|
float n = constrain(mag / (REMOTE_TILT_FULL_SCALE_DEG - REMOTE_TILT_DEADZONE_DEG), 0.0f, 1.0f);
|
|
return (int16_t)lroundf(copysignf(n, deg) * 1000.0f * blend);
|
|
}
|
|
|
|
CmdPacket PacketBuilder::build(const StickFrame& s, const RemoteIMU& imu, const RemoteState& st, uint32_t now_ms) {
|
|
CmdPacket p;
|
|
p.seq = seq_++;
|
|
p.speed_scale = st.speedCurrent();
|
|
p.body_height = st.height(); // a setting, not motion: sent even while disarmed
|
|
|
|
uint8_t f = 0;
|
|
if (st.armed()) f |= FLAG_ARMED;
|
|
if (st.standRequested()) f |= FLAG_STAND;
|
|
if (st.modeB()) f |= FLAG_MODE_B;
|
|
if (st.rstickXlate()) f |= FLAG_RSTICK_XLATE;
|
|
if (st.gaitWave()) f |= FLAG_GAIT_WAVE;
|
|
if (st.robotGyro()) f |= FLAG_ROBOT_GYRO;
|
|
|
|
// Disarmed: send a neutral command. The robot ignores motion then anyway,
|
|
// but this keeps its pose smoothers from winding up to a stale stick.
|
|
if (!st.armed()) {
|
|
p.flags = f;
|
|
return p;
|
|
}
|
|
|
|
// ---- Left stick: walking. Handset stick frame -> body frame:
|
|
// stick forward (+Y) -> +vx forward, stick right (+X) -> -vy (vy + = left),
|
|
// twist CCW -> +yaw_rate (turn left).
|
|
p.vx = s.v[L_Y];
|
|
p.vy = -s.v[L_X];
|
|
p.yaw_rate = s.v[L_TWIST];
|
|
|
|
// ---- Right stick: body pose. Each packet field is "about / along" one body
|
|
// axis, so the physical-axis mapping differs per sub-mode:
|
|
if (st.rstickXlate()) {
|
|
p.rx = s.v[R_Y]; // X offset: stick forward -> body forward
|
|
p.ry = -s.v[R_X]; // Y offset: stick right -> body right (-Y)
|
|
p.rz = s.v[R_TWIST]; // Z offset: twist CCW -> body up
|
|
} else {
|
|
p.rx = s.v[R_X]; // roll: stick right -> right side down
|
|
p.ry = s.v[R_Y]; // pitch: stick forward -> nose down
|
|
p.rz = s.v[R_TWIST]; // yaw: twist CCW -> body yaws left
|
|
}
|
|
|
|
// ---- Walk flag: in mode B the robot ignores yaw_rate, so twist alone
|
|
// shouldn't start stepping. Linger avoids STAND<->WALKING chatter.
|
|
bool walk_input = p.vx != 0 || p.vy != 0 || (!st.modeB() && p.yaw_rate != 0);
|
|
if (walk_input) { walk_seen_ = true; last_walk_input_ms_ = now_ms; }
|
|
bool walking = walk_seen_ && (now_ms - last_walk_input_ms_) < WALK_LINGER_MS;
|
|
if (st.standRequested() && (walk_input || walking)) f |= FLAG_WALK;
|
|
|
|
// ---- Remote tilt feedforward: keep the flag up for the whole fade-out so
|
|
// the robot sees the tilt ramp to zero rather than being cut off.
|
|
float blend = st.remoteTiltBlend();
|
|
if (st.remoteGyro() || blend > 0.0f) {
|
|
f |= FLAG_REMOTE_GYRO;
|
|
p.remote_roll = tiltToWire(imu.rollDeg(), blend);
|
|
p.remote_pitch = tiltToWire(imu.pitchDeg(), blend);
|
|
}
|
|
|
|
p.flags = f;
|
|
return p;
|
|
}
|