Skip to content

Commit

Permalink
Merge pull request #72 from xrobot-org/dev
Browse files Browse the repository at this point in the history 
Dev
  • Loading branch information
@Jiu-xiao
Jiu-xiao authored May 31, 2024
2 parents 1886926 + 34db46b commit ae64cf9
Show file tree
Hide file tree
Showing 16 changed files with 728 additions and 1,259 deletions.
109 changes: 0 additions & 109 deletions hw/bsp/rm-c/config/engineer.config
View file

This file was deleted.

102 changes: 6 additions & 96 deletions src/module/engineer_chassis/mod_engineer_chassis.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,10 +9,11 @@
*
*/

#include "mod_engineer_chassis.hpp"

#include <random>

#include "bsp_time.h"
#include "mod_chassis.hpp"

#define ROTOR_WZ_MIN 0.6f /* 小陀螺旋转位移下界 */
#define ROTOR_WZ_MAX 0.8f /* 小陀螺旋转位移上界 */
Expand All @@ -21,31 +22,13 @@

#define MOTOR_MAX_ROTATIONAL_SPEED 7000.0f /* 电机的最大转速 */

#if POWER_LIMIT_WITH_CAP
/* 保证电容电量宏定义在正确范围内 */
#if ((CAP_PERCENT_NO_LIM < 0) || (CAP_PERCENT_NO_LIM > 100) || \
(CAP_PERCENT_WORK < 0) || (CAP_PERCENT_WORK > 100))
#error "Cap percentage should be in the range from 0 to 100."
#endif

/* 保证电容功率宏定义在正确范围内 */
#if ((CAP_MAX_LOAD < 60) || (CAP_MAX_LOAD > 200))
#error "The capacitor power should be in in the range from 60 to 200."
#endif

static const float kCAP_PERCENTAGE_NO_LIM = (float)CAP_PERCENT_NO_LIM / 100.0f;
static const float kCAP_PERCENTAGE_WORK = (float)CAP_PERCENT_WORK / 100.0f;

#endif

using namespace Module;

template <typename Motor, typename MotorParam>
Chassis<Motor, MotorParam>::Chassis(Param& param, float control_freq)
: param_(param),
mode_(Chassis::RELAX),
mixer_(param.type),
follow_pid_(param.follow_pid_param, control_freq),
ctrl_lock_(true) {
memset(&(this->cmd_), 0, sizeof(this->cmd_));

Expand All @@ -70,12 +53,6 @@ Chassis<Motor, MotorParam>::Chassis(Param& param, float control_freq)
case SET_MODE_RELAX:
chassis->SetMode(RELAX);
break;
case SET_MODE_FOLLOW:
chassis->SetMode(FOLLOW_GIMBAL);
break;
case SET_MODE_ROTOR:
chassis->SetMode(ROTOR);
break;
case SET_MODE_INDENPENDENT:
chassis->SetMode(INDENPENDENT);
break;
Expand All @@ -95,26 +72,24 @@ Chassis<Motor, MotorParam>::Chassis(Param& param, float control_freq)
auto chassis_thread = [](Chassis* chassis) {
auto cmd_sub =
Message::Subscriber<Component::CMD::ChassisCMD>("cmd_chassis");

uint32_t last_online_time = bsp_time_get_ms();
while (1) {
/* 读取控制指令、电容、裁判系统、电机反馈 */
cmd_sub.DumpData(chassis->cmd_);

/* 更新反馈值 */
chassis->PraseRef();

chassis->ctrl_lock_.Wait(UINT32_MAX);
chassis->UpdateFeedback();
chassis->Control();
chassis->ctrl_lock_.Post();

/* 运行结束，等待下一次唤醒 */
chassis->thread_.SleepUntil(2);
chassis->thread_.SleepUntil(2, last_online_time);
}
};

this->thread_.Create(chassis_thread, this, "chassis_thread",
MODULE_CHASSIS_TASK_STACK_DEPTH, System::Thread::MEDIUM);
this->thread_.Create(chassis_thread, this, "chassis_thread", 512,
System::Thread::MEDIUM);

System::Timer::Create(this->DrawUIStatic, this, 2100);

Expand Down Expand Up @@ -152,16 +127,6 @@ void Chassis<Motor, MotorParam>::Control() {
break;

case Chassis::RELAX:
case Chassis::FOLLOW_GIMBAL: /* 按照云台方向换算运动向量
*/
case Chassis::ROTOR: {
float beta = this->yaw_;
float cos_beta = cosf(beta);
float sin_beta = sinf(beta);
this->move_vec_.vx = cos_beta * this->cmd_.x - sin_beta * this->cmd_.y;
this->move_vec_.vy = sin_beta * this->cmd_.x + cos_beta * this->cmd_.y;
break;
}
case Chassis::REVERSE: {
this->move_vec_.vx = -this->cmd_.x;
this->move_vec_.vy = -this->cmd_.y;
Expand All @@ -177,19 +142,6 @@ void Chassis<Motor, MotorParam>::Control() {
case Chassis::INDENPENDENT: /* 独立模式wz为0 */
this->move_vec_.wz = this->cmd_.z;
break;

case Chassis::FOLLOW_GIMBAL: /* 跟随模式通过PID控制使车头跟随云台
*/
this->move_vec_.wz =
this->follow_pid_.Calculate(0.0f, this->yaw_, this->dt_);
break;

case Chassis::ROTOR: { /* 小陀螺模式使底盘以一定速度旋转
*/
this->move_vec_.wz =
this->wz_dir_mult_ * CalcWz(ROTOR_WZ_MIN, ROTOR_WZ_MAX);
break;
}
case Chassis::REVERSE: {
this->move_vec_.wz = this->cmd_.z;
break;
Expand All @@ -207,21 +159,8 @@ void Chassis<Motor, MotorParam>::Control() {
/* 根据底盘模式计算输出值 */
switch (this->mode_) {
case Chassis::BREAK:
case Chassis::FOLLOW_GIMBAL:
case Chassis::ROTOR:
case Chassis::REVERSE:
case Chassis::INDENPENDENT: /* 独立模式,受PID控制 */ {
float percentage = 0.0f;
if (cap_.online_) {
percentage = cap_.percentage_;
} else if (ref_.status == Device::Referee::RUNNING) {
percentage = this->ref_.chassis_pwr_buff / 30.0f;
} else {
percentage = 1.0f;
}

clampf(&percentage, 0.0f, 1.0f);

for (unsigned i = 0; i < this->mixer_.len_; i++) {
float out = this->actuator_[i]->Calculate(
this->setpoint_.motor_rotational_speed[i] *
Expand All @@ -243,15 +182,6 @@ void Chassis<Motor, MotorParam>::Control() {
}
}

template <typename Motor, typename MotorParam>
void Chassis<Motor, MotorParam>::PraseRef() {
this->ref_.chassis_power_limit =
this->raw_ref_.robot_status.chassis_power_limit;
this->ref_.chassis_pwr_buff = this->raw_ref_.power_heat.chassis_pwr_buff;
this->ref_.chassis_watt = this->raw_ref_.power_heat.chassis_watt;
this->ref_.status = this->raw_ref_.status;
}

template <typename Motor, typename MotorParam>
float Chassis<Motor, MotorParam>::CalcWz(const float LO, const float HI) {
float wz_vary = fabsf(0.2f * sinf(ROTOR_OMEGA * this->now_)) + LO;
Expand All @@ -265,10 +195,6 @@ void Chassis<Motor, MotorParam>::SetMode(Chassis::Mode mode) {
return; /* 模式未改变直接返回 */
}

if (mode == Chassis::ROTOR && this->mode_ != Chassis::ROTOR) {
std::srand(this->now_);
this->wz_dir_mult_ = (std::rand() % 2) ? -1 : 1;
}
/* 切换模式后重置PID和滤波器 */
for (size_t i = 0; i < this->mixer_.len_; i++) {
this->actuator_[i]->Reset();
Expand All @@ -294,14 +220,6 @@ void Chassis<Motor, MotorParam>::DrawUIStatic(

/* 更新底盘模式选择框 */
switch (chassis->mode_) {
case FOLLOW_GIMBAL:
box_pos_left = REF_UI_MODE_OFFSET_2_LEFT;
box_pos_right = REF_UI_MODE_OFFSET_2_RIGHT;
break;
case ROTOR:
box_pos_left = REF_UI_MODE_OFFSET_4_LEFT;
box_pos_right = REF_UI_MODE_OFFSET_4_RIGHT;
break;
case INDENPENDENT:
box_pos_left = REF_UI_MODE_OFFSET_3_LEFT;
box_pos_right = REF_UI_MODE_OFFSET_3_RIGHT;
Expand Down Expand Up @@ -342,14 +260,6 @@ void Chassis<Motor, MotorParam>::DrawUIDynamic(

/* 更新底盘模式选择框 */
switch (chassis->mode_) {
case FOLLOW_GIMBAL:
box_pos_left = REF_UI_MODE_OFFSET_2_LEFT;
box_pos_right = REF_UI_MODE_OFFSET_2_RIGHT;
break;
case ROTOR:
box_pos_left = REF_UI_MODE_OFFSET_4_LEFT;
box_pos_right = REF_UI_MODE_OFFSET_4_RIGHT;
break;
case RELAX:

case BREAK:
Expand Down
Loading

0 comments on commit ae64cf9

Please sign in to comment.