robofish/rm_balance
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

收腿

Browse Source
This commit is contained in:
Robofish 2025-09-18 23:05:44 +08:00
parent 221673f702
commit 01377f99e7
5 changed files with 548 additions and 443 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

99
User/module/balance_chassis.c
View File

@ -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], &param->vmc_param[1], target_freq);
/*初始化pid*/
PID_Init(&c->pid.leglength[0], KPID_MODE_CALC_D, target_freq, &param->leglength);
PID_Init(&c->pid.leglength[1], KPID_MODE_CALC_D, target_freq, &param->leglength);
PID_Init(&c->pid.leg_length[0], KPID_MODE_CALC_D, target_freq, &param->leg_length);
PID_Init(&c->pid.leg_length[1], KPID_MODE_CALC_D, target_freq, &param->leg_length);
PID_Init(&c->pid.yaw, KPID_MODE_CALC_D, target_freq, &param->yaw);
PID_Init(&c->pid.roll, KPID_MODE_CALC_D, target_freq, &param->roll);
PID_Init(&c->pid.tp_pid[0], KPID_MODE_CALC_D, target_freq, &param->tp_pid);
PID_Init(&c->pid.tp_pid[1], KPID_MODE_CALC_D, target_freq, &param->tp_pid);
PID_Init(&c->pid.tp[0], KPID_MODE_CALC_D, target_freq, &param->tp);
PID_Init(&c->pid.tp[1], KPID_MODE_CALC_D, target_freq, &param->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: 实现恢复逻辑
break;
{
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]) +

29
User/module/balance_chassis.h
View File

@ -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;
/* 滤波器 */

36
User/module/config.c
View File

@ -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
}
};

823
User/module/mod_wheelleg_chassis.cpp
View File

@ -4,7 +4,7 @@
#include <tuple>
using namespace Module;
Wheelleg::Wheelleg(Param& param)
Wheelleg::Wheelleg(Param &param)
: 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,31 +47,34 @@ 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) {
case SET_MODE_RELAX:
chassis->SetMode(RELAX);
break;
case SET_MODE_STAND:
chassis->SetMode(STAND);
break;
case SET_MODE_ROTOR:
chassis->SetMode(ROTOR);
break;
case SET_MODE_RESET:
chassis->SetMode(RESET);
break;
default:
break;
switch (event)
{
case SET_MODE_RELAX:
chassis->SetMode(RELAX);
break;
case SET_MODE_STAND:
chassis->SetMode(STAND);
break;
case SET_MODE_ROTOR:
chassis->SetMode(ROTOR);
break;
case SET_MODE_RESET:
chassis->SetMode(RESET);
break;
default:
break;
}
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_;
@ -148,27 +162,30 @@ void Wheelleg::UpdateFeedback() {
this->leg_argu_[0].phi4_ =
this->hip_motor_[0]->GetAngle() -
this->param_.wheel_param.mech_zero[0]; // 前关节角度
this->param_.wheel_param.mech_zero[0]; // 前关节角度
this->leg_argu_[0].phi1_ =
this->hip_motor_[1]->GetAngle() -
this->param_.wheel_param.mech_zero[1]; // 后关节角度
this->param_.wheel_param.mech_zero[1]; // 后关节角度
this->leg_argu_[1].phi4_ =
(-this->hip_motor_[2]->GetAngle() +
this->param_.wheel_param.mech_zero[3]); // 前关节角度
if (leg_argu_[1].phi4_ > M_PI) {
this->param_.wheel_param.mech_zero[3]); // 前关节角度
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) {
this->param_.wheel_param.mech_zero[2]); // 后关节角度
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,344 +266,388 @@ void Wheelleg::UpdateFeedback() {
}
}
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) {
// move_argu_.target_dot_x -= 0.004;
// }
// if (move_argu_.target_dot_x < cmd_.y * 1.5f) {
// move_argu_.target_dot_x += 0.004;
// }
// } else {
// move_argu_.target_dot_x = cmd_.y * 1.5f;
// }
// move_argu_.target_x = move_argu_.target_dot_x * dt_ +
// move_argu_.target_x;
move_argu_.target_x = 0;
move_argu_.target_dot_x = 0;
break;
case Wheelleg::STAND:
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) {
// move_argu_.target_dot_x -= 0.004;
// }
// if (move_argu_.target_dot_x < cmd_.y * 1.5f) {
// move_argu_.target_dot_x += 0.004;
// }
// } else {
// move_argu_.target_dot_x = cmd_.y * 1.5f;
// }
// move_argu_.target_x = move_argu_.target_dot_x * dt_ +
// move_argu_.target_x;
move_argu_.target_x = 0;
move_argu_.target_dot_x = 0;
break;
case Wheelleg::STAND:
/* 0.002为最大加速度 即500hz*0.002 梯度式递增减 */
if (fabs(move_argu_.target_dot_x -
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) {
move_argu_.target_dot_x -= 0.004;
}
if (move_argu_.target_dot_x <
cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) {
move_argu_.target_dot_x += 0.004;
}
} else {
move_argu_.target_dot_x =
cosf(yaw_) * cmd_.y * param_.wheel_param.max_speed;
/* 0.002为最大加速度 即500hz*0.002 梯度式递增减 */
if (fabs(move_argu_.target_dot_x -
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)
{
move_argu_.target_dot_x -= 0.004;
}
move_argu_.target_x = move_argu_.target_dot_x * dt_ + move_argu_.target_x;
this->move_argu_.target_yaw = 0.0f;
/*双零点*/
if (this->yaw_ > M_PI_2) {
this->move_argu_.target_yaw = 3.14158f;
if (move_argu_.target_dot_x <
cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed)
{
move_argu_.target_dot_x += 0.004;
}
if (this->yaw_ < -M_PI_2) {
this->move_argu_.target_yaw = 3.14158f;
}
else
{
move_argu_.target_dot_x =
cosf(yaw_) * cmd_.y * param_.wheel_param.max_speed;
}
move_argu_.target_x = move_argu_.target_dot_x * dt_ + move_argu_.target_x;
this->move_argu_.target_yaw = 0.0f;
/*双零点*/
if (this->yaw_ > M_PI_2)
{
this->move_argu_.target_yaw = 3.14158f;
}
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)
{
if (move_argu_.target_dot_x >
cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed)
{
move_argu_.target_dot_x -= 0.004;
}
break;
case Wheelleg::ROTOR:
if (fabs(move_argu_.target_dot_x -
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) {
move_argu_.target_dot_x -= 0.004;
}
if (move_argu_.target_dot_x <
cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) {
move_argu_.target_dot_x += 0.004;
}
} else {
move_argu_.target_dot_x =
cosf(yaw_) * cmd_.y * param_.wheel_param.max_speed;
if (move_argu_.target_dot_x <
cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed)
{
move_argu_.target_dot_x += 0.004;
}
move_argu_.target_x = move_argu_.target_dot_x * dt_ + move_argu_.target_x;
this->move_argu_.target_yaw = this->yaw_ + 0.01;
}
else
{
move_argu_.target_dot_x =
cosf(yaw_) * cmd_.y * param_.wheel_param.max_speed;
}
move_argu_.target_x = move_argu_.target_dot_x * dt_ + move_argu_.target_x;
this->move_argu_.target_yaw = this->yaw_ + 0.01;
break;
// move_argu_.target_dot_x = cmd_.x;
// move_argu_.target_x =
// move_argu_.target_dot_x * dt_ + move_argu_.target_dot_x;
// // move_argu_.target_dot_x = sqrtf(cmd_.x * cmd_.x + cmd_.y *
// cmd_.y);
// // move_argu_.x_dot =
// // move_argu_.target_dot_x * dt_ + move_argu_.target_dot_x;
// // move_argu_.target_yaw = atan2(cmd_.x, cmd_.y);
// break;
break;
// move_argu_.target_dot_x = cmd_.x;
// move_argu_.target_x =
// move_argu_.target_dot_x * dt_ + move_argu_.target_dot_x;
// // move_argu_.target_dot_x = sqrtf(cmd_.x * cmd_.x + cmd_.y *
// cmd_.y);
// // move_argu_.x_dot =
// // move_argu_.target_dot_x * dt_ + move_argu_.target_dot_x;
// // move_argu_.target_yaw = atan2(cmd_.x, cmd_.y);
// break;
case Wheelleg::RESET:
this->move_argu_.target_dot_x = 0;
move_argu_.target_x = 0;
this->move_argu_.target_yaw = this->eulr_.yaw;
this->move_argu_.xhat = 0;
case Wheelleg::RESET:
this->move_argu_.target_dot_x = 0;
move_argu_.target_x = 0;
this->move_argu_.target_yaw = this->eulr_.yaw;
this->move_argu_.xhat = 0;
// move_argu_.target_yaw - atan2(cmd_.x, cmd_.y);
// if ((fabs(move_argu_.target_yaw) - fabs(eulr_.yaw)) > M_PI / 2) {
// this->move_argu_.target_dot_x = -this->move_argu_.target_dot_x;
// }
break;
// move_argu_.target_yaw - atan2(cmd_.x, cmd_.y);
// if ((fabs(move_argu_.target_yaw) - fabs(eulr_.yaw)) > M_PI / 2) {
// this->move_argu_.target_dot_x = -this->move_argu_.target_dot_x;
// }
break;
default:
XB_ASSERT(false);
return;
default:
XB_ASSERT(false);
return;
}
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_) {
case Wheelleg::RELAX:
case Wheelleg::ROTOR:
case Wheelleg::STAND:
switch (this->mode_)
{
case Wheelleg::RELAX:
case Wheelleg::ROTOR:
case Wheelleg::STAND:
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++)
{
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++)
{
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);
}
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_)))
{
leg_argu_[0].LQR_K[2] = 0;
leg_argu_[1].LQR_K[2] = 0;
this->move_argu_.xhat = 0;
move_argu_.target_x = 0;
}
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);
}
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_))) {
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_)
{
leg_argu_[0].Tw =
(leg_argu_[0].LQR_K[0] *
(-0.8845 * leg_argu_[0].L0 + 0.40663 - leg_argu_[0].theta) +
leg_argu_[0].LQR_K[1] * (-leg_argu_[0].d_theta) +
leg_argu_[0].LQR_K[2] *
(-move_argu_.xhat + move_argu_.target_x - 0.56) +
leg_argu_[0].LQR_K[3] *
(-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
leg_argu_[0].LQR_K[4] * (-this->pit_ + 0.16) +
leg_argu_[0].LQR_K[5] * (-this->gyro_.x + 0.0f));
leg_argu_[0].Tp =
(leg_argu_[0].LQR_K[6] *
(-0.8845 * leg_argu_[0].L0 + 0.40663 - leg_argu_[0].theta) +
leg_argu_[0].LQR_K[7] * (-leg_argu_[0].d_theta) +
leg_argu_[0].LQR_K[8] *
(-move_argu_.xhat + move_argu_.target_x - 0.56) +
leg_argu_[0].LQR_K[9] *
(-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
{
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_)
{
leg_argu_[1].Tw =
(leg_argu_[1].LQR_K[0] *
(-0.8845 * leg_argu_[1].L0 + 0.40663 - leg_argu_[1].theta) +
leg_argu_[1].LQR_K[1] * (-leg_argu_[1].d_theta + 0.0f) +
leg_argu_[1].LQR_K[2] *
(-move_argu_.xhat + move_argu_.target_x - 0.56) +
leg_argu_[1].LQR_K[3] *
(-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
leg_argu_[1].LQR_K[4] * (-this->pit_ + 0.16) +
leg_argu_[1].LQR_K[5] * (-this->gyro_.x + 0.0f));
leg_argu_[1].Tp =
(leg_argu_[1].LQR_K[6] *
(-0.8845 * leg_argu_[1].L0 + 0.40663 - leg_argu_[1].theta) +
leg_argu_[1].LQR_K[7] * (-leg_argu_[1].d_theta + 0.0f) +
leg_argu_[1].LQR_K[8] *
(-move_argu_.xhat + move_argu_.target_x - 0.56) +
leg_argu_[1].LQR_K[9] *
(-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
{
leg_argu_[1].Tw = 0;
leg_argu_[1].Tp =
(leg_argu_[1].LQR_K[6] * (-leg_argu_[1].theta + 0.0f) +
leg_argu_[1].LQR_K[7] * (-leg_argu_[1].d_theta + 0.0f));
}
this->leg_argu_[0].Delat_L0 = fabs(0.35 * cmd_.z) - fabs(gyro_.z) / 100 +
this->param_.wheel_param.static_L0[0] +
+roll_pid_.Calculate(0, eulr_.rol, dt_);
this->leg_argu_[1].Delat_L0 = fabs(0.35 * cmd_.z) - fabs(gyro_.z) / 100 +
this->param_.wheel_param.static_L0[1] -
roll_pid_.Calculate(0, eulr_.rol, dt_);
leg_argu_[0].F0 =
leg_argu_[0].Tp * sinf(leg_argu_[0].theta) +
this->param_.wheel_param.static_F0[0] +
leglength_pid_.at(0)->Calculate(this->leg_argu_[0].Delat_L0,
this->leg_argu_[0].L0, this->dt_);
leg_argu_[1].F0 =
leg_argu_[1].Tp * sinf(leg_argu_[1].theta) +
this->param_.wheel_param.static_F0[1] +
leglength_pid_.at(1)->Calculate(this->leg_argu_[1].Delat_L0,
this->leg_argu_[1].L0, this->dt_);
this->leg_argu_[0].Delta_Tp =
leg_argu_[0].L0 * 10.0f *
tp_pid_.at(0)->Calculate(this->leg_argu_[1].theta,
this->leg_argu_[0].theta, this->dt_);
this->leg_argu_[1].Delta_Tp =
-leg_argu_[1].L0 * 10.0f *
tp_pid_.at(0)->Calculate(this->leg_argu_[1].theta,
this->leg_argu_[0].theta, this->dt_);
result3 = leg_[0]->VMCinserve(
-leg_argu_[0].phi1_, -leg_argu_[0].phi4_,
-leg_argu_[0].Tp - this->leg_argu_[0].Delta_Tp, leg_argu_[0].F0);
this->leg_argu_[0].T1 = std::get<0>(result3);
this->leg_argu_[0].T2 = std::get<1>(result3);
result4 = leg_[1]->VMCinserve(
-leg_argu_[1].phi1_, -leg_argu_[1].phi4_,
-leg_argu_[1].Tp - this->leg_argu_[1].Delta_Tp, leg_argu_[1].F0);
this->leg_argu_[1].T1 = -std::get<0>(result4);
this->leg_argu_[1].T2 = -std::get<1>(result4);
if (onground0_flag_ & onground1_flag_)
{
move_argu_.yaw_force =
-this->yaw_pid_.Calculate(move_argu_.target_yaw, this->yaw_, dt_);
}
else
{
move_argu_.yaw_force = 0;
}
/*3508不带减速箱是Tw*3.2f*/
/*2006带减速是Tw/1.8f*/
/* 3508带15.7减速箱是Tw/4.9256 */
this->wheel_motor_out_[0] =
this->leg_argu_[0].Tw / 4.9256f - move_argu_.yaw_force;
this->wheel_motor_out_[1] =
this->leg_argu_[1].Tw / 4.9256f + move_argu_.yaw_force;
this->hip_motor_out_[0] = this->leg_argu_[0].T1;
this->hip_motor_out_[1] = this->leg_argu_[0].T2;
this->hip_motor_out_[2] = this->leg_argu_[1].T1;
this->hip_motor_out_[3] = this->leg_argu_[1].T2;
this->hip_motor_flag_ = 1;
break;
case Wheelleg::RESET:
if (fabs(pit_) > M_PI / 2 || fabs(leg_argu_[0].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)
{
leg_argu_[1].target_theta = leg_argu_[1].theta + 0.001;
}
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) +
leg_argu_[0].LQR_K[1] * (-leg_argu_[0].d_theta) +
leg_argu_[0].LQR_K[2] *
(-move_argu_.xhat + move_argu_.target_x - 0.56) +
leg_argu_[0].LQR_K[3] *
(-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
leg_argu_[0].LQR_K[4] * (-this->pit_ + 0.16) +
leg_argu_[0].LQR_K[5] * (-this->gyro_.x + 0.0f));
leg_argu_[0].Tp =
(leg_argu_[0].LQR_K[6] *
(-0.8845 * leg_argu_[0].L0 + 0.40663 - leg_argu_[0].theta) +
leg_argu_[0].LQR_K[7] * (-leg_argu_[0].d_theta) +
leg_argu_[0].LQR_K[8] *
(-move_argu_.xhat + move_argu_.target_x - 0.56) +
leg_argu_[0].LQR_K[9] *
(-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 {
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 ((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(
0.65f, this->leg_argu_[0].L0, this->dt_);
leg_argu_[1].F0 = 10 * leglength_pid_.at(1)->Calculate(
0.65f, this->leg_argu_[1].L0, this->dt_);
}
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) +
leg_argu_[1].LQR_K[1] * (-leg_argu_[1].d_theta + 0.0f) +
leg_argu_[1].LQR_K[2] *
(-move_argu_.xhat + move_argu_.target_x - 0.56) +
leg_argu_[1].LQR_K[3] *
(-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
leg_argu_[1].LQR_K[4] * (-this->pit_ + 0.16) +
leg_argu_[1].LQR_K[5] * (-this->gyro_.x + 0.0f));
leg_argu_[1].Tp =
(leg_argu_[1].LQR_K[6] *
(-0.8845 * leg_argu_[1].L0 + 0.40663 - leg_argu_[1].theta) +
leg_argu_[1].LQR_K[7] * (-leg_argu_[1].d_theta + 0.0f) +
leg_argu_[1].LQR_K[8] *
(-move_argu_.xhat + move_argu_.target_x - 0.56) +
leg_argu_[1].LQR_K[9] *
(-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 {
leg_argu_[1].Tw = 0;
leg_argu_[1].Tp =
(leg_argu_[1].LQR_K[6] * (-leg_argu_[1].theta + 0.0f) +
leg_argu_[1].LQR_K[7] * (-leg_argu_[1].d_theta + 0.0f));
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;
}
this->leg_argu_[0].Delat_L0 = fabs(0.35 * cmd_.z) - fabs(gyro_.z) / 100 +
this->param_.wheel_param.static_L0[0] +
+roll_pid_.Calculate(0, eulr_.rol, dt_);
this->leg_argu_[1].Delat_L0 = fabs(0.35 * cmd_.z) - fabs(gyro_.z) / 100 +
this->param_.wheel_param.static_L0[1] -
roll_pid_.Calculate(0, eulr_.rol, dt_);
leg_argu_[0].F0 =
leg_argu_[0].Tp * sinf(leg_argu_[0].theta) +
this->param_.wheel_param.static_F0[0] +
leglength_pid_.at(0)->Calculate(this->leg_argu_[0].Delat_L0,
this->leg_argu_[0].L0, this->dt_);
leg_argu_[1].F0 =
leg_argu_[1].Tp * sinf(leg_argu_[1].theta) +
this->param_.wheel_param.static_F0[1] +
leglength_pid_.at(1)->Calculate(this->leg_argu_[1].Delat_L0,
this->leg_argu_[1].L0, this->dt_);
this->leg_argu_[0].Delta_Tp =
leg_argu_[0].L0 * 10.0f *
tp_pid_.at(0)->Calculate(this->leg_argu_[1].theta,
this->leg_argu_[0].theta, this->dt_);
this->leg_argu_[1].Delta_Tp =
-leg_argu_[1].L0 * 10.0f *
tp_pid_.at(0)->Calculate(this->leg_argu_[1].theta,
this->leg_argu_[0].theta, this->dt_);
result3 = leg_[0]->VMCinserve(
-leg_argu_[0].phi1_, -leg_argu_[0].phi4_,
-leg_argu_[0].Tp - this->leg_argu_[0].Delta_Tp, leg_argu_[0].F0);
this->leg_argu_[0].T1 = std::get<0>(result3);
this->leg_argu_[0].T2 = std::get<1>(result3);
result4 = leg_[1]->VMCinserve(
-leg_argu_[1].phi1_, -leg_argu_[1].phi4_,
-leg_argu_[1].Tp - this->leg_argu_[1].Delta_Tp, leg_argu_[1].F0);
this->leg_argu_[1].T1 = -std::get<0>(result4);
this->leg_argu_[1].T2 = -std::get<1>(result4);
if (onground0_flag_ & onground1_flag_) {
move_argu_.yaw_force =
-this->yaw_pid_.Calculate(move_argu_.target_yaw, this->yaw_, dt_);
} else {
move_argu_.yaw_force = 0;
}
/*3508不带减速箱是Tw*3.2f*/
/*2006带减速是Tw/1.8f*/
/* 3508带15.7减速箱是Tw/4.9256 */
this->wheel_motor_out_[0] =
this->leg_argu_[0].Tw / 4.9256f - move_argu_.yaw_force;
this->wheel_motor_out_[1] =
this->leg_argu_[1].Tw / 4.9256f + move_argu_.yaw_force;
this->hip_motor_out_[0] = this->leg_argu_[0].T1;
this->hip_motor_out_[1] = this->leg_argu_[0].T2;
this->hip_motor_out_[2] = this->leg_argu_[1].T1;
this->hip_motor_out_[3] = this->leg_argu_[1].T2;
this->hip_motor_flag_ = 1;
break;
case Wheelleg::RESET:
if (fabs(pit_) > M_PI / 2 || fabs(leg_argu_[0].theta) > 1.57 ||
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;
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) {
leg_argu_[1].target_theta = leg_argu_[1].theta - 0.001;
}
leg_argu_[0].F0 = 10 * leglength_pid_.at(0)->Calculate(
0.65f, this->leg_argu_[0].L0, this->dt_);
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) {
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) {
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) {
leg_argu_[0].target_theta -= 2 * 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) {
leg_argu_[1].target_theta -= 2 * M_PI;
}
if (leg_argu_[1].target_theta < -M_PI) {
leg_argu_[1].target_theta += 2 * M_PI;
}
leg_argu_[0].Tp =
500 * this->theta_pid_[0]->Calculate(leg_argu_[0].target_theta,
leg_argu_[0].theta, dt_);
leg_argu_[1].Tp =
500 * this->theta_pid_[1]->Calculate(leg_argu_[1].target_theta,
leg_argu_[1].theta, dt_);
} 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)) {
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 {
leg_argu_[0].Tp = 0;
leg_argu_[1].Tp = 0;
}
leg_argu_[1].target_theta = leg_argu_[1].theta;
}
result3 = leg_[0]->VMCinserve(-leg_argu_[0].phi1_, -leg_argu_[0].phi4_,
-leg_argu_[0].Tp, leg_argu_[0].F0);
this->leg_argu_[0].T1 = std::get<0>(result3);
this->leg_argu_[0].T2 = std::get<1>(result3);
if (leg_argu_[0].target_theta > M_PI)
{
leg_argu_[0].target_theta -= 2 * 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)
{
leg_argu_[1].target_theta -= 2 * M_PI;
}
if (leg_argu_[1].target_theta < -M_PI)
{
leg_argu_[1].target_theta += 2 * M_PI;
}
leg_argu_[0].Tp =
500 * this->theta_pid_[0]->Calculate(leg_argu_[0].target_theta,
leg_argu_[0].theta, dt_);
leg_argu_[1].Tp =
500 * this->theta_pid_[1]->Calculate(leg_argu_[1].target_theta,
leg_argu_[1].theta, dt_);
}
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_);
result4 = leg_[1]->VMCinserve(-leg_argu_[1].phi1_, -leg_argu_[1].phi4_,
-leg_argu_[1].Tp, leg_argu_[1].F0);
this->leg_argu_[1].T1 = -std::get<0>(result4);
this->leg_argu_[1].T2 = -std::get<1>(result4);
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
{
leg_argu_[0].Tp = 0;
leg_argu_[1].Tp = 0;
}
}
this->hip_motor_out_[0] = this->leg_argu_[0].T1;
this->hip_motor_out_[1] = this->leg_argu_[0].T2;
this->hip_motor_out_[2] = this->leg_argu_[1].T1;
this->hip_motor_out_[3] = this->leg_argu_[1].T2;
result3 = leg_[0]->VMCinserve(-leg_argu_[0].phi1_, -leg_argu_[0].phi4_,
-leg_argu_[0].Tp, leg_argu_[0].F0);
this->leg_argu_[0].T1 = std::get<0>(result3);
this->leg_argu_[0].T2 = std::get<1>(result3);
this->hip_motor_flag_ = 1;
break;
result4 = leg_[1]->VMCinserve(-leg_argu_[1].phi1_, -leg_argu_[1].phi4_,
-leg_argu_[1].Tp, leg_argu_[1].F0);
this->leg_argu_[1].T1 = -std::get<0>(result4);
this->leg_argu_[1].T2 = -std::get<1>(result4);
this->hip_motor_out_[0] = this->leg_argu_[0].T1;
this->hip_motor_out_[1] = this->leg_argu_[0].T2;
this->hip_motor_out_[2] = this->leg_argu_[1].T1;
this->hip_motor_out_[3] = this->leg_argu_[1].T2;
this->hip_motor_flag_ = 1;
break;
}
}
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,51 +665,54 @@ void Wheelleg::Control() {
// }
// }
switch (this->mode_) {
case Wheelleg::RELAX:
switch (this->mode_)
{
case Wheelleg::RELAX:
this->wheel_motor_[0]->Relax();
this->wheel_motor_[1]->Relax();
hip_motor_flag_ = 0;
hip_motor_[0]->SetMit(0.0f);
hip_motor_[1]->SetMit(0.0f);
hip_motor_[2]->SetMit(0.0f);
hip_motor_[3]->SetMit(0.0f);
break;
this->wheel_motor_[0]->Relax();
this->wheel_motor_[1]->Relax();
hip_motor_flag_ = 0;
hip_motor_[0]->SetMit(0.0f);
hip_motor_[1]->SetMit(0.0f);
hip_motor_[2]->SetMit(0.0f);
hip_motor_[3]->SetMit(0.0f);
break;
case Wheelleg::STAND:
case Wheelleg::ROTOR:
// this->wheel_motor_[0]->Relax();
// this->wheel_motor_[1]->Relax();
this->wheel_motor_[0]->Control(-wheel_motor_out_[0]);
this->wheel_motor_[1]->Control(wheel_motor_out_[1]);
hip_motor_[0]->SetMit(this->hip_motor_out_[0]);
hip_motor_[1]->SetMit(this->hip_motor_out_[1]);
hip_motor_[2]->SetMit(this->hip_motor_out_[2]);
hip_motor_[3]->SetMit(this->hip_motor_out_[3]);
// hip_motor_[0]->SetMit(0.0f);
// hip_motor_[1]->SetMit(0.0f);
// hip_motor_[2]->SetMit(0.0f);
// hip_motor_[3]->SetMit(0.0f);
break;
case Wheelleg::STAND:
case Wheelleg::ROTOR:
// this->wheel_motor_[0]->Relax();
// this->wheel_motor_[1]->Relax();
this->wheel_motor_[0]->Control(-wheel_motor_out_[0]);
this->wheel_motor_[1]->Control(wheel_motor_out_[1]);
hip_motor_[0]->SetMit(this->hip_motor_out_[0]);
hip_motor_[1]->SetMit(this->hip_motor_out_[1]);
hip_motor_[2]->SetMit(this->hip_motor_out_[2]);
hip_motor_[3]->SetMit(this->hip_motor_out_[3]);
// hip_motor_[0]->SetMit(0.0f);
// hip_motor_[1]->SetMit(0.0f);
// hip_motor_[2]->SetMit(0.0f);
// hip_motor_[3]->SetMit(0.0f);
break;
case Wheelleg::RESET:
case Wheelleg::RESET:
this->wheel_motor_[0]->Relax();
this->wheel_motor_[1]->Relax();
this->wheel_motor_[0]->Relax();
this->wheel_motor_[1]->Relax();
hip_motor_[0]->SetMit(this->hip_motor_out_[0]);
hip_motor_[1]->SetMit(this->hip_motor_out_[1]);
hip_motor_[0]->SetMit(this->hip_motor_out_[0]);
hip_motor_[1]->SetMit(this->hip_motor_out_[1]);
hip_motor_[2]->SetMit(this->hip_motor_out_[2]);
hip_motor_[3]->SetMit(this->hip_motor_out_[3]);
hip_motor_[2]->SetMit(this->hip_motor_out_[2]);
hip_motor_[3]->SetMit(this->hip_motor_out_[3]);
break;
break;
}
}
void Wheelleg::SetMode(Wheelleg::Mode mode) {
if (mode == this->mode_) {
void Wheelleg::SetMode(Wheelleg::Mode mode)
{
if (mode == this->mode_)
{
return;
}

2
User/task/rc.c
View File

@ -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;