收腿
This commit is contained in:
parent
221673f702
commit
01377f99e7
@ -8,8 +8,8 @@
|
||||
#include <string.h>
|
||||
|
||||
/**
|
||||
* @brief
|
||||
* @param c
|
||||
* @brief 使能所有电机
|
||||
* @param c 底盘结构体指针
|
||||
* @return
|
||||
*/
|
||||
static int8_t Chassis_MotorEnable(Chassis_t *c) {
|
||||
@ -25,18 +25,38 @@ static int8_t Chassis_MotorEnable(Chassis_t *c) {
|
||||
return CHASSIS_OK;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief 关闭所有电机
|
||||
* @param c 底盘结构体指针
|
||||
* @return
|
||||
*/
|
||||
static int8_t Chassis_MotorDisable(Chassis_t *c){
|
||||
if (c == NULL) return CHASSIS_ERR_NULL;
|
||||
|
||||
for (int i = 0; i < 2; i++) {
|
||||
MOTOR_LK_MotorOff(&c->param->wheel_motors[i]);
|
||||
}
|
||||
for (int i = 0; i < 4; i++) {
|
||||
MOTOR_LZ_Disable(&c->param->joint_motors[i], true);
|
||||
}
|
||||
|
||||
return CHASSIS_OK;
|
||||
}
|
||||
|
||||
static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) {
|
||||
if (c == NULL) return CHASSIS_ERR_NULL; /* 主结构体不能为空 */
|
||||
if (mode == c->mode) return CHASSIS_OK; /* 模式未改变直接返回 */
|
||||
|
||||
|
||||
Chassis_MotorEnable(c);
|
||||
// 清空pid积分
|
||||
PID_Reset(&c->pid.leglength[0]);
|
||||
PID_Reset(&c->pid.leglength[1]);
|
||||
|
||||
PID_Reset(&c->pid.leg_length[0]);
|
||||
PID_Reset(&c->pid.leg_length[1]);
|
||||
PID_Reset(&c->pid.yaw);
|
||||
PID_Reset(&c->pid.roll);
|
||||
PID_Reset(&c->pid.tp_pid[0]);
|
||||
PID_Reset(&c->pid.tp_pid[1]);
|
||||
PID_Reset(&c->pid.tp[0]);
|
||||
PID_Reset(&c->pid.tp[1]);
|
||||
|
||||
c->yaw_control.target_yaw = c->feedback.imu.euler.yaw;
|
||||
|
||||
@ -104,12 +124,12 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq){
|
||||
VMC_Init(&c->vmc_[1], ¶m->vmc_param[1], target_freq);
|
||||
|
||||
/*初始化pid*/
|
||||
PID_Init(&c->pid.leglength[0], KPID_MODE_CALC_D, target_freq, ¶m->leglength);
|
||||
PID_Init(&c->pid.leglength[1], KPID_MODE_CALC_D, target_freq, ¶m->leglength);
|
||||
PID_Init(&c->pid.leg_length[0], KPID_MODE_CALC_D, target_freq, ¶m->leg_length);
|
||||
PID_Init(&c->pid.leg_length[1], KPID_MODE_CALC_D, target_freq, ¶m->leg_length);
|
||||
PID_Init(&c->pid.yaw, KPID_MODE_CALC_D, target_freq, ¶m->yaw);
|
||||
PID_Init(&c->pid.roll, KPID_MODE_CALC_D, target_freq, ¶m->roll);
|
||||
PID_Init(&c->pid.tp_pid[0], KPID_MODE_CALC_D, target_freq, ¶m->tp_pid);
|
||||
PID_Init(&c->pid.tp_pid[1], KPID_MODE_CALC_D, target_freq, ¶m->tp_pid);
|
||||
PID_Init(&c->pid.tp[0], KPID_MODE_CALC_D, target_freq, ¶m->tp);
|
||||
PID_Init(&c->pid.tp[1], KPID_MODE_CALC_D, target_freq, ¶m->tp);
|
||||
|
||||
/*初始化LQR控制器*/
|
||||
LQR_Init(&c->lqr[0], param->lqr_param.max_wheel_torque, param->lqr_param.max_joint_torque);
|
||||
@ -207,31 +227,38 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd){
|
||||
VMC_ForwardSolve(&c->vmc_[1], c->feedback.joint[3].rotor_abs_angle, c->feedback.joint[2].rotor_abs_angle,
|
||||
c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
|
||||
|
||||
// VMC_InverseSolve(&c->vmc_[0], fn, tp);
|
||||
|
||||
// VMC_GetJointTorques(&c->vmc_[0], &t1, &t2);
|
||||
|
||||
// MOTOR_LZ_MotionControl(&c->param->joint_motors[0], &(MOTOR_LZ_MotionParam_t){.torque = t1});
|
||||
if (Chassis_LQRControl(c, c_cmd) != 0) {
|
||||
// LQR控制失败，切换到安全模式
|
||||
return CHASSIS_ERR;
|
||||
}
|
||||
Chassis_LQRControl(c, c_cmd);
|
||||
|
||||
break;
|
||||
|
||||
case CHASSIS_MODE_RECOVER:
|
||||
// 恢复模式，使用简单的关节位置控制回到初始姿态
|
||||
// TODO: 实现恢复逻辑
|
||||
{
|
||||
VMC_ForwardSolve(&c->vmc_[0], c->feedback.joint[0].rotor_abs_angle, c->feedback.joint[1].rotor_abs_angle,
|
||||
c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
|
||||
VMC_ForwardSolve(&c->vmc_[1], c->feedback.joint[3].rotor_abs_angle, c->feedback.joint[2].rotor_abs_angle,
|
||||
c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
|
||||
|
||||
float fn;
|
||||
float tp1, tp2;
|
||||
fn = -20.0f;
|
||||
|
||||
|
||||
tp1 = PID_Calc(&c->pid.tp[0], 0.0f, c->vmc_[0].leg.theta, c->vmc_[0].leg.d_theta, c->dt);
|
||||
tp2 = PID_Calc(&c->pid.tp[1], 0.0f, c->vmc_[1].leg.theta, c->vmc_[1].leg.d_theta, c->dt);
|
||||
|
||||
VMC_InverseSolve(&c->vmc_[0], fn, tp1);
|
||||
VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0].torque, &c->output.joint[1].torque);
|
||||
VMC_InverseSolve(&c->vmc_[1], fn, tp2);
|
||||
VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3].torque, &c->output.joint[2].torque);
|
||||
|
||||
c->output.wheel[0] = 0.0f;
|
||||
c->output.wheel[1] = 0.0f;
|
||||
Chassis_Output(c); // 统一输出
|
||||
}
|
||||
break;
|
||||
|
||||
case CHASSIS_MODE_WHELL_BALANCE:
|
||||
// 更新VMC正解算用于状态估计
|
||||
// MOTOR_LZ_Relax(&c->param->joint_motors[0]);
|
||||
// MOTOR_LZ_Relax(&c->param->joint_motors[1]);
|
||||
// MOTOR_LZ_Relax(&c->param->joint_motors[2]);
|
||||
// MOTOR_LZ_Relax(&c->param->joint_motors[3]);
|
||||
// MOTOR_LK_Relax(&c->param->wheel_motors[0]);
|
||||
// MOTOR_LK_Relax(&c->param->wheel_motors[1]);
|
||||
for (int i = 0; i < 4; i++) {
|
||||
c->output.joint[i].torque = 0.0f;
|
||||
}
|
||||
@ -330,8 +357,8 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
|
||||
xhat = c->chassis_state.position_x;
|
||||
|
||||
// 目标设定
|
||||
target_dot_x = c_cmd->move_vec.vx/3.0f;
|
||||
target_dot_x = SpeedLimit_TargetSpeed(1000.0f, c->chassis_state.velocity_x, target_dot_x, c->dt); // 速度限制器，假设最大加速度为1 m/s²
|
||||
target_dot_x = c_cmd->move_vec.vx;
|
||||
target_dot_x = SpeedLimit_TargetSpeed(300.0f, c->chassis_state.velocity_x, target_dot_x, c->dt); // 速度限制器，假设最大加速度为1 m/s²
|
||||
target_x += target_dot_x * c->dt;
|
||||
|
||||
/* 分别计算左右腿的LQR控制 */
|
||||
@ -405,8 +432,8 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
|
||||
|
||||
|
||||
/* 轮毂力矩输出（参考C++版本的减速比） */
|
||||
c->output.wheel[0] = Tw[0] / 5.0f - c->yaw_control.yaw_force;
|
||||
c->output.wheel[1] = Tw[1] / 5.0f + c->yaw_control.yaw_force;
|
||||
c->output.wheel[0] = Tw[0] / 4.5f - c->yaw_control.yaw_force;
|
||||
c->output.wheel[1] = Tw[1] / 4.5f + c->yaw_control.yaw_force;
|
||||
/* 腿长控制和VMC逆解算（使用PID控制） */
|
||||
float virtual_force[2];
|
||||
float target_L0[2];
|
||||
@ -427,13 +454,13 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
|
||||
float Delta_Tp[2];
|
||||
// 使用tp_pid进行左右腿摆角同步控制
|
||||
// 左腿补偿力矩：使左腿theta向右腿theta靠拢
|
||||
Delta_Tp[0] = -leg_L0[0] * 10.0f * PID_Calc(&c->pid.tp_pid[0], leg_theta[1], leg_theta[0], leg_d_theta[0], c->dt);
|
||||
Delta_Tp[0] = -leg_L0[0] * 10.0f * PID_Calc(&c->pid.tp[0], leg_theta[1], leg_theta[0], leg_d_theta[0], c->dt);
|
||||
// 右腿补偿力矩：使右腿theta向左腿theta靠拢（符号相反）
|
||||
Delta_Tp[1] = leg_L0[1] * 10.0f * PID_Calc(&c->pid.tp_pid[1], leg_theta[0], leg_theta[1], leg_d_theta[1], c->dt);
|
||||
Delta_Tp[1] = leg_L0[1] * 10.0f * PID_Calc(&c->pid.tp[1], leg_theta[0], leg_theta[1], leg_d_theta[1], c->dt);
|
||||
|
||||
for (int leg = 0; leg < 2; leg++) {
|
||||
// 使用PID进行腿长控制
|
||||
float pid_output = PID_Calc(&c->pid.leglength[leg], target_L0[leg], leg_L0[leg], leg_d_length[leg], c->dt);
|
||||
float pid_output = PID_Calc(&c->pid.leg_length[leg], target_L0[leg], leg_L0[leg], leg_d_length[leg], c->dt);
|
||||
|
||||
// 腿长控制力 = LQR摆杆力矩的径向分量 + PID腿长控制输出 + 基础支撑力
|
||||
virtual_force[leg] = (Tp[leg] + Delta_Tp[leg]) * sinf(leg_theta[leg]) +
|
||||
|
||||
@ -69,17 +69,11 @@ typedef struct {
|
||||
/* 底盘参数的结构体，包含所有初始化用的参数，通常是const，存好几组 */
|
||||
typedef struct {
|
||||
|
||||
VMC_Param_t vmc_param[2]; /* VMC参数 */
|
||||
KPID_Params_t leglength; /* 腿长PID控制参数，用于控制虚拟腿长度 */
|
||||
KPID_Params_t yaw; /* yaw轴PID控制参数，用于控制底盘朝向 */
|
||||
KPID_Params_t roll; /* roll轴PID控制参数，用于横滚角补偿 */
|
||||
KPID_Params_t tp_pid; /* 摆力矩PID控制参数，用于控制摆力矩 */
|
||||
|
||||
|
||||
MOTOR_LZ_Param_t joint_motors[4]; /* 四个关节电机参数 */
|
||||
MOTOR_LK_Param_t wheel_motors[2]; /* 两个轮子电机参数 */
|
||||
|
||||
float mech_zero_yaw; /* 机械零点 */
|
||||
VMC_Param_t vmc_param[2]; /* VMC参数 */
|
||||
LQR_GainMatrix_t lqr_gains; /* LQR增益矩阵参数 */
|
||||
|
||||
/* LQR控制器参数 */
|
||||
struct {
|
||||
@ -87,7 +81,13 @@ typedef struct {
|
||||
float max_joint_torque; /* 关节电机最大力矩限制 */
|
||||
} lqr_param;
|
||||
|
||||
LQR_GainMatrix_t lqr_gains; /* LQR增益矩阵参数 */
|
||||
KPID_Params_t yaw; /* yaw轴PID控制参数，用于控制底盘朝向 */
|
||||
KPID_Params_t roll; /* roll轴PID控制参数，用于横滚角补偿 */
|
||||
KPID_Params_t tp; /* 摆力矩PID控制参数，用于控制摆力矩 */
|
||||
KPID_Params_t leg_length; /* 腿长PID控制参数，用于控制虚拟腿长度 */
|
||||
KPID_Params_t leg_theta; /* 摆角PID控制参数，用于控制虚拟腿摆角 */
|
||||
|
||||
float mech_zero_yaw; /* 机械零点 */
|
||||
|
||||
/* 低通滤波器截止频率 */
|
||||
struct {
|
||||
@ -146,14 +146,11 @@ typedef struct {
|
||||
|
||||
/* 反馈控制用的PID */
|
||||
struct {
|
||||
KPID_t left_wheel; /* 左轮PID */
|
||||
KPID_t right_wheel; /* 右轮PID */
|
||||
KPID_t follow; /* 跟随云台用的PID */
|
||||
KPID_t balance; /* 平衡用的PID */
|
||||
KPID_t yaw; /* yaw轴控制PID */
|
||||
KPID_t yaw; /* 跟随云台用的PID */
|
||||
KPID_t roll; /* 横滚角控制PID */
|
||||
KPID_t tp_pid[2];
|
||||
KPID_t leglength[2]; /* 两条腿的腿长PID */
|
||||
KPID_t tp[2];
|
||||
KPID_t leg_length[2]; /* 两条腿的腿长PID */
|
||||
KPID_t leg_theta[2]; /* 两条腿的摆角PID */
|
||||
} pid;
|
||||
|
||||
/* 滤波器 */
|
||||
|
||||
@ -68,16 +68,6 @@ Config_RobotParam_t robot_config = {
|
||||
},
|
||||
|
||||
.chassis_param = {
|
||||
.leglength={
|
||||
.k = 20.0f,
|
||||
.p = 1.0f,
|
||||
.i = 0.0f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 0.0f,
|
||||
.out_limit = 100.0f,
|
||||
.d_cutoff_freq = -1.0f,
|
||||
.range = -1.0f,
|
||||
},
|
||||
.yaw={
|
||||
.k=1.0f,
|
||||
.p=1.0f,
|
||||
@ -88,6 +78,7 @@ Config_RobotParam_t robot_config = {
|
||||
.d_cutoff_freq=30.0f,
|
||||
.range=M_2PI, /* 2*PI，用于处理角度循环误差 */
|
||||
},
|
||||
|
||||
.roll={
|
||||
.k=1.0f,
|
||||
.p=5.0f, /* 横滚角比例系数 */
|
||||
@ -99,7 +90,28 @@ Config_RobotParam_t robot_config = {
|
||||
.range=-1.0f, /* 不使用循环误差处理 */
|
||||
},
|
||||
|
||||
.tp_pid={
|
||||
.leg_length={
|
||||
.k = 20.0f,
|
||||
.p = 1.0f,
|
||||
.i = 0.0f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 0.0f,
|
||||
.out_limit = 100.0f,
|
||||
.d_cutoff_freq = -1.0f,
|
||||
.range = -1.0f,
|
||||
},
|
||||
.leg_theta={
|
||||
.k=1.0f,
|
||||
.p=5.0f, /* 摆角比例系数 */
|
||||
.i=0.0f, /* 摆角积分系数 */
|
||||
.d=0.0f, /* 摆角微分系数 */
|
||||
.i_limit=0.0f, /* 积分限幅 */
|
||||
.out_limit=0.05f, /* 输出限幅，腿长差值限制 */
|
||||
.d_cutoff_freq=30.0f, /* 微分截止频率 */
|
||||
.range=-1.0f, /* 不使用循环误差处理 */
|
||||
},
|
||||
|
||||
.tp={
|
||||
.k=1.0f,
|
||||
.p=1.0f, /* 俯仰角比例系数 */
|
||||
.i=0.0f, /* 俯仰角积分系数 */
|
||||
@ -236,7 +248,7 @@ Config_RobotParam_t robot_config = {
|
||||
|
||||
},
|
||||
.lqr_param.max_joint_torque = 20.0f, // 关节电机最大力矩 20Nm
|
||||
.lqr_param.max_wheel_torque = 2.5f, // 轮毂电机最大力矩 2.5Nm
|
||||
.lqr_param.max_wheel_torque = 4.5f, // 轮毂电机最大力矩 2.5Nm
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
@ -4,7 +4,7 @@
|
||||
#include <tuple>
|
||||
|
||||
using namespace Module;
|
||||
Wheelleg::Wheelleg(Param& param)
|
||||
Wheelleg::Wheelleg(Param ¶m)
|
||||
: param_(param),
|
||||
roll_pid_(param.roll_pid_param, 333.0f),
|
||||
yaw_pid_(param.yaw_pid_param, 333.0f),
|
||||
@ -13,7 +13,8 @@ Wheelleg::Wheelleg(Param& param)
|
||||
acclfilter_(333.0f, 30.0f),
|
||||
manfilter_(param.manfilter_param),
|
||||
|
||||
ctrl_lock_(true) {
|
||||
ctrl_lock_(true)
|
||||
{
|
||||
memset(&(this->cmd_), 0, sizeof(this->cmd_));
|
||||
|
||||
this->hip_motor_.at(0) =
|
||||
@ -46,9 +47,11 @@ Wheelleg::Wheelleg(Param& param)
|
||||
this->leg_.at(0) = new Component::VMC(param_.leg_param.at(0), 333.0f);
|
||||
this->leg_.at(1) = new Component::VMC(param_.leg_param.at(1), 333.0f);
|
||||
|
||||
auto event_callback = [](Wheelleg::ChassisEvent event, Wheelleg* chassis) {
|
||||
auto event_callback = [](Wheelleg::ChassisEvent event, Wheelleg *chassis)
|
||||
{
|
||||
chassis->ctrl_lock_.Wait(UINT32_MAX);
|
||||
switch (event) {
|
||||
switch (event)
|
||||
{
|
||||
case SET_MODE_RELAX:
|
||||
chassis->SetMode(RELAX);
|
||||
break;
|
||||
@ -67,10 +70,11 @@ Wheelleg::Wheelleg(Param& param)
|
||||
chassis->ctrl_lock_.Post();
|
||||
};
|
||||
|
||||
Component::CMD::RegisterEvent<Wheelleg*, ChassisEvent>(
|
||||
Component::CMD::RegisterEvent<Wheelleg *, ChassisEvent>(
|
||||
event_callback, this, this->param_.EVENT_MAP);
|
||||
|
||||
auto chassis_thread = [](Wheelleg* chassis) {
|
||||
auto chassis_thread = [](Wheelleg *chassis)
|
||||
{
|
||||
auto cmd_sub =
|
||||
Message::Subscriber<Component::CMD::ChassisCMD>("cmd_chassis");
|
||||
auto eulr_sub =
|
||||
@ -85,7 +89,8 @@ Wheelleg::Wheelleg(Param& param)
|
||||
// auto yaw_sub = Message::Subscriber<float>("chassis_yaw");
|
||||
uint32_t last_online_time = bsp_time_get_ms();
|
||||
|
||||
while (1) {
|
||||
while (1)
|
||||
{
|
||||
eulr_sub.DumpData(chassis->eulr_); /* imu */
|
||||
gyro_sub.DumpData(chassis->gyro_); /* imu */
|
||||
accl_sub.DumpData(chassis->accl_);
|
||||
@ -109,8 +114,10 @@ Wheelleg::Wheelleg(Param& param)
|
||||
System::Thread::MEDIUM);
|
||||
}
|
||||
|
||||
void Wheelleg::MotorSetAble() {
|
||||
if (this->hip_motor_flag_ == 0) {
|
||||
void Wheelleg::MotorSetAble()
|
||||
{
|
||||
if (this->hip_motor_flag_ == 0)
|
||||
{
|
||||
this->hip_motor_[0]->Relax();
|
||||
this->hip_motor_[1]->Relax();
|
||||
this->hip_motor_[2]->Relax();
|
||||
@ -118,18 +125,24 @@ void Wheelleg::MotorSetAble() {
|
||||
this->dm_motor_flag_ = 0;
|
||||
}
|
||||
|
||||
else {
|
||||
if (this->dm_motor_flag_ == 0) {
|
||||
for (int i = 0; i < 5; i++) {
|
||||
else
|
||||
{
|
||||
if (this->dm_motor_flag_ == 0)
|
||||
{
|
||||
for (int i = 0; i < 5; i++)
|
||||
{
|
||||
this->hip_motor_[0]->Enable();
|
||||
}
|
||||
for (int i = 0; i < 5; i++) {
|
||||
for (int i = 0; i < 5; i++)
|
||||
{
|
||||
this->hip_motor_[1]->Enable();
|
||||
}
|
||||
for (int i = 0; i < 5; i++) {
|
||||
for (int i = 0; i < 5; i++)
|
||||
{
|
||||
this->hip_motor_[2]->Enable();
|
||||
}
|
||||
for (int i = 0; i < 5; i++) {
|
||||
for (int i = 0; i < 5; i++)
|
||||
{
|
||||
this->hip_motor_[3]->Enable();
|
||||
}
|
||||
|
||||
@ -138,7 +151,8 @@ void Wheelleg::MotorSetAble() {
|
||||
};
|
||||
}
|
||||
|
||||
void Wheelleg::UpdateFeedback() {
|
||||
void Wheelleg::UpdateFeedback()
|
||||
{
|
||||
this->now_ = bsp_time_get();
|
||||
this->dt_ = TIME_DIFF(this->last_wakeup_, this->now_);
|
||||
this->last_wakeup_ = this->now_;
|
||||
@ -156,19 +170,22 @@ void Wheelleg::UpdateFeedback() {
|
||||
this->leg_argu_[1].phi4_ =
|
||||
(-this->hip_motor_[2]->GetAngle() +
|
||||
this->param_.wheel_param.mech_zero[3]); // 前关节角度
|
||||
if (leg_argu_[1].phi4_ > M_PI) {
|
||||
if (leg_argu_[1].phi4_ > M_PI)
|
||||
{
|
||||
this->leg_argu_[1].phi4_ = this->leg_argu_[1].phi4_ - 2 * M_PI;
|
||||
}
|
||||
|
||||
this->leg_argu_[1].phi1_ =
|
||||
(-this->hip_motor_[3]->GetAngle() +
|
||||
this->param_.wheel_param.mech_zero[2]); // 后关节角度
|
||||
if (leg_argu_[1].phi1_ > M_PI) {
|
||||
if (leg_argu_[1].phi1_ > M_PI)
|
||||
{
|
||||
this->leg_argu_[1].phi1_ = this->leg_argu_[1].phi1_ - 2 * M_PI;
|
||||
}
|
||||
|
||||
this->pit_ = this->eulr_.pit;
|
||||
if (this->pit_ > M_PI) {
|
||||
if (this->pit_ > M_PI)
|
||||
{
|
||||
this->pit_ = this->eulr_.pit - 2 * M_PI;
|
||||
}
|
||||
|
||||
@ -181,10 +198,12 @@ void Wheelleg::UpdateFeedback() {
|
||||
this->leg_argu_[0].theta = -std::get<2>(result0);
|
||||
this->leg_argu_[0].d_theta = -std::get<3>(result0);
|
||||
|
||||
if (leg_argu_[0].theta > M_PI) {
|
||||
if (leg_argu_[0].theta > M_PI)
|
||||
{
|
||||
leg_argu_[0].theta = leg_argu_[0].theta - 2 * M_PI;
|
||||
}
|
||||
if (leg_argu_[0].theta < -M_PI) {
|
||||
if (leg_argu_[0].theta < -M_PI)
|
||||
{
|
||||
leg_argu_[0].theta = leg_argu_[0].theta + 2 * M_PI;
|
||||
}
|
||||
|
||||
@ -196,10 +215,12 @@ void Wheelleg::UpdateFeedback() {
|
||||
this->leg_argu_[1].theta = -std::get<2>(result1);
|
||||
this->leg_argu_[1].d_theta = -std::get<3>(result1);
|
||||
|
||||
if (leg_argu_[1].theta > M_PI) {
|
||||
if (leg_argu_[1].theta > M_PI)
|
||||
{
|
||||
leg_argu_[1].theta = leg_argu_[1].theta - 2 * M_PI;
|
||||
}
|
||||
if (leg_argu_[1].theta < -M_PI) {
|
||||
if (leg_argu_[1].theta < -M_PI)
|
||||
{
|
||||
leg_argu_[1].theta = leg_argu_[1].theta + 2 * M_PI;
|
||||
}
|
||||
|
||||
@ -230,7 +251,8 @@ void Wheelleg::UpdateFeedback() {
|
||||
|
||||
this->move_argu_.accl = acclfilter_.Apply(this->accl_.y * 9.8f);
|
||||
|
||||
if (now_ > 1000000) {
|
||||
if (now_ > 1000000)
|
||||
{
|
||||
this->move_argu_.x_dot_hat =
|
||||
manfilter_.comp_filter(move_argu_.x_dot, move_argu_.delta_speed,
|
||||
this->move_argu_.last_x_dot, accl_.y * 9.8f,
|
||||
@ -244,8 +266,10 @@ void Wheelleg::UpdateFeedback() {
|
||||
}
|
||||
}
|
||||
|
||||
void Wheelleg::Calculate() {
|
||||
switch (this->mode_) {
|
||||
void Wheelleg::Calculate()
|
||||
{
|
||||
switch (this->mode_)
|
||||
{
|
||||
case Wheelleg::RELAX:
|
||||
// if (fabs(move_argu_.target_dot_x - cmd_.y * 1.5f) > 0.005) {
|
||||
// if (move_argu_.target_dot_x > cmd_.y * 1.5f) {
|
||||
@ -266,16 +290,21 @@ void Wheelleg::Calculate() {
|
||||
|
||||
/* 0.002为最大加速度 即500hz*0.002 梯度式递增减 */
|
||||
if (fabs(move_argu_.target_dot_x -
|
||||
cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) > 0.005) {
|
||||
cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) > 0.005)
|
||||
{
|
||||
if (move_argu_.target_dot_x >
|
||||
cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) {
|
||||
cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed)
|
||||
{
|
||||
move_argu_.target_dot_x -= 0.004;
|
||||
}
|
||||
if (move_argu_.target_dot_x <
|
||||
cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) {
|
||||
cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed)
|
||||
{
|
||||
move_argu_.target_dot_x += 0.004;
|
||||
}
|
||||
} else {
|
||||
}
|
||||
else
|
||||
{
|
||||
move_argu_.target_dot_x =
|
||||
cosf(yaw_) * cmd_.y * param_.wheel_param.max_speed;
|
||||
}
|
||||
@ -284,26 +313,33 @@ void Wheelleg::Calculate() {
|
||||
this->move_argu_.target_yaw = 0.0f;
|
||||
|
||||
/*双零点*/
|
||||
if (this->yaw_ > M_PI_2) {
|
||||
if (this->yaw_ > M_PI_2)
|
||||
{
|
||||
this->move_argu_.target_yaw = 3.14158f;
|
||||
}
|
||||
if (this->yaw_ < -M_PI_2) {
|
||||
if (this->yaw_ < -M_PI_2)
|
||||
{
|
||||
this->move_argu_.target_yaw = 3.14158f;
|
||||
}
|
||||
break;
|
||||
|
||||
case Wheelleg::ROTOR:
|
||||
if (fabs(move_argu_.target_dot_x -
|
||||
cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) > 0.005) {
|
||||
cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) > 0.005)
|
||||
{
|
||||
if (move_argu_.target_dot_x >
|
||||
cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) {
|
||||
cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed)
|
||||
{
|
||||
move_argu_.target_dot_x -= 0.004;
|
||||
}
|
||||
if (move_argu_.target_dot_x <
|
||||
cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) {
|
||||
cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed)
|
||||
{
|
||||
move_argu_.target_dot_x += 0.004;
|
||||
}
|
||||
} else {
|
||||
}
|
||||
else
|
||||
{
|
||||
move_argu_.target_dot_x =
|
||||
cosf(yaw_) * cmd_.y * param_.wheel_param.max_speed;
|
||||
}
|
||||
@ -340,29 +376,34 @@ void Wheelleg::Calculate() {
|
||||
|
||||
leg_argu_[0].Fn = leg_[0]->GndDetector(leg_argu_[0].Tp, leg_argu_[0].F0);
|
||||
onground0_flag_ = false;
|
||||
if (leg_argu_[0].Fn > 30) {
|
||||
if (leg_argu_[0].Fn > 30)
|
||||
{
|
||||
onground0_flag_ = true;
|
||||
}
|
||||
leg_argu_[1].Fn = leg_[1]->GndDetector(leg_argu_[1].Tp, leg_argu_[1].F0);
|
||||
onground1_flag_ = false;
|
||||
if (leg_argu_[1].Fn > 30) {
|
||||
if (leg_argu_[1].Fn > 30)
|
||||
{
|
||||
onground1_flag_ = true;
|
||||
}
|
||||
std::tuple<float, float> result3;
|
||||
std::tuple<float, float> result4;
|
||||
|
||||
switch (this->mode_) {
|
||||
switch (this->mode_)
|
||||
{
|
||||
case Wheelleg::RELAX:
|
||||
case Wheelleg::ROTOR:
|
||||
case Wheelleg::STAND:
|
||||
|
||||
for (int i = 0; i < 12; i++) {
|
||||
for (int i = 0; i < 12; i++)
|
||||
{
|
||||
leg_argu_[0].LQR_K[i] = this->leg_[0]->LQR_K_calc(
|
||||
&this->param_.wheel_param.K_Poly_Coefficient_L[i][0],
|
||||
leg_argu_[0].L0);
|
||||
}
|
||||
|
||||
for (int i = 0; i < 12; i++) {
|
||||
for (int i = 0; i < 12; i++)
|
||||
{
|
||||
leg_argu_[1].LQR_K[i] = this->leg_[1]->LQR_K_calc(
|
||||
&this->param_.wheel_param.K_Poly_Coefficient_R[i][0],
|
||||
leg_argu_[1].L0);
|
||||
@ -370,14 +411,16 @@ void Wheelleg::Calculate() {
|
||||
if (now_ > 1000000)
|
||||
if (fabs(move_argu_.target_dot_x) > 0.5 ||
|
||||
fabs(move_argu_.target_dot_x - move_argu_.x_dot_hat) > 0.7 ||
|
||||
((!onground0_flag_) & (!onground1_flag_))) {
|
||||
((!onground0_flag_) & (!onground1_flag_)))
|
||||
{
|
||||
leg_argu_[0].LQR_K[2] = 0;
|
||||
leg_argu_[1].LQR_K[2] = 0;
|
||||
this->move_argu_.xhat = 0;
|
||||
move_argu_.target_x = 0;
|
||||
}
|
||||
|
||||
if (onground0_flag_) {
|
||||
if (onground0_flag_)
|
||||
{
|
||||
leg_argu_[0].Tw =
|
||||
(leg_argu_[0].LQR_K[0] *
|
||||
(-0.8845 * leg_argu_[0].L0 + 0.40663 - leg_argu_[0].theta) +
|
||||
@ -398,14 +441,17 @@ void Wheelleg::Calculate() {
|
||||
(-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
|
||||
leg_argu_[0].LQR_K[10] * (-this->pit_ + 0.16) +
|
||||
leg_argu_[0].LQR_K[11] * (-this->gyro_.x + 0.0f));
|
||||
} else {
|
||||
}
|
||||
else
|
||||
{
|
||||
leg_argu_[0].Tw = 0;
|
||||
|
||||
leg_argu_[0].Tp =
|
||||
(leg_argu_[0].LQR_K[6] * (-leg_argu_[0].theta + 0.0f) +
|
||||
leg_argu_[0].LQR_K[7] * (-leg_argu_[0].d_theta + 0.0f));
|
||||
}
|
||||
if (onground1_flag_) {
|
||||
if (onground1_flag_)
|
||||
{
|
||||
leg_argu_[1].Tw =
|
||||
(leg_argu_[1].LQR_K[0] *
|
||||
(-0.8845 * leg_argu_[1].L0 + 0.40663 - leg_argu_[1].theta) +
|
||||
@ -426,7 +472,9 @@ void Wheelleg::Calculate() {
|
||||
(-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
|
||||
leg_argu_[1].LQR_K[10] * (-this->pit_ + 0.16) +
|
||||
leg_argu_[1].LQR_K[11] * (-this->gyro_.x + 0.0f));
|
||||
} else {
|
||||
}
|
||||
else
|
||||
{
|
||||
leg_argu_[1].Tw = 0;
|
||||
leg_argu_[1].Tp =
|
||||
(leg_argu_[1].LQR_K[6] * (-leg_argu_[1].theta + 0.0f) +
|
||||
@ -472,10 +520,13 @@ void Wheelleg::Calculate() {
|
||||
this->leg_argu_[1].T1 = -std::get<0>(result4);
|
||||
this->leg_argu_[1].T2 = -std::get<1>(result4);
|
||||
|
||||
if (onground0_flag_ & onground1_flag_) {
|
||||
if (onground0_flag_ & onground1_flag_)
|
||||
{
|
||||
move_argu_.yaw_force =
|
||||
-this->yaw_pid_.Calculate(move_argu_.target_yaw, this->yaw_, dt_);
|
||||
} else {
|
||||
}
|
||||
else
|
||||
{
|
||||
move_argu_.yaw_force = 0;
|
||||
}
|
||||
/*3508不带减速箱是Tw*3.2f*/
|
||||
@ -498,14 +549,18 @@ void Wheelleg::Calculate() {
|
||||
|
||||
case Wheelleg::RESET:
|
||||
if (fabs(pit_) > M_PI / 2 || fabs(leg_argu_[0].theta) > 1.57 ||
|
||||
fabs(leg_argu_[1].theta) > 1.57) {
|
||||
fabs(leg_argu_[1].theta) > 1.57)
|
||||
{
|
||||
leg_argu_[0].target_theta = leg_argu_[0].theta + cmd_.y / 1000;
|
||||
|
||||
if (fabs(pit_) > M_PI / 2) {
|
||||
if ((leg_argu_[0].theta - leg_argu_[1].theta) > 0.03) {
|
||||
if (fabs(pit_) > M_PI / 2)
|
||||
{
|
||||
if ((leg_argu_[0].theta - leg_argu_[1].theta) > 0.03)
|
||||
{
|
||||
leg_argu_[1].target_theta = leg_argu_[1].theta + 0.001;
|
||||
}
|
||||
if ((leg_argu_[0].theta - leg_argu_[1].theta) < -0.03) {
|
||||
if ((leg_argu_[0].theta - leg_argu_[1].theta) < -0.03)
|
||||
{
|
||||
leg_argu_[1].target_theta = leg_argu_[1].theta - 0.001;
|
||||
}
|
||||
leg_argu_[0].F0 = 10 * leglength_pid_.at(0)->Calculate(
|
||||
@ -513,26 +568,32 @@ void Wheelleg::Calculate() {
|
||||
leg_argu_[1].F0 = 10 * leglength_pid_.at(1)->Calculate(
|
||||
0.65f, this->leg_argu_[1].L0, this->dt_);
|
||||
}
|
||||
if (fabs(leg_argu_[0].theta) < 1.57) {
|
||||
if (fabs(leg_argu_[0].theta) < 1.57)
|
||||
{
|
||||
leg_argu_[1].target_theta = leg_argu_[1].theta + cmd_.y / 1000;
|
||||
leg_argu_[0].target_theta = leg_argu_[0].theta;
|
||||
}
|
||||
|
||||
if (fabs(leg_argu_[1].theta) < 1.57) {
|
||||
if (fabs(leg_argu_[1].theta) < 1.57)
|
||||
{
|
||||
leg_argu_[0].target_theta = leg_argu_[0].theta + cmd_.y / 1000;
|
||||
leg_argu_[1].target_theta = leg_argu_[1].theta;
|
||||
}
|
||||
|
||||
if (leg_argu_[0].target_theta > M_PI) {
|
||||
if (leg_argu_[0].target_theta > M_PI)
|
||||
{
|
||||
leg_argu_[0].target_theta -= 2 * M_PI;
|
||||
}
|
||||
if (leg_argu_[0].target_theta < -M_PI) {
|
||||
if (leg_argu_[0].target_theta < -M_PI)
|
||||
{
|
||||
leg_argu_[0].target_theta += 2 * M_PI;
|
||||
}
|
||||
if (leg_argu_[1].target_theta > M_PI) {
|
||||
if (leg_argu_[1].target_theta > M_PI)
|
||||
{
|
||||
leg_argu_[1].target_theta -= 2 * M_PI;
|
||||
}
|
||||
if (leg_argu_[1].target_theta < -M_PI) {
|
||||
if (leg_argu_[1].target_theta < -M_PI)
|
||||
{
|
||||
leg_argu_[1].target_theta += 2 * M_PI;
|
||||
}
|
||||
leg_argu_[0].Tp =
|
||||
@ -541,21 +602,25 @@ void Wheelleg::Calculate() {
|
||||
leg_argu_[1].Tp =
|
||||
500 * this->theta_pid_[1]->Calculate(leg_argu_[1].target_theta,
|
||||
leg_argu_[1].theta, dt_);
|
||||
|
||||
} else {
|
||||
}
|
||||
else
|
||||
{
|
||||
leg_argu_[0].F0 = 3 * leglength_pid_.at(0)->Calculate(
|
||||
0.10f, this->leg_argu_[0].L0, this->dt_);
|
||||
leg_argu_[1].F0 = 3 * leglength_pid_.at(1)->Calculate(
|
||||
0.10f, this->leg_argu_[1].L0, this->dt_);
|
||||
|
||||
if ((this->leg_argu_[0].L0 < 0.20) && (this->leg_argu_[1].L0 < 0.20)) {
|
||||
if ((this->leg_argu_[0].L0 < 0.20) && (this->leg_argu_[1].L0 < 0.20))
|
||||
{
|
||||
leg_argu_[0].Tp = 5.5 * this->theta_pid_[0]->Calculate(
|
||||
0.1, leg_argu_[0].theta, dt_);
|
||||
leg_argu_[1].Tp = 5.5 * this->theta_pid_[1]->Calculate(
|
||||
0.1, leg_argu_[1].theta, dt_);
|
||||
clampf(&leg_argu_[0].Tp, -10, 10);
|
||||
clampf(&leg_argu_[1].Tp, -10, 10);
|
||||
} else {
|
||||
}
|
||||
else
|
||||
{
|
||||
leg_argu_[0].Tp = 0;
|
||||
leg_argu_[1].Tp = 0;
|
||||
}
|
||||
@ -581,7 +646,8 @@ void Wheelleg::Calculate() {
|
||||
}
|
||||
}
|
||||
|
||||
void Wheelleg::Control() {
|
||||
void Wheelleg::Control()
|
||||
{
|
||||
clampf(&wheel_motor_out_[0], -0.8f, 0.8f);
|
||||
clampf(&wheel_motor_out_[1], -0.8f, 0.8f);
|
||||
clampf(&hip_motor_out_[0], -20.0f, 20.0f);
|
||||
@ -599,7 +665,8 @@ void Wheelleg::Control() {
|
||||
// }
|
||||
// }
|
||||
|
||||
switch (this->mode_) {
|
||||
switch (this->mode_)
|
||||
{
|
||||
case Wheelleg::RELAX:
|
||||
|
||||
this->wheel_motor_[0]->Relax();
|
||||
@ -642,8 +709,10 @@ void Wheelleg::Control() {
|
||||
}
|
||||
}
|
||||
|
||||
void Wheelleg::SetMode(Wheelleg::Mode mode) {
|
||||
if (mode == this->mode_) {
|
||||
void Wheelleg::SetMode(Wheelleg::Mode mode)
|
||||
{
|
||||
if (mode == this->mode_)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
@ -46,7 +46,7 @@ void Task_rc(void *argument) {
|
||||
cmd_to_chassis.mode = CHASSIS_MODE_RELAX;
|
||||
break;
|
||||
case 3: // 中位
|
||||
cmd_to_chassis.mode = CHASSIS_MODE_RELAX;
|
||||
cmd_to_chassis.mode = CHASSIS_MODE_RECOVER;
|
||||
break;
|
||||
case 2: // 下位
|
||||
cmd_to_chassis.mode = CHASSIS_MODE_WHELL_BALANCE;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user