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>
76 lines
3.2 KiB
C++
76 lines
3.2 KiB
C++
#pragma once
|
|
#include "Kinematics.h"
|
|
#include "BodyPose.h"
|
|
#include "Config.h"
|
|
|
|
struct GaitPattern {
|
|
float duty_factor; // fraction of the cycle each leg spends in stance
|
|
float leg_phase_offset[6]; // each leg's phase offset within the cycle, 0..1
|
|
};
|
|
|
|
extern const GaitPattern GAIT_TRIPOD;
|
|
extern const GaitPattern GAIT_WAVE;
|
|
|
|
struct Twist {
|
|
float vx = 0, vy = 0; // mm/s, body frame
|
|
float omega = 0; // rad/s, body frame
|
|
};
|
|
|
|
class GaitEngine {
|
|
public:
|
|
void begin();
|
|
void setRequestedPattern(const GaitPattern* p) { requested_pattern_ = p; }
|
|
void setCycleDuration(float sec) { cycle_sec_ = sec; }
|
|
|
|
// Advances every leg by dt under the current twist and pattern, writing each
|
|
// leg's target foot position (body frame, before the body pose) into out[6].
|
|
// `pose` is the body pose that will be applied on top: planted feet only
|
|
// move as far as stays physically possible with it.
|
|
void update(float dt, const Twist& twist, const BodyPose& pose, FootTarget out[6]);
|
|
|
|
// Puts every foot back at its neutral stance position and restarts the
|
|
// cycle to match. Call when walking starts, since the stand pose holds the
|
|
// feet at neutral in between.
|
|
void resetFeet();
|
|
|
|
bool legInStance(int i) const { return leg_phase_[i] < legs_pattern_[i]->duty_factor; }
|
|
|
|
// The extreme foot positions a stride at this twist reaches:
|
|
// [0] even legs at the leading end of their stride, odd legs at the trailing end,
|
|
// [1] the other way round (tripod legs alternate even/odd, so neighbours are
|
|
// always in opposite phases - this is when they come closest),
|
|
// [2] every foot at neutral, lifted by SWING_LIFT_MM (mid-swing).
|
|
static constexpr int ENVELOPE_SETS = 3;
|
|
void strideEnvelope(const Twist& twist, FootTarget sets[ENVELOPE_SETS][6]) const;
|
|
|
|
// Scales the twist down (all components together) until that envelope,
|
|
// with `pose` applied, is physically possible: every foot reachable and no
|
|
// legs colliding. Pass GaitEngine::NO_POSE for the stride alone.
|
|
// twistScale() reports the factor applied (1 = as commanded); with
|
|
// `chained`, this call's factor multiplies onto the previous call's, for a
|
|
// second limit applied to an already-limited twist.
|
|
Twist limitTwist(const Twist& twist, const BodyPose& pose, bool chained = false);
|
|
float twistScale() const { return twist_scale_; }
|
|
static const BodyPose NO_POSE;
|
|
|
|
// The twist the planted feet actually carried out in the last update()
|
|
// (it can be less than asked when a stride change runs a foot out of reach).
|
|
const Twist& appliedTwist() const { return applied_twist_; }
|
|
|
|
// Current foot targets (body frame, before the body pose).
|
|
const FootTarget* feet() const { return foot_; }
|
|
|
|
private:
|
|
Twist applied_twist_;
|
|
bool envelopeFeasible(const Twist& twist, const BodyPose& pose) const;
|
|
float twist_scale_ = 1.0f;
|
|
|
|
float leg_phase_[6];
|
|
const GaitPattern* legs_pattern_[6];
|
|
FootTarget foot_[6]; // current foot target, body frame (stance feet are integrated)
|
|
FootTarget liftoff_[6]; // where each swing started
|
|
float swing_start_[6]; // phase at which each swing started (duty factor, except right after resetFeet)
|
|
const GaitPattern* requested_pattern_ = &GAIT_TRIPOD;
|
|
float cycle_sec_ = DEFAULT_CYCLE_SEC;
|
|
};
|