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>
60 lines
2.3 KiB
C++
60 lines
2.3 KiB
C++
#pragma once
|
|
#include "SmoothedValue.h"
|
|
#include "Kinematics.h"
|
|
#include "Config.h"
|
|
|
|
struct PoseCommand {
|
|
float roll_deg = 0, pitch_deg = 0, yaw_deg = 0;
|
|
float x_mm = 0, y_mm = 0, z_mm = 0;
|
|
};
|
|
|
|
class BodyPose {
|
|
public:
|
|
void begin();
|
|
void setCommand(const PoseCommand& c) { command_ = c; }
|
|
// From RobotIMU stabilization; pass 0,0 when the robot-gyro toggle is off.
|
|
void setCorrection(float roll_deg, float pitch_deg) {
|
|
correction_roll_ = roll_deg;
|
|
correction_pitch_ = pitch_deg;
|
|
}
|
|
|
|
void update(float dt);
|
|
|
|
// Scales the whole pose down (one factor for all six legs) until every set
|
|
// of six feet in `sets` is still physically possible with the pose applied
|
|
// (reachable, no colliding legs). Pass the stride envelope while walking,
|
|
// so the pose fits the whole stride rather than this tick's feet, or the
|
|
// stand targets otherwise. Call after update() and before apply().
|
|
void fitToReach(const FootTarget sets[][6], int count, float dt);
|
|
float reachScale() const { return reach_scale_; } // 1 = full commanded pose
|
|
|
|
// The operator's tilt as actually applied (smoothed and reach-scaled),
|
|
// without the gyro correction - what self-leveling should keep.
|
|
float commandRollDeg() const { return s_roll_.current * reach_scale_; }
|
|
float commandPitchDeg() const { return s_pitch_.current * reach_scale_; }
|
|
|
|
// Applies the current (smoothed, reach-scaled) pose transform to a body-frame foot target.
|
|
FootTarget apply(const FootTarget& in) const;
|
|
|
|
// True if these (un-posed) feet are physically possible with the current
|
|
// pose applied - see Limits::feetFeasible.
|
|
bool feasible(const FootTarget feet[6]) const;
|
|
|
|
private:
|
|
PoseCommand command_;
|
|
float correction_roll_ = 0, correction_pitch_ = 0;
|
|
float reach_scale_ = 1.0f;
|
|
static constexpr float REACH_RECOVER_PER_SEC = 1.0f;
|
|
static constexpr float REACH_DROP_PER_SEC = 3.0f;
|
|
|
|
FootTarget applyScaled(const FootTarget& in, float scale) const;
|
|
bool reachable(const FootTarget sets[][6], int count, float scale) const;
|
|
|
|
SmoothedValue s_roll_{POSE_TILT_MAX_RATE_DEG};
|
|
SmoothedValue s_pitch_{POSE_TILT_MAX_RATE_DEG};
|
|
SmoothedValue s_yaw_{POSE_TILT_MAX_RATE_DEG};
|
|
SmoothedValue s_x_{POSE_OFFSET_MAX_RATE_MM};
|
|
SmoothedValue s_y_{POSE_OFFSET_MAX_RATE_MM};
|
|
SmoothedValue s_z_{POSE_OFFSET_MAX_RATE_MM};
|
|
};
|