36 lines
1.0 KiB
C++
36 lines
1.0 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);
|
||
|
|
|
||
|
|
// Applies the current (smoothed) pose transform to a body-frame foot target.
|
||
|
|
FootTarget apply(const FootTarget& in) const;
|
||
|
|
|
||
|
|
private:
|
||
|
|
PoseCommand command_;
|
||
|
|
float correction_roll_ = 0, correction_pitch_ = 0;
|
||
|
|
|
||
|
|
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};
|
||
|
|
};
|