663 lines
24 KiB
C++
663 lines
24 KiB
C++
#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;
|
|
}
|