合并 main 到 MC_02
This commit is contained in:
commit
e4fda840a0
@ -7,8 +7,6 @@
|
||||
#include <stddef.h>
|
||||
|
||||
/* Private macros ----------------------------------------------------------- */
|
||||
#define LQR_LIMIT(x, min, max) ((x) < (min) ? (min) : ((x) > (max) ? (max) : (x)))
|
||||
|
||||
/* Private variables -------------------------------------------------------- */
|
||||
|
||||
/* Private function prototypes ---------------------------------------------- */
|
||||
@ -21,7 +19,62 @@ int8_t LQR_Init(LQR_t *lqr, LQR_GainMatrix_t *gain_matrix) {
|
||||
if (lqr == NULL || gain_matrix == NULL) {
|
||||
return -1;
|
||||
}
|
||||
/* 计算状态误差 */
|
||||
lqr->error_state.theta = lqr->current_state.theta - lqr->target_state.theta;
|
||||
lqr->error_state.d_theta = lqr->current_state.d_theta - lqr->target_state.d_theta;
|
||||
lqr->error_state.x = lqr->current_state.x - lqr->target_state.x;
|
||||
lqr->error_state.d_x = lqr->current_state.d_x - lqr->target_state.d_x;
|
||||
lqr->error_state.phi = lqr->current_state.phi - lqr->target_state.phi;
|
||||
lqr->error_state.d_phi = lqr->current_state.d_phi - lqr->target_state.d_phi;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static float LQR_PolynomialCalc(const float *coeff, float leg_length) {
|
||||
if (coeff == NULL) {
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
/* 计算3阶多项式: coeff[0]*L^3 + coeff[1]*L^2 + coeff[2]*L + coeff[3] */
|
||||
float L = leg_length;
|
||||
float L2 = L * L;
|
||||
float L3 = L2 * L;
|
||||
|
||||
return coeff[0] * L3 + coeff[1] * L2 + coeff[2] * L + coeff[3];
|
||||
}
|
||||
|
||||
|
||||
// static int8_t LQR_ApplyControlLimits(LQR_t *lqr) {
|
||||
// if (lqr == NULL) {
|
||||
// return -1;
|
||||
// }
|
||||
|
||||
// /* 限制轮毂力矩 */
|
||||
// if (fabsf(lqr->control_output.T) > lqr->param.max_wheel_torque) {
|
||||
// lqr->control_output.T = LQR_LIMIT(lqr->control_output.T,
|
||||
// -lqr->param.max_wheel_torque,
|
||||
// lqr->param.max_wheel_torque);
|
||||
// lqr->wheel_torque_limited = true;
|
||||
// }
|
||||
|
||||
// /* 限制髋关节力矩 */
|
||||
// if (fabsf(lqr->control_output.Tp) > lqr->param.max_joint_torque) {
|
||||
// lqr->control_output.Tp = LQR_LIMIT(lqr->control_output.Tp,
|
||||
// -lqr->param.max_joint_torque,
|
||||
// lqr->param.max_joint_torque);
|
||||
// lqr->joint_torque_limited = true;
|
||||
// }
|
||||
|
||||
// return 0;
|
||||
// }
|
||||
|
||||
|
||||
/* Public functions --------------------------------------------------------- */
|
||||
|
||||
int8_t LQR_Init(LQR_t *lqr, LQR_GainMatrix_t *gain_matrix) {
|
||||
if (lqr == NULL) {
|
||||
return -1;
|
||||
}
|
||||
/* 清零结构体 */
|
||||
memset(lqr, 0, sizeof(LQR_t));
|
||||
|
||||
@ -68,7 +121,6 @@ int8_t LQR_CalculateGainMatrix(LQR_t *lqr, float leg_length) {
|
||||
}
|
||||
|
||||
/* 限制腿长范围 */
|
||||
leg_length = LQR_LIMIT(leg_length, 0.1f, 0.4f);
|
||||
lqr->current_leg_length = leg_length;
|
||||
|
||||
/* 使用多项式拟合计算当前增益矩阵 */
|
||||
|
||||
@ -255,7 +255,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd);
|
||||
* \param s 包含底盘数据的结构体
|
||||
* \param out CAN设备底盘输出结构体
|
||||
*/
|
||||
void Chassis_Output(Chassis_t *c);
|
||||
int8_t Chassis_Output(Chassis_t *c);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
|
||||
662
mod_wheelleg_chassis.cpp
Normal file
662
mod_wheelleg_chassis.cpp
Normal file
@ -0,0 +1,662 @@
|
||||
#include "mod_wheelleg_chassis.hpp"
|
||||
|
||||
#include <cmath>
|
||||
#include <tuple>
|
||||
|
||||
using namespace Module;
|
||||
Wheelleg::Wheelleg(Param& param)
|
||||
: param_(param),
|
||||
roll_pid_(param.roll_pid_param, 333.0f),
|
||||
yaw_pid_(param.yaw_pid_param, 333.0f),
|
||||
yaccl_pid_(param.yaccl_pid_param, 333.0f),
|
||||
lowfilter_(333.0f, 50.0f),
|
||||
acclfilter_(333.0f, 30.0f),
|
||||
manfilter_(param.manfilter_param),
|
||||
|
||||
ctrl_lock_(true) {
|
||||
memset(&(this->cmd_), 0, sizeof(this->cmd_));
|
||||
|
||||
this->hip_motor_.at(0) =
|
||||
new Device::MitMotor(param.hip_motor_param.at(0), "hip_left_front");
|
||||
this->hip_motor_.at(1) =
|
||||
new Device::MitMotor(param.hip_motor_param.at(1), "hip_left_back");
|
||||
this->hip_motor_.at(2) =
|
||||
new Device::MitMotor(param.hip_motor_param.at(2), "hip_right_front");
|
||||
this->hip_motor_.at(3) =
|
||||
new Device::MitMotor(param.hip_motor_param.at(3), "hip_left_back");
|
||||
|
||||
this->wheel_motor_.at(0) =
|
||||
new Device::RMMotor(param.wheel_motor_param.at(0), "wheel_left");
|
||||
this->wheel_motor_.at(1) =
|
||||
new Device::RMMotor(param.wheel_motor_param.at(1), "wheel_right");
|
||||
|
||||
this->leglength_pid_.at(0) =
|
||||
new Component::PID(param.leglength_pid_param.at(0), 333.0f);
|
||||
this->leglength_pid_.at(1) =
|
||||
new Component::PID(param.leglength_pid_param.at(1), 333.0f);
|
||||
|
||||
this->theta_pid_.at(0) =
|
||||
new Component::PID(param.theta_pid_param.at(0), 333.0f);
|
||||
this->theta_pid_.at(1) =
|
||||
new Component::PID(param.theta_pid_param.at(1), 333.0f);
|
||||
|
||||
this->tp_pid_.at(0) = new Component::PID(param.Tp_pid_param.at(0), 333.0);
|
||||
this->tp_pid_.at(1) = new Component::PID(param.Tp_pid_param.at(1), 333.0);
|
||||
|
||||
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) {
|
||||
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;
|
||||
}
|
||||
chassis->ctrl_lock_.Post();
|
||||
};
|
||||
|
||||
Component::CMD::RegisterEvent<Wheelleg*, ChassisEvent>(
|
||||
event_callback, this, this->param_.EVENT_MAP);
|
||||
|
||||
auto chassis_thread = [](Wheelleg* chassis) {
|
||||
auto cmd_sub =
|
||||
Message::Subscriber<Component::CMD::ChassisCMD>("cmd_chassis");
|
||||
auto eulr_sub =
|
||||
Message::Subscriber<Component::Type::Eulr>("chassis_imu_eulr");
|
||||
auto gyro_sub =
|
||||
Message::Subscriber<Component::Type::Vector3>("chassis_gyro");
|
||||
|
||||
auto yaw_sub = Message::Subscriber<float>("chassis_yaw");
|
||||
|
||||
auto accl_sub =
|
||||
Message::Subscriber<Component::Type::Vector3>("chassis_imu_accl_abs");
|
||||
// auto yaw_sub = Message::Subscriber<float>("chassis_yaw");
|
||||
uint32_t last_online_time = bsp_time_get_ms();
|
||||
|
||||
while (1) {
|
||||
eulr_sub.DumpData(chassis->eulr_); /* imu */
|
||||
gyro_sub.DumpData(chassis->gyro_); /* imu */
|
||||
accl_sub.DumpData(chassis->accl_);
|
||||
|
||||
yaw_sub.DumpData(chassis->yaw_);
|
||||
cmd_sub.DumpData(chassis->cmd_);
|
||||
// yaw_sub.DumpData(chassis->yaw_);
|
||||
|
||||
chassis->ctrl_lock_.Wait(UINT32_MAX);
|
||||
chassis->MotorSetAble();
|
||||
chassis->UpdateFeedback();
|
||||
chassis->Calculate();
|
||||
chassis->Control();
|
||||
chassis->ctrl_lock_.Post();
|
||||
|
||||
/* 运行结束，等待下一次唤醒 */
|
||||
chassis->thread_.SleepUntil(3, last_online_time);
|
||||
}
|
||||
};
|
||||
this->thread_.Create(chassis_thread, this, "chassis_thread", 1536,
|
||||
System::Thread::MEDIUM);
|
||||
}
|
||||
|
||||
void Wheelleg::MotorSetAble() {
|
||||
if (this->hip_motor_flag_ == 0) {
|
||||
this->hip_motor_[0]->Relax();
|
||||
this->hip_motor_[1]->Relax();
|
||||
this->hip_motor_[2]->Relax();
|
||||
this->hip_motor_[3]->Relax();
|
||||
this->dm_motor_flag_ = 0;
|
||||
}
|
||||
|
||||
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++) {
|
||||
this->hip_motor_[1]->Enable();
|
||||
}
|
||||
for (int i = 0; i < 5; i++) {
|
||||
this->hip_motor_[2]->Enable();
|
||||
}
|
||||
for (int i = 0; i < 5; i++) {
|
||||
this->hip_motor_[3]->Enable();
|
||||
}
|
||||
|
||||
this->dm_motor_flag_ = 1;
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
void Wheelleg::UpdateFeedback() {
|
||||
this->now_ = bsp_time_get();
|
||||
this->dt_ = TIME_DIFF(this->last_wakeup_, this->now_);
|
||||
this->last_wakeup_ = this->now_;
|
||||
|
||||
this->wheel_motor_[0]->Update();
|
||||
this->wheel_motor_[1]->Update();
|
||||
|
||||
this->leg_argu_[0].phi4_ =
|
||||
this->hip_motor_[0]->GetAngle() -
|
||||
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->leg_argu_[1].phi4_ =
|
||||
(-this->hip_motor_[2]->GetAngle() +
|
||||
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->leg_argu_[1].phi1_ = this->leg_argu_[1].phi1_ - 2 * M_PI;
|
||||
}
|
||||
|
||||
this->pit_ = this->eulr_.pit;
|
||||
if (this->pit_ > M_PI) {
|
||||
this->pit_ = this->eulr_.pit - 2 * M_PI;
|
||||
}
|
||||
|
||||
/* 注意极性 */
|
||||
std::tuple<float, float, float, float> result0 =
|
||||
this->leg_[0]->VMCsolve(-leg_argu_[0].phi1_, -leg_argu_[0].phi4_,
|
||||
this->pit_, -this->gyro_.x, this->dt_);
|
||||
this->leg_argu_[0].L0 = std::get<0>(result0);
|
||||
this->leg_argu_[0].d_L0 = std::get<1>(result0);
|
||||
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) {
|
||||
leg_argu_[0].theta = leg_argu_[0].theta - 2 * M_PI;
|
||||
}
|
||||
if (leg_argu_[0].theta < -M_PI) {
|
||||
leg_argu_[0].theta = leg_argu_[0].theta + 2 * M_PI;
|
||||
}
|
||||
|
||||
std::tuple<float, float, float, float> result1 =
|
||||
this->leg_[1]->VMCsolve(-leg_argu_[1].phi1_, -leg_argu_[1].phi4_,
|
||||
this->pit_, -this->gyro_.x, this->dt_);
|
||||
this->leg_argu_[1].L0 = std::get<0>(result1);
|
||||
this->leg_argu_[1].d_L0 = std::get<1>(result1);
|
||||
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) {
|
||||
leg_argu_[1].theta = leg_argu_[1].theta - 2 * M_PI;
|
||||
}
|
||||
if (leg_argu_[1].theta < -M_PI) {
|
||||
leg_argu_[1].theta = leg_argu_[1].theta + 2 * M_PI;
|
||||
}
|
||||
|
||||
/* 轮子转速近似于编码器速度 由此推测机体速度 */
|
||||
this->leg_argu_[0].single_x_dot =
|
||||
-wheel_motor_[0]->GetSpeed() * 2 * M_PI *
|
||||
(this->param_.wheel_param.wheel_radius) / 60.f / 15.765 +
|
||||
leg_argu_[0].L0 * leg_argu_[0].d_theta * cosf(leg_argu_[0].theta) +
|
||||
leg_argu_[0].d_L0 * sinf(leg_argu_[0].theta);
|
||||
|
||||
this->leg_argu_[1].single_x_dot =
|
||||
wheel_motor_[1]->GetSpeed() * 2 * M_PI *
|
||||
(this->param_.wheel_param.wheel_radius) / 60.f / 15.765 +
|
||||
leg_argu_[1].L0 * leg_argu_[1].d_theta * cosf(leg_argu_[1].theta) +
|
||||
leg_argu_[1].d_L0 * sinf(leg_argu_[1].theta);
|
||||
|
||||
this->move_argu_.last_x_dot = this->move_argu_.x_dot;
|
||||
this->move_argu_.x_dot =
|
||||
|
||||
(this->leg_argu_[0].single_x_dot + this->leg_argu_[1].single_x_dot) / 2;
|
||||
this->move_argu_.x_dot =
|
||||
(-wheel_motor_[0]->GetSpeed() + wheel_motor_[1]->GetSpeed()) * M_PI *
|
||||
this->param_.wheel_param.wheel_radius / 60.f / 15.765;
|
||||
this->move_argu_.x = this->move_argu_.x_dot * dt_ + move_argu_.x;
|
||||
|
||||
this->move_argu_.delta_speed =
|
||||
lowfilter_.Apply((move_argu_.x_dot - move_argu_.last_x_dot) / dt_);
|
||||
|
||||
this->move_argu_.accl = acclfilter_.Apply(this->accl_.y * 9.8f);
|
||||
|
||||
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,
|
||||
dt_) -
|
||||
((leg_argu_[0].L0 * leg_argu_[0].d_theta * cosf(leg_argu_[0].theta) +
|
||||
leg_argu_[0].d_L0 * sinf(leg_argu_[0].theta)) +
|
||||
(leg_argu_[1].L0 * leg_argu_[1].d_theta * cosf(leg_argu_[1].theta) +
|
||||
leg_argu_[1].d_L0 * sinf(leg_argu_[1].theta))) /
|
||||
2;
|
||||
this->move_argu_.xhat = dt_ * move_argu_.x_dot_hat + move_argu_.xhat;
|
||||
}
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
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;
|
||||
}
|
||||
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;
|
||||
}
|
||||
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;
|
||||
|
||||
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;
|
||||
|
||||
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) {
|
||||
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) {
|
||||
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:
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
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 ((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;
|
||||
}
|
||||
}
|
||||
|
||||
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);
|
||||
|
||||
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() {
|
||||
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);
|
||||
clampf(&hip_motor_out_[1], -20.0f, 20.0f);
|
||||
clampf(&hip_motor_out_[2], -20.0f, 20.0f);
|
||||
clampf(&hip_motor_out_[3], -20.0f, 20.0f);
|
||||
|
||||
// if (fabs(wheel_motor_[0]->GetSpeed()) > 5000 ||
|
||||
// fabs(wheel_motor_[1]->GetSpeed()) > 5000) {
|
||||
// wheel_motor_out_[0] = 0;
|
||||
// wheel_motor_out_[1] = 0;
|
||||
|
||||
// if (fabs(this->pit_) > 0.5f) {
|
||||
// this->hip_motor_flag_ = 0;
|
||||
// }
|
||||
// }
|
||||
|
||||
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;
|
||||
|
||||
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:
|
||||
|
||||
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_[2]->SetMit(this->hip_motor_out_[2]);
|
||||
hip_motor_[3]->SetMit(this->hip_motor_out_[3]);
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void Wheelleg::SetMode(Wheelleg::Mode mode) {
|
||||
if (mode == this->mode_) {
|
||||
return;
|
||||
}
|
||||
|
||||
this->leg_[0]->Reset();
|
||||
this->leg_[1]->Reset();
|
||||
move_argu_.x = 0;
|
||||
move_argu_.x_dot = 0;
|
||||
move_argu_.last_x_dot = 0;
|
||||
move_argu_.target_x = move_argu_.xhat;
|
||||
move_argu_.target_yaw = this->eulr_.yaw;
|
||||
move_argu_.target_dot_x = 0;
|
||||
move_argu_.xhat = 0;
|
||||
move_argu_.x_dot_hat = 0;
|
||||
this->manfilter_.reset_comp();
|
||||
this->mode_ = mode;
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user