From 99a94d045f22564e74f26eb8d714b54edcc4f140 Mon Sep 17 00:00:00 2001 From: 52cwc <1340561738@qq.com> Date: Sun, 19 May 2024 20:55:54 +0800 Subject: [PATCH 1/3] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E5=B7=A5=E7=A8=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../engineer_chassis/mod_engineer_chassis.cpp | 102 +------ .../engineer_chassis/mod_engineer_chassis.hpp | 28 +- src/module/robot_arm/Kconfig | 0 src/module/robot_arm/info.cmake | 6 + src/module/robot_arm/mod_robot_arm.cpp | 287 ++++++++++++++++++ src/module/robot_arm/mod_robot_arm.hpp | 116 +++++++ src/robot/arm_engineer/Kconfig | 0 src/robot/arm_engineer/robot.cpp | 250 +++++++++++++++ src/robot/arm_engineer/robot.hpp | 37 +++ 9 files changed, 704 insertions(+), 122 deletions(-) create mode 100644 src/module/robot_arm/Kconfig create mode 100644 src/module/robot_arm/info.cmake create mode 100644 src/module/robot_arm/mod_robot_arm.cpp create mode 100644 src/module/robot_arm/mod_robot_arm.hpp create mode 100644 src/robot/arm_engineer/Kconfig create mode 100644 src/robot/arm_engineer/robot.cpp create mode 100644 src/robot/arm_engineer/robot.hpp diff --git a/src/module/engineer_chassis/mod_engineer_chassis.cpp b/src/module/engineer_chassis/mod_engineer_chassis.cpp index e0f5de88..25c6b43b 100644 --- a/src/module/engineer_chassis/mod_engineer_chassis.cpp +++ b/src/module/engineer_chassis/mod_engineer_chassis.cpp @@ -9,10 +9,11 @@ * */ +#include "mod_engineer_chassis.hpp" + #include #include "bsp_time.h" -#include "mod_chassis.hpp" #define ROTOR_WZ_MIN 0.6f /* 小陀螺旋转位移下界 */ #define ROTOR_WZ_MAX 0.8f /* 小陀螺旋转位移上界 */ @@ -21,23 +22,6 @@ #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 @@ -45,7 +29,6 @@ Chassis::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_)); @@ -70,12 +53,6 @@ Chassis::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; @@ -95,26 +72,24 @@ Chassis::Chassis(Param& param, float control_freq) auto chassis_thread = [](Chassis* chassis) { auto cmd_sub = Message::Subscriber("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); @@ -152,16 +127,6 @@ void Chassis::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; @@ -177,19 +142,6 @@ void Chassis::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; @@ -207,21 +159,8 @@ void Chassis::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] * @@ -243,15 +182,6 @@ void Chassis::Control() { } } -template -void Chassis::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 float Chassis::CalcWz(const float LO, const float HI) { float wz_vary = fabsf(0.2f * sinf(ROTOR_OMEGA * this->now_)) + LO; @@ -265,10 +195,6 @@ void Chassis::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(); @@ -294,14 +220,6 @@ void Chassis::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; @@ -342,14 +260,6 @@ void Chassis::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: diff --git a/src/module/engineer_chassis/mod_engineer_chassis.hpp b/src/module/engineer_chassis/mod_engineer_chassis.hpp index a9dfd267..a2e44140 100644 --- a/src/module/engineer_chassis/mod_engineer_chassis.hpp +++ b/src/module/engineer_chassis/mod_engineer_chassis.hpp @@ -18,7 +18,6 @@ #include "comp_filter.hpp" #include "comp_mixer.hpp" #include "comp_pid.hpp" -#include "dev_cap.hpp" #include "dev_motor.hpp" #include "dev_referee.hpp" #include "dev_rm_motor.hpp" @@ -31,27 +30,21 @@ class Chassis { typedef enum { RELAX, /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */ BREAK, /* 刹车模式，电机闭环控制保持静止。用于机器人停止状态 */ - FOLLOW_GIMBAL, /* 通过闭环控制使车头方向跟随云台 */ - ROTOR, /* 小陀螺模式，通过闭环控制使底盘不停旋转 */ INDENPENDENT, /* 独立模式。底盘运行不受云台影响 */ REVERSE, } Mode; typedef enum { SET_MODE_RELAX, - SET_MODE_FOLLOW, - SET_MODE_ROTOR, SET_MODE_INDENPENDENT, SET_MODE_REVERSE, } ChassisEvent; /* 底盘参数的结构体，包含所有初始Component化用的参数，通常是const，存好几组 */ - typedef struct { + typedef struct Param { Component::Mixer::Mode type = Component::Mixer::MECANUM; /* 底盘类型，底盘的机械设计和轮子选型 */ - Component::PID::Param follow_pid_param{}; /* 跟随云台PID的参数 */ - const std::vector EVENT_MAP; std::array actuator_param{}; @@ -59,13 +52,6 @@ class Chassis { std::array motor_param; } Param; - typedef struct { - Device::Referee::Status status; - float chassis_power_limit; - float chassis_pwr_buff; - float chassis_watt; - } RefForChassis; - Chassis(Param ¶m, float control_freq); void UpdateFeedback(); @@ -74,10 +60,6 @@ class Chassis { void SetMode(Mode mode); - void PackOutput(); - - void PraseRef(); - static void DrawUIStatic(Chassis *chassis); static void DrawUIDynamic(Chassis *chassis); @@ -93,12 +75,8 @@ class Chassis { uint64_t now_ = 0; - RefForChassis ref_; - Mode mode_ = RELAX; - Device::Cap::Info cap_; - std::array actuator_; std::array motor_; @@ -117,14 +95,12 @@ class Chassis { /* 反馈控制用的PID */ - Component::PID follow_pid_; /* 跟随云台用的PID */ - System::Thread thread_; System::Semaphore ctrl_lock_; float yaw_; - Device::Referee::Data raw_ref_; + Component::CMD::ChassisCMD cmd_; Component::UI::String string_; diff --git a/src/module/robot_arm/Kconfig b/src/module/robot_arm/Kconfig new file mode 100644 index 00000000..e69de29b diff --git a/src/module/robot_arm/info.cmake b/src/module/robot_arm/info.cmake new file mode 100644 index 00000000..35463fe0 --- /dev/null +++ b/src/module/robot_arm/info.cmake @@ -0,0 +1,6 @@ +CHECK_SUB_ENABLE(MODULE_ENABLE module) +if(${MODULE_ENABLE}) + file(GLOB CUR_SOURCES "${SUB_DIR}/*.cpp") + SUB_ADD_SRC(CUR_SOURCES) + SUB_ADD_INC(SUB_DIR) +endif() \ No newline at end of file diff --git a/src/module/robot_arm/mod_robot_arm.cpp b/src/module/robot_arm/mod_robot_arm.cpp new file mode 100644 index 00000000..7cba3696 --- /dev/null +++ b/src/module/robot_arm/mod_robot_arm.cpp @@ -0,0 +1,287 @@ +#include "mod_robot_arm.hpp" + +#include + +#include "bsp_time.h" +#include "dev_rm_motor.hpp" + +using namespace Module; + +RobotArm::RobotArm(Param& param, float control_freq) + : param_(param), + mode_(RobotArm::WORK_BOT), + roll2_actr_(this->param_.roll2_actr, control_freq), + + yaw1_motor_(this->param_.yaw1_motor, "RobotArm_Yaw1"), + pitch1_motor_(this->param_.pitch1_motor, "RobotArm_Pitch1"), + pitch2_motor_(this->param_.pitch2_motor, "RobotArm_Pitch2"), + roll1_motor_(this->param_.roll1_motor, "RobotArm_Roll1"), + yaw2_motor_(this->param_.yaw2_motor, "RobotArm_Yaw2"), + roll2_motor_(this->param_.roll2_motor, "RobotArm_Roll2"), + custom_ctrl_(param.cust_ctrl), + ctrl_lock_(true) { + memset(&(this->cmd_), 0, sizeof(this->cmd_)); + + memset(&(this->setpoint_), 0, sizeof(this->setpoint_)); + + auto event_callback = [](RobotArmEvent event, RobotArm* robotarm) { + robotarm->ctrl_lock_.Wait(UINT32_MAX); + + switch (event) { + case SET_MODE_RELAX: + robotarm->SetMode(RELAX); + break; + case SET_MODE_WORK_TOP: + robotarm->SetMode(WORK_TOP); + break; + case SET_MODE_WORK_MID: + robotarm->SetMode(WORK_MID); + break; + case SET_MODE_WORK_BOT: + robotarm->SetMode(WORK_BOT); + break; + case SET_MODE_SAFE: + robotarm->SetMode(SAFE); + break; + default: + break; + } + + robotarm->ctrl_lock_.Post(); + }; + + Component::CMD::RegisterEvent( + event_callback, this, this->param_.EVENT_MAP); + + auto robotarm_thread = [](RobotArm* robotarm) { + auto cmd_sub = Message::Subscriber("cmd_gimbal"); + + uint32_t last_online_time = bsp_time_get_ms(); + + while (1) { + cmd_sub.DumpData(robotarm->cmd_); + robotarm->ctrl_lock_.Wait(UINT32_MAX); + robotarm->DamiaoSetAble(); + robotarm->UpdateFeedback(); + robotarm->Control(); + robotarm->ctrl_lock_.Post(); + + robotarm->thread_.SleepUntil(2, last_online_time); + } + }; + + this->thread_.Create(robotarm_thread, this, "robotarm_thread", 1024, + System::Thread::MEDIUM); +} + +void RobotArm::DamiaoSetAble() { + if (this->yaw1_able_ == 0) { + this->yaw1_motor_.Disable(); + } else { + this->yaw1_motor_.Enable(); + }; + if (this->pitch1_able_ == 0) { + this->pitch1_motor_.Disable(); + } else { + this->pitch1_motor_.Enable(); + }; + if (this->pitch2_able_ == 0) { + this->pitch2_motor_.Disable(); + } else { + this->pitch2_motor_.Enable(); + }; + if (this->roll1_able_ == 0) { + this->roll1_motor_.Disable(); + } else { + this->roll1_motor_.Enable(); + }; + if (this->yaw2_able_ == 0) { + this->yaw2_motor_.Disable(); + } else { + this->yaw2_motor_.Enable(); + }; +} + +void RobotArm::UpdateFeedback() { + this->yaw1_motor_.Update(); + this->pitch1_motor_.Update(); + this->pitch2_motor_.Update(); + this->roll1_motor_.Update(); + this->yaw2_motor_.Update(); + this->roll2_motor_.Update(); +} + +void RobotArm::Control() { + this->now_ = bsp_time_get(); + + this->dt_ = TIME_DIFF(this->last_wakeup_, this->now_); + + this->last_wakeup_ = this->now_; + + // this->setpoint_.yaw1_theta_ = this->cmd_.theta.yaw1; + // this->setpoint_.pitch1_theta_ = this->cmd_.theta.pitch1; + // this->setpoint_.pitch2_theta_ = this->cmd_.theta.pitch2; + // this->setpoint_.pitch3_theta_ = this->cmd_.theta.pitch3; + // this->setpoint_.yaw2_theta_ = this->cmd_.theta.yaw2; + // this->setpoint_.roll_theta_ = this->cmd_.theta.roll; + + float pit_cmd = 0.0f; + float yaw_cmd = 0.0f; + + yaw_cmd = this->cmd_.eulr.yaw * this->dt_; + pit_cmd = this->cmd_.eulr.pit * this->dt_; + + /*电机限幅*/ + // clampf(&(this->setpoint_.yaw1_theta_), this->param_.limit.yaw1_min, + // this->param_.limit.yaw1_max); + // clampf(&(this->setpoint_.pitch1_theta_), this->param_.limit.pitch1_min, + // this->param_.limit.pitch1_max); + // clampf(&(this->setpoint_.pitch2_theta_), this->param_.limit.pitch2_min, + // this->param_.limit.pitch2_max); + // clampf(&(this->setpoint_.roll1_theta_), this->param_.limit.roll1_min, + // this->param_.limit.roll1_max); + // clampf(&(this->setpoint_.yaw2_theta_), this->param_.limit.yaw2_min, + // this->param_.limit.yaw2_max); + + // this->yaw1_motor_.SetPosSpeed(this->setpoint_.yaw1_theta_, 0.1f); + // this->pitch1_motor_.SetPosSpeed(this->setpoint_.pitch1_theta_, 0.3f); + // this->pitch2_motor_.SetPosSpeed(this->setpoint_.pitch2_theta_, 0.3f); + // this->roll1_motor_.SetPosSpeed(this->setpoint_.roll1_theta_, 0.3f); + // this->yaw2_motor_.SetPosSpeed(this->setpoint_.yaw2_theta_, 0.3f); + + // float roll2_out = this->roll2_actr_.Calculate( + // this->setpoint_roll2_, this->roll2_motor_.GetSpeed(), + // this->roll2_motor_.GetAngle(), this->dt_); + // this->roll2_motor_.Control(roll2_out); + + switch (this->mode_) { + case RobotArm::WORK_BOT: { + this->setpoint_.yaw1_theta_ += yaw_cmd; + this->setpoint_.pitch1_theta_ += pit_cmd; + + clampf(&(this->setpoint_.yaw1_theta_), this->param_.limit.yaw1_min, + this->param_.limit.yaw1_max); + clampf(&(this->setpoint_.pitch1_theta_), this->param_.limit.pitch1_min, + this->param_.limit.pitch1_max); + + this->yaw1_motor_.SetPosSpeed(this->setpoint_.yaw1_theta_, 0.1f); + this->pitch1_motor_.SetPosSpeed(this->setpoint_.pitch1_theta_, 0.3f); + break; + } + case RobotArm::WORK_MID: { + this->setpoint_.roll1_theta_ += yaw_cmd; + this->setpoint_.pitch2_theta_ += pit_cmd; + + clampf(&(this->setpoint_.pitch2_theta_), this->param_.limit.pitch2_min, + this->param_.limit.pitch2_max); + + this->pitch2_motor_.SetPosSpeed(this->setpoint_.pitch2_theta_, 0.3f); + this->roll1_motor_.SetPosSpeed(this->setpoint_.roll1_theta_, 0.3f); + break; + } + case RobotArm::WORK_TOP: { + this->setpoint_.yaw2_theta_ += pit_cmd; + this->setpoint_roll2_ = + Component::Type::CycleValue(this->setpoint_roll2_ + yaw_cmd); + + clampf(&(this->setpoint_.yaw2_theta_), this->param_.limit.yaw2_min, + this->param_.limit.yaw2_max); + + this->yaw2_motor_.SetPosSpeed(this->setpoint_.yaw2_theta_, 0.3f); + + float roll2_out = this->roll2_actr_.Calculate( + this->setpoint_roll2_, this->roll2_motor_.GetSpeed(), + this->roll2_motor_.GetAngle(), this->dt_); + this->roll2_motor_.Control(roll2_out); + + break; + } + case RobotArm::SAFE: { + break; + } + + case RobotArm::RELAX: + this->yaw1_able_ = 0; + this->pitch1_able_ = 0; + this->pitch2_able_ = 0; + this->roll1_able_ = 0; + this->yaw2_able_ = 0; + this->roll2_motor_.Relax(); + break; + case RobotArm::WORK_CUSTOM_CTRL: + if (custom_ctrl_.online_) { + this->setpoint_.yaw1_theta_ = custom_ctrl_.data_.angle[0]; + this->setpoint_.pitch1_theta_ = custom_ctrl_.data_.angle[1]; + + clampf(&(this->setpoint_.yaw1_theta_), this->param_.limit.yaw1_min, + this->param_.limit.yaw1_max); + clampf(&(this->setpoint_.pitch1_theta_), this->param_.limit.pitch1_min, + this->param_.limit.pitch1_max); + + this->yaw1_motor_.SetPosSpeed(this->setpoint_.yaw1_theta_, 0.1f); + this->pitch1_motor_.SetPosSpeed(this->setpoint_.pitch1_theta_, 0.3f); + + this->setpoint_.roll1_theta_ = custom_ctrl_.data_.angle[2]; + this->setpoint_.pitch2_theta_ = custom_ctrl_.data_.angle[3]; + + clampf(&(this->setpoint_.pitch2_theta_), this->param_.limit.pitch2_min, + this->param_.limit.pitch2_max); + + this->pitch2_motor_.SetPosSpeed(this->setpoint_.pitch2_theta_, 0.3f); + this->roll1_motor_.SetPosSpeed(this->setpoint_.roll1_theta_, 0.3f); + + this->setpoint_.yaw2_theta_ = custom_ctrl_.data_.angle[4]; + this->setpoint_roll2_ = custom_ctrl_.data_.angle[5]; + + clampf(&(this->setpoint_.yaw2_theta_), this->param_.limit.yaw2_min, + this->param_.limit.yaw2_max); + + this->yaw2_motor_.SetPosSpeed(this->setpoint_.yaw2_theta_, 0.3f); + + float roll2_out = this->roll2_actr_.Calculate( + this->setpoint_roll2_, this->roll2_motor_.GetSpeed(), + this->roll2_motor_.GetAngle(), this->dt_); + this->roll2_motor_.Control(roll2_out); + } else { + this->yaw1_able_ = 0; + this->pitch1_able_ = 0; + this->pitch2_able_ = 0; + this->roll1_able_ = 0; + this->yaw2_able_ = 0; + this->roll2_motor_.Relax(); + } + + break; + // default: + // XB_ASSERT(false); + // return; + } +} + +void RobotArm::SetMode(RobotArm::Mode mode) { + if (mode == this->mode_) { + return; + } + if (mode == WORK_BOT) { + this->yaw1_able_ = 1; + this->pitch1_able_ = 1; + this->pitch2_able_ = 1; + this->roll1_able_ = 1; + this->yaw2_able_ = 1; + } + if (mode == WORK_MID) { + this->yaw1_able_ = 1; + this->pitch1_able_ = 1; + this->pitch2_able_ = 1; + this->roll1_able_ = 1; + this->yaw2_able_ = 1; + } + if (mode == WORK_TOP) { + this->yaw1_able_ = 1; + this->pitch1_able_ = 1; + this->pitch2_able_ = 1; + this->roll1_able_ = 1; + this->yaw2_able_ = 1; + } + this->mode_ = mode; +} diff --git a/src/module/robot_arm/mod_robot_arm.hpp b/src/module/robot_arm/mod_robot_arm.hpp new file mode 100644 index 00000000..154bf9b4 --- /dev/null +++ b/src/module/robot_arm/mod_robot_arm.hpp @@ -0,0 +1,116 @@ +#include +#include + +#include "comp_actuator.hpp" +#include "comp_cmd.hpp" +#include "comp_pid.hpp" +#include "dev_damiaomotor.hpp" +#include "dev_motor.hpp" +// #include "dev_referee.hpp" +#include "dev_custom_controller.hpp" +#include "dev_rm_motor.hpp" +#include "module.hpp" + +namespace Module { +class RobotArm { + public: + /*机械臂运行模式*/ + typedef enum { + RELAX, /*放松，电机不输出*/ + WORK_TOP, + WORK_MID, + WORK_BOT, + WORK_CUSTOM_CTRL, + SAFE, + } Mode; + + typedef enum { + SET_MODE_RELAX, + SET_MODE_WORK_TOP, + SET_MODE_WORK_MID, + SET_MODE_WORK_BOT, + SET_MODE_SAFE, + } RobotArmEvent; + + typedef struct Param { + const std::vector EVENT_MAP; + + Component::PosActuator::Param roll2_actr; + + Device::DamiaoMotor::Param yaw1_motor; + Device::DamiaoMotor::Param pitch1_motor; + Device::DamiaoMotor::Param pitch2_motor; + Device::DamiaoMotor::Param roll1_motor; + Device::DamiaoMotor::Param yaw2_motor; + Device::RMMotor::Param roll2_motor; + + struct { + float yaw1_max; /*大yaw,180度活动范围*/ + float yaw1_min; + float pitch1_max; /*pitch1,0,85*/ + float pitch1_min; + float pitch2_max; /*pitch2，-270,0*/ + float pitch2_min; + float yaw2_max; /*小yaw,-90,+90*/ + float yaw2_min; + } limit; + + Device::CustomController::Param cust_ctrl; + } Param; + + RobotArm(Param ¶m, float control_freq); + + void UpdateFeedback(); + + void Control(); + + void DamiaoSetAble(); + + void SetMode(Mode mode); + + private: + Param param_; + + float dt_ = 0.0f; + + uint64_t last_wakeup_ = 0; + + uint64_t now_ = 0; + + Mode mode_ = WORK_BOT; + + Component::PosActuator roll2_actr_; + + Device::DamiaoMotor yaw1_motor_; + Device::DamiaoMotor pitch1_motor_; + Device::DamiaoMotor pitch2_motor_; + Device::DamiaoMotor roll1_motor_; + Device::DamiaoMotor yaw2_motor_; + Device::RMMotor roll2_motor_; + + Device::CustomController custom_ctrl_; + + bool yaw1_able_ = 1; + bool pitch1_able_ = 1; + bool pitch2_able_ = 1; + bool roll1_able_ = 1; + bool yaw2_able_ = 1; + + System::Thread thread_; + + System::Semaphore ctrl_lock_; + + struct { + float yaw1_theta_ = 0; + float pitch1_theta_ = 0; + float pitch2_theta_ = 0; + float roll1_theta_ = 0; + float yaw2_theta_ = 0; + } setpoint_; /*用于接收控制器发来的各关节角度*/ + + float setpoint_roll2_; + + Component::CMD::GimbalCMD cmd_; +}; + +} // namespace Module diff --git a/src/robot/arm_engineer/Kconfig b/src/robot/arm_engineer/Kconfig new file mode 100644 index 00000000..e69de29b diff --git a/src/robot/arm_engineer/robot.cpp b/src/robot/arm_engineer/robot.cpp new file mode 100644 index 00000000..f615ac96 --- /dev/null +++ b/src/robot/arm_engineer/robot.cpp @@ -0,0 +1,250 @@ +#include "robot.hpp" + +#include +#include + +#include "dev_rm_motor.hpp" +// #include "mod_engineer_chassis.hpp" + +using namespace Robot; + +/* clang-format off */ +Robot::ArmEngineer::Param param = { + .blink = { + .gpio = BSP_GPIO_LED, + .timeout = 200, + }, + +.chassis = { + .type = Component::Mixer::MECANUM, + + + .EVENT_MAP = { + Component::CMD::EventMapItem{ + Component::CMD::CMD_EVENT_LOST_CTRL, + Module::RMChassis::SET_MODE_RELAX + }, + Component::CMD::EventMapItem{ + Device::DR16::DR16_SW_L_POS_TOP, + Module::RMChassis::SET_MODE_RELAX + }, + Component::CMD::EventMapItem{ + Device::DR16::DR16_SW_L_POS_MID, + Module::RMChassis::SET_MODE_INDENPENDENT + }, + }, + + .actuator_param = { + Component::SpeedActuator::Param{ + .speed = { + .k = 0.0001f, + .p = 1.0f, + .i = 0.0f, + .d = 0.0f, + .i_limit = 1.0f, + .out_limit = 1.0f, + .d_cutoff_freq = -1.0f, + .cycle = false, + }, + + .in_cutoff_freq = -1.0f, + + .out_cutoff_freq = -1.0f, + + }, + Component::SpeedActuator::Param{ + .speed = { + .k = 0.0001f, + .p = 1.0f, + .i = 0.0f, + .d = 0.0f, + .i_limit = 1.0f, + .out_limit = 1.0f, + .d_cutoff_freq = -1.0f, + .cycle = false, + }, + + .in_cutoff_freq = -1.0f, + + .out_cutoff_freq = -1.0f, + }, + Component::SpeedActuator::Param{ + .speed = { + .k = 0.0001f, + .p = 1.0f, + .i = 0.0f, + .d = 0.0f, + .i_limit = 1.0f, + .out_limit = 1.0f, + .d_cutoff_freq = -1.0f, + .cycle = false, + }, + + .in_cutoff_freq = -1.0f, + + .out_cutoff_freq = -1.0f, + }, + Component::SpeedActuator::Param{ + .speed = { + .k = 0.0001f, + .p = 1.0f, + .i = 0.0f, + .d = 0.0f, + .i_limit = 1.0f, + .out_limit = 1.0f, + .d_cutoff_freq = -1.0f, + .cycle = false, + }, + + .in_cutoff_freq = -1.0f, + + .out_cutoff_freq = -1.0f, + }, + }, + + .motor_param = { + Device::RMMotor::Param{ + .id_feedback = 0x201, + .id_control = M3508_M2006_CTRL_ID_BASE, + .model = Device::RMMotor::MOTOR_M3508, + .can = BSP_CAN_3, + }, + Device::RMMotor::Param{ + .id_feedback = 0x202, + .id_control = M3508_M2006_CTRL_ID_BASE, + .model = Device::RMMotor::MOTOR_M3508, + .can = BSP_CAN_3, + }, + Device::RMMotor::Param{ + .id_feedback = 0x203, + .id_control = M3508_M2006_CTRL_ID_BASE, + .model = Device::RMMotor::MOTOR_M3508, + .can = BSP_CAN_3, + }, + Device::RMMotor::Param{ + .id_feedback = 0x204, + .id_control = M3508_M2006_CTRL_ID_BASE, + .model = Device::RMMotor::MOTOR_M3508, + .can = BSP_CAN_3, + }, + }, + }, + + .robotarm = { + .EVENT_MAP = { + Component::CMD::EventMapItem{ + Component::CMD::CMD_EVENT_LOST_CTRL, + Module::RobotArm::SET_MODE_RELAX + }, + Component::CMD::EventMapItem{ + Device::DR16::DR16_SW_R_POS_TOP, + Module::RobotArm::SET_MODE_WORK_TOP + }, + Component::CMD::EventMapItem{ + Device::DR16::DR16_SW_R_POS_MID, + Module::RobotArm::SET_MODE_WORK_MID + }, + Component::CMD::EventMapItem{ + Device::DR16::DR16_SW_R_POS_BOT, + Module::RobotArm::SET_MODE_WORK_BOT + }, + }, + .roll2_actr={ +.speed = { + .k = 0.1f, + .p = 0.025f, + .i = 0.2f, + .d = 0.0f, + .i_limit = 0.2f, + .out_limit = 1.0f, + .d_cutoff_freq = -1.0f, + .cycle = false, + }, + + .position = { + .k = 40.0f, + .p = 6.1f, + .i = 1.0f, + .d = 0.4f, + .i_limit = 2.0f, + .out_limit = 200.0f, + .d_cutoff_freq = -1.0f, + .cycle = true, + }, + + .in_cutoff_freq = -1.0f, + + .out_cutoff_freq = -1.0f, + }, + +.yaw1_motor={ +.id = 01, +.can = BSP_CAN_1, +.feedback_id = 0, +.reverse = 0, +}, +.pitch1_motor={ + .id = 02, +.can = BSP_CAN_1, +.feedback_id = 1, +.reverse = 0, +}, +.pitch2_motor={ + .id = 03, +.can = BSP_CAN_3, +.feedback_id = 2, +.reverse = 0, +}, +.roll1_motor={ + .id = 04, +.can = BSP_CAN_2, +.feedback_id = 3, +.reverse = 0, +}, +.yaw2_motor={ + .id = 05, +.can = BSP_CAN_2, +.feedback_id = 4, +.reverse = 0, +}, + +.roll2_motor={ + .id_feedback = 0x208, + .id_control = GM6020_CTRL_ID_BASE, + .model = Device::RMMotor::MOTOR_GM6020, + .can = BSP_CAN_2, +}, + +.limit = { + .yaw1_max = 0.0f, + .yaw1_min =-6.28f, + .pitch1_max = 3.5f, + .pitch1_min = 0.0f, + .pitch2_max = 0.0f, + .pitch2_min = -4.7f, + .yaw2_max = 1.57f, + .yaw2_min = -1.57f, + }, + +// mt6701的范围为0-2PI，先修改自定义控制器六个轴的零点，保证在电机内置编码器反馈值最小的时候该轴的mt6701的值接近0（保证不会突变到6.28） +// 再修改下面的offset，例如yaw_1轴电机内置编码器的范围为-6.28到0，该轴的mt6701的值为0.1，则offset为-6.28-0.1 + +.cust_ctrl = { + .offset = { + -6.28f, + 0.0f, + 0.0f, + 0.0f, + 0.0f, + 0.0f + } +} +}, + + +}; +/* clang-format on */ + +void robot_init() { + System::Start(param, 500.0f); +} diff --git a/src/robot/arm_engineer/robot.hpp b/src/robot/arm_engineer/robot.hpp new file mode 100644 index 00000000..3998f1b2 --- /dev/null +++ b/src/robot/arm_engineer/robot.hpp @@ -0,0 +1,37 @@ +#include "comp_cmd.hpp" +#include "dev_blink_led.hpp" +#include "dev_can.hpp" +#include "dev_damiaomotor.hpp" +#include "dev_dr16.hpp" +#include "dev_motor.hpp" +#include "dev_referee.hpp" +#include "mod_engineer_chassis.hpp" +#include "mod_robot_arm.hpp" +void robot_init(); + +namespace Robot { +class ArmEngineer { + public: + typedef struct { + Device::BlinkLED::Param blink; + Module::RMChassis::Param chassis; + Module::RobotArm::Param robotarm; + + } Param; + + Component::CMD cmd_; + + Device::Can can_; + Device::Referee referee_; + Device::DR16 dr16_; + Device::BlinkLED blink_; + Module::RMChassis chassis_; + Module::RobotArm robotarm_; + + ArmEngineer(Param ¶m, float control_freq) + : cmd_(Component::CMD::CMD_OP_CTRL), + blink_(param.blink), + chassis_(param.chassis, control_freq), + robotarm_(param.robotarm, control_freq) {} +}; +} // namespace Robot From 1ee80826219be9c418c1213802a6d2d123c2cfb4 Mon Sep 17 00:00:00 2001 From: iceshadows <2212708+iceshadows@user.noreply.gitee.com> Date: Fri, 31 May 2024 07:13:58 +0000 Subject: [PATCH 2/3] update utils/python/tool.py for windows Signed-off-by: iceshadows <2212708+iceshadows@user.noreply.gitee.com> --- utils/python/tool.py | 42 ++++++++++++++++++++++++++++++------------ 1 file changed, 30 insertions(+), 12 deletions(-) diff --git a/utils/python/tool.py b/utils/python/tool.py index 10a2e8ea..c60e3624 100644 --- a/utils/python/tool.py +++ b/utils/python/tool.py @@ -1,6 +1,7 @@ import sys import os import shutil +import subprocess class ProjectTools: @@ -22,14 +23,24 @@ def __init__(self): exit() def guiconfig(self, path): - print("Start menu config.") - os.system("cd " + path + " && python3 " + self.kconfig_path + "/guiconfig.py") - print("Menu config done.") + print("Start GUI config.") + try: + # 使用sys.executable调用当前虚拟环境的Python解释器 + subprocess.run([sys.executable, self.kconfig_path + "/guiconfig.py"], cwd=path, check=True) + except subprocess.CalledProcessError as e: + print("Failed to run GUI config: ", e) + else: + print("GUI config done.") def menuconfig(self, path): print("Start menu config.") - os.system("cd " + path + " && python3 " + self.kconfig_path + "/menuconfig.py") - print("Menu config done.") + try: + # 使用sys.executable调用当前虚拟环境的Python解释器 + subprocess.run([sys.executable, self.kconfig_path + "/menuconfig.py"], cwd=path, check=True) + except subprocess.CalledProcessError as e: + print("Failed to run menu config: ", e) + else: + print("Menu config done.") def clean_cache(self): filepath = self.project_path + "/build" @@ -48,13 +59,20 @@ def clean_cache(self): shutil.rmtree(file_path) def config_cmake(self, type="Debug"): - os.system( - "cd " - + self.project_path - + " && cmake --no-warn-unused-cli -DCMAKE_TOOLCHAIN_FILE:STRING=utils/CMake/toolchain.cmake -DCMAKE_EXPORT_COMPILE_COMMANDS:BOOL=TRUE -DCMAKE_BUILD_TYPE:STRING=" - + type - + " -Bbuild -G Ninja" - ) + try: + subprocess.run([ + "cmake", + "--no-warn-unused-cli", + f"-DCMAKE_TOOLCHAIN_FILE:STRING=utils/CMake/toolchain.cmake", + "-DCMAKE_EXPORT_COMPILE_COMMANDS:BOOL=TRUE", + f"-DCMAKE_BUILD_TYPE:STRING={type}", + "-Bbuild", + "-G", "Ninja" + ], cwd=self.project_path, check=True) + except subprocess.CalledProcessError as e: + print(f"Failed to configure CMake: {e}") + else: + print("CMake configuration successful.") def config_cmake_idf(self, type="Debug"): os.system( From 34db46b3df9bb923c08faed6a99e83d94a0cd9cc Mon Sep 17 00:00:00 2001 From: Jiu-xiao <2592509183@qq.com> Date: Fri, 31 May 2024 19:04:29 +0800 Subject: [PATCH 3/3] Remove old engineer. --- hw/bsp/rm-c/config/engineer.config | 109 ---- src/module/ore_collect/Kconfig | 4 - src/module/ore_collect/info.cmake | 6 - src/module/ore_collect/mod_ore_collect.cpp | 147 ----- src/module/ore_collect/mod_ore_collect.hpp | 100 --- src/robot/engineer/Kconfig | 0 src/robot/engineer/robot.cpp | 718 --------------------- src/robot/engineer/robot.hpp | 47 -- 8 files changed, 1131 deletions(-) delete mode 100644 hw/bsp/rm-c/config/engineer.config delete mode 100644 src/module/ore_collect/Kconfig delete mode 100644 src/module/ore_collect/info.cmake delete mode 100644 src/module/ore_collect/mod_ore_collect.cpp delete mode 100644 src/module/ore_collect/mod_ore_collect.hpp delete mode 100644 src/robot/engineer/Kconfig delete mode 100644 src/robot/engineer/robot.cpp delete mode 100644 src/robot/engineer/robot.hpp diff --git a/hw/bsp/rm-c/config/engineer.config b/hw/bsp/rm-c/config/engineer.config deleted file mode 100644 index c7b132a8..00000000 --- a/hw/bsp/rm-c/config/engineer.config +++ /dev/null @@ -1,109 +0,0 @@ -# CONFIG_auto_generated_config_prefix_board-c-mini is not set -# CONFIG_auto_generated_config_prefix_board-esp32-c3 is not set -# CONFIG_auto_generated_config_prefix_board-node_imu is not set -# CONFIG_auto_generated_config_prefix_board-f103_can is not set -# CONFIG_auto_generated_config_prefix_board-MiniPC is not set -CONFIG_auto_generated_config_prefix_board-rm-c=y -# CONFIG_auto_generated_config_prefix_board-microswitch is not set -# CONFIG_auto_generated_config_prefix_board-Webots is not set -# CONFIG_auto_generated_config_prefix_board-demo-board is not set -# CONFIG_auto_generated_config_prefix_system-Linux_Webots is not set -CONFIG_auto_generated_config_prefix_system-FreeRTOS=y -# CONFIG_auto_generated_config_prefix_system-None is not set -# CONFIG_auto_generated_config_prefix_system-Linux is not set -CONFIG_INIT_TASK_STACK_DEPTH=1536 - -# -# FreeRTOS -# -CONFIG_FREERTOS_TIMER_TASK_STACK_DEPTH=512 -CONFIG_FREERTOS_USB_TASK_STACK_DEPTH=256 -CONFIG_FREERTOS_TERM_TASK_STACK_DEPTH=512 -# end of FreeRTOS - -# CONFIG_auto_generated_config_prefix_robot-hero is not set -# CONFIG_auto_generated_config_prefix_robot-balance_infantry is not set -# CONFIG_auto_generated_config_prefix_robot-sentry is not set -# CONFIG_auto_generated_config_prefix_robot-dart is not set -# CONFIG_auto_generated_config_prefix_robot-can_to_uart is not set -# CONFIG_auto_generated_config_prefix_robot-infantry is not set -# CONFIG_auto_generated_config_prefix_robot-sim_mecanum is not set -# CONFIG_auto_generated_config_prefix_robot-blink is not set -# CONFIG_auto_generated_config_prefix_robot-microswitch is not set -# CONFIG_auto_generated_config_prefix_robot-sim_balance is not set -CONFIG_auto_generated_config_prefix_robot-engineer=y -# CONFIG_auto_generated_config_prefix_robot-wearlab_imu is not set -# CONFIG_auto_generated_config_prefix_robot-demo-robot is not set - -# -# 组件 -# -# CONFIG_auto_generated_config_prefix_component-demo-component is not set -# end of 组件 - -# -# 设备 -# -# CONFIG_auto_generated_config_prefix_device-servo is not set -# CONFIG_auto_generated_config_prefix_device-simulator is not set -CONFIG_REF_LAUNCH_SPEED=30 -CONFIG_REF_HEAT_LIMIT_17=100 -CONFIG_REF_HEAT_LIMIT_42=100 -CONFIG_REF_POWER_LIMIT=200 -CONFIG_REF_POWER_BUFF=100 -# CONFIG_auto_generated_config_prefix_device-cap is not set -# CONFIG_auto_generated_config_prefix_device-led_rgb is not set -CONFIG_auto_generated_config_prefix_device-referee=y -CONFIG_DEVICE_REF_TRANS_TASK_STACK_DEPTH=256 -CONFIG_DEVICE_REF_RECV_TASK_STACK_DEPTH=256 - -# -# 裁判系统 -# -# CONFIG_REF_VIRTUAL is not set -# end of 裁判系统 - -# -# 操作手UI -# -CONFIG_UI_DYNAMIC_CYCLE=20 -CONFIG_UI_STATIC_CYCLE=1000 -# end of 操作手UI - -# CONFIG_auto_generated_config_prefix_device-laser is not set -CONFIG_auto_generated_config_prefix_device-can=y -CONFIG_auto_generated_config_prefix_device-motor=y -# CONFIG_auto_generated_config_prefix_device-bmi088 is not set -CONFIG_auto_generated_config_prefix_device-mech=y -# CONFIG_auto_generated_config_prefix_device-buzzer is not set -# CONFIG_auto_generated_config_prefix_device-tof is not set -CONFIG_auto_generated_config_prefix_device-blink_led=y -CONFIG_auto_generated_config_prefix_device-dr16=y -CONFIG_DEVICE_DR16_TASK_STACK_DEPTH=384 -CONFIG_auto_generated_config_prefix_device-ahrs=y -CONFIG_DEVICE_AHRS_TASK_STACK_DEPTH=256 -# CONFIG_auto_generated_config_prefix_device-ai is not set -# CONFIG_auto_generated_config_prefix_device-wearlab is not set -CONFIG_auto_generated_config_prefix_device-imu=y -CONFIG_DEVICE_CAN_IMU_TASK_STACK_DEPTH=256 -CONFIG_auto_generated_config_prefix_device-microswitch=y -# CONFIG_auto_generated_config_prefix_device-demo-device is not set -# end of 设备 - -# -# 模块 -# -# CONFIG_auto_generated_config_prefix_module-gimbal is not set -# CONFIG_auto_generated_config_prefix_module-balance is not set -# CONFIG_auto_generated_config_prefix_module-chassis is not set -# CONFIG_auto_generated_config_prefix_module-dart_launcher is not set -# CONFIG_auto_generated_config_prefix_module-launcher is not set -CONFIG_auto_generated_config_prefix_module-ore_collect=y -CONFIG_MODULE_ORE_TASK_STACK_DEPTH=384 -# CONFIG_auto_generated_config_prefix_module-can_usart is not set -# CONFIG_auto_generated_config_prefix_module-microswitch is not set -# CONFIG_auto_generated_config_prefix_module-can_imu is not set -# CONFIG_auto_generated_config_prefix_module-wheel_leg is not set -# CONFIG_auto_generated_config_prefix_module-dart_gimbal is not set -# CONFIG_auto_generated_config_prefix_module-demo-module is not set -# end of 模块 diff --git a/src/module/ore_collect/Kconfig b/src/module/ore_collect/Kconfig deleted file mode 100644 index 689a543b..00000000 --- a/src/module/ore_collect/Kconfig +++ /dev/null @@ -1,4 +0,0 @@ -config MODULE_ORE_TASK_STACK_DEPTH - int "ORE_COLLECT任务堆栈大小" - range 128 4096 - default 384 diff --git a/src/module/ore_collect/info.cmake b/src/module/ore_collect/info.cmake deleted file mode 100644 index 35463fe0..00000000 --- a/src/module/ore_collect/info.cmake +++ /dev/null @@ -1,6 +0,0 @@ -CHECK_SUB_ENABLE(MODULE_ENABLE module) -if(${MODULE_ENABLE}) - file(GLOB CUR_SOURCES "${SUB_DIR}/*.cpp") - SUB_ADD_SRC(CUR_SOURCES) - SUB_ADD_INC(SUB_DIR) -endif() \ No newline at end of file diff --git a/src/module/ore_collect/mod_ore_collect.cpp b/src/module/ore_collect/mod_ore_collect.cpp deleted file mode 100644 index 60a3eb62..00000000 --- a/src/module/ore_collect/mod_ore_collect.cpp +++ /dev/null @@ -1,147 +0,0 @@ -#include "mod_ore_collect.hpp" - -#include "bsp_gpio.h" -#include "bsp_time.h" -#include "comp_cmd.hpp" - -using namespace Module; - -Device::PosStream ps = { - .target_angle_ = {0.0, 0.0f / 180.0f * M_PI, 0.0}, - .target_pos_ = {0.0, 0.05, 0.2}, - .angle_ = {0.0, 0.0, 0.0}, -}; - -OreCollect::OreCollect(Param& param, float control_freq) - : ctrl_lock_(true), - param_(param), - x_actr_(param_.x_actr, control_freq), - pitch_actr_(param.pitch_actr, control_freq), - pitch_1_actr_(param.pitch_1_actr, control_freq), - yaw_actr_(param.yaw_actr, control_freq), - roll_actr_(param.roll_actr, control_freq), - y_actr_(param.y_actr, control_freq), - z_actr_(param.z_actr, control_freq), - z_1_actr_(param.z_1_actr, control_freq) { - auto event_callback = [](Collect_Event event, OreCollect* ore) { - switch (event) { - case START_VACUUM: - bsp_gpio_write_pin(BSP_GPIO_SWITCH, true); - break; - case STOP_VACUUM: - bsp_gpio_write_pin(BSP_GPIO_SWITCH, false); - break; - case RESET: - ore->mode_ = RELAX; - break; - case FOLD: - ore->mode_ = CALI; - ore->setpoint_.x = 0.13f; - ore->setpoint_.pitch = 0.0f; - ore->setpoint_.pitch_1 = 1.5f; - ore->setpoint_.yaw = 0.0f; - ore->setpoint_.roll = 0.0f; - ore->setpoint_.y = 0.02f; - ore->setpoint_.z = 0.0f; - ore->setpoint_.z_1 = 0.0f; - break; - case WORK: - ore->mode_ = MOVE; - case START_AUTO_COLLECT: - Component::CMD::SetCtrlSource(Component::CMD::CTRL_SOURCE_AI); - case STOP_AUTO_COLLECT: - Component::CMD::SetCtrlSource(Component::CMD::CTRL_SOURCE_EXT); - break; - } - }; - - Component::CMD::RegisterEvent( - event_callback, this, this->param_.EVENT_MAP); - - auto ore_thread = [](OreCollect* ore) { - auto eulr_sub = - Message::Subscriber("custom_ctrl_eulr"); - - uint32_t last_online_time = bsp_time_get_ms(); - - while (1) { - /* 更新反馈值 */ - eulr_sub.DumpData(ore->eulr_); - ore->UpdateFeedback(); - ore->Control(); - - /* 运行结束，等待下一次唤醒 */ - ore->thread_.SleepUntil(2, last_online_time); - } - }; - - this->thread_.Create(ore_thread, this, "ore_thread", - MODULE_ORE_TASK_STACK_DEPTH, System::Thread::MEDIUM); -} - -void OreCollect::Control() { - this->now_ = bsp_time_get(); - - this->dt_ = TIME_DIFF(this->last_wakeup_, this->now_); - - this->last_wakeup_ = this->now_; - - switch (mode_) { - case RELAX: - x_actr_.Relax(); - pitch_actr_.Relax(); - pitch_1_actr_.Relax(); - yaw_actr_.Relax(); - roll_actr_.Relax(); - y_actr_.Relax(); - z_actr_.Relax(); - z_1_actr_.Relax(); - break; - case CALI: { - x_actr_.Control(setpoint_.x, dt_); - pitch_actr_.Control(setpoint_.pitch, dt_); - pitch_1_actr_.Control(setpoint_.pitch_1, dt_); - if (roll_actr_.Ready()) { - yaw_actr_.Control(setpoint_.yaw, dt_); - } else { - yaw_actr_.Relax(); - } - if (pitch_actr_.Ready()) { - if (!yaw_actr_.Ready()) { - roll_actr_.Control(-M_PI * 0.4, dt_); - } else { - roll_actr_.Control(setpoint_.roll, dt_); - } - } else { - roll_actr_.Relax(); - } - y_actr_.Control(setpoint_.y, dt_); - z_actr_.Control(setpoint_.z, dt_); - z_1_actr_.Control(setpoint_.z_1, dt_); - break; - } - case MOVE: - ps.angle_ = {0.0, 0.0, 0.0}; - ps.pos_ = param_.zero_position; - pitch_1_actr_.GroupControl(ps, pitch_actr_, pitch_1_actr_) >> yaw_actr_ >> - roll_actr_ >> x_actr_ >> y_actr_ >> z_actr_ >> z_1_actr_; - break; - } - - ps.angle_ = {0.0, 0.0, 0.0}; - ps.pos_ = param_.zero_position; - ps.target_angle_.pit = eulr_.pit - Component::Type::CycleValue(0.0f); - ps.target_angle_.rol = eulr_.rol - Component::Type::CycleValue(0.0f); - ps.target_angle_.yaw = -(eulr_.yaw - Component::Type::CycleValue(0.0f)); -} - -void OreCollect::UpdateFeedback() { - x_actr_.UpdateFeedback(); - pitch_actr_.UpdateFeedback(); - pitch_1_actr_.UpdateFeedback(); - yaw_actr_.UpdateFeedback(); - roll_actr_.UpdateFeedback(); - y_actr_.UpdateFeedback(); - z_actr_.UpdateFeedback(); - z_1_actr_.UpdateFeedback(); -} diff --git a/src/module/ore_collect/mod_ore_collect.hpp b/src/module/ore_collect/mod_ore_collect.hpp deleted file mode 100644 index bc67f3b2..00000000 --- a/src/module/ore_collect/mod_ore_collect.hpp +++ /dev/null @@ -1,100 +0,0 @@ -#include - -#include "comp_cmd.hpp" -#include "dev_mech.hpp" -#include "dev_rm_motor.hpp" -#include "module.hpp" - -namespace Module { -class OreCollect { - public: - typedef struct { - const std::vector EVENT_MAP; - Device::LinearMech::Param - x_actr; - Device::SteeringMech::Param - pitch_actr; - Device::SteeringMech::Param - pitch_1_actr; - Device::SteeringMech::Param - yaw_actr; - Device::SteeringMech::Param - roll_actr; - - Device::LinearMech::Param - y_actr; - Device::LinearMech::Param - z_actr; - Device::LinearMech::Param - z_1_actr; - - Component::Type::Vector3 zero_position; - } Param; - - typedef struct { - float x; - float pitch; - float pitch_1; - float yaw; - float roll; - float z; - float z_1; - float y; - } Setpoint; - - typedef enum { - RESET, - FOLD, - WORK, - START_VACUUM, - STOP_VACUUM, - START_AUTO_COLLECT, - STOP_AUTO_COLLECT, - } Collect_Event; - - typedef enum { RELAX, CALI, MOVE } Mode; - - OreCollect(Param& param, float control_freq); - - void Control(); - - void UpdateFeedback(); - - private: - System::Semaphore ctrl_lock_; - - Param& param_; - - float dt_ = 0.0f; - - uint64_t last_wakeup_ = 0; - - uint64_t now_ = 0; - - Mode mode_ = RELAX; - - Setpoint setpoint_; - - Component::Type::Eulr eulr_; - - Device::LinearMech x_actr_; - - Device::SteeringMech - pitch_actr_; - - Device::SteeringMech - pitch_1_actr_; - - Device::SteeringMech yaw_actr_; - - Device::SteeringMech roll_actr_; - - Device::LinearMech y_actr_; - - Device::LinearMech z_actr_; - - Device::LinearMech z_1_actr_; - - System::Thread thread_; -}; -} // namespace Module diff --git a/src/robot/engineer/Kconfig b/src/robot/engineer/Kconfig deleted file mode 100644 index e69de29b..00000000 diff --git a/src/robot/engineer/robot.cpp b/src/robot/engineer/robot.cpp deleted file mode 100644 index 4ca00abd..00000000 --- a/src/robot/engineer/robot.cpp +++ /dev/null @@ -1,718 +0,0 @@ -#include "robot.hpp" - -#include -#include - -#include "dev_rm_motor.hpp" - -using namespace Robot; - -/* clang-format off */ -Robot::Engineer::Param param = { - .led = { - .gpio = BSP_GPIO_LED, - .timeout = 200, - }, - - .sw_2 = { - .can = BSP_CAN_2, - .id = 3, - }, - - - .sw_3 = { - .can = BSP_CAN_2, - .id = 4, - }, - - .sw_4 = { - .can = BSP_CAN_2, - .id = 5, - }, - - .imu = { - .tp_name_prefix = "custom_ctrl", - .can = BSP_CAN_2, - .index = 31, - }, - - .ore_collect = { - .EVENT_MAP = { - Component::CMD::EventMapItem{ - Component::CMD::CMD_EVENT_LOST_CTRL, - Module::OreCollect::RESET - }, - Component::CMD::EventMapItem{ - Device::DR16::DR16_SW_L_POS_TOP, - Module::OreCollect::FOLD - }, - Component::CMD::EventMapItem{ - Device::DR16::DR16_SW_L_POS_MID, - Module::OreCollect::WORK - } - }, - - .x_actr = { - .limit_param = { - Device::MicroSwitchLimit::Param{ - .id = 14, - }, - }, - .pos_actuator = { - Component::PosActuator::Param{ - .speed = { - .k = 0.0002f, - .p = 1.0f, - .i = 0.5f, - .d = 0.0f, - .i_limit = 0.5f, - .out_limit = 0.5f, - .d_cutoff_freq = -1.0f, - .cycle = false, - }, - - .position = { - .k = 120000.0f, - .p = 1.0f, - .i = 0.3f, - .d = 0.0f, - .i_limit = 1000.0f, - .out_limit = 5000.0f, - .d_cutoff_freq = -1.0f, - .cycle = false, - }, - - .in_cutoff_freq = 10.0f, - - .out_cutoff_freq = 30.0f, - }, - }, - - .motor_param = { - Device::RMMotor::Param{ - .id_feedback = 0x201, - .id_control = M3508_M2006_CTRL_ID_BASE, - .model = Device::RMMotor::MOTOR_M2006, - .can = BSP_CAN_2, - .reverse = true, - }, - }, - - .motor_name = { - "x_actr" - }, - - .cali_speed = -3000.0f, - - .max_distance = 25.7f * 0.01f, - - .margin_error = 0.0f, - - .zero_position = 0.0f, - - .reduction_ratio = 22.896133278f * 100.0f, - - .axis = Device::AXIS_X - }, - - .pitch_actr = { - .limit_param = { - Device::MicroSwitchLimit::Param{ - .id = 19, - }, - }, - .pos_actuator = { - Component::PosActuator::Param{ - .speed = { - .k = 0.0001f, - .p = 1.0f, - .i = 0.5f, - .d = 0.0f, - .i_limit = 1.0f, - .out_limit = 1.0f, - .d_cutoff_freq = -1.0f, - .cycle = false, - }, - - .position = { - .k = 120000.0f, - .p = 1.0f, - .i = 0.3f, - .d = 0.0f, - .i_limit = 1000.0f, - .out_limit = 5000.0f, - .d_cutoff_freq = -1.0f, - .cycle = false, - }, - - .in_cutoff_freq = 10.0f, - - .out_cutoff_freq = 30.0f, - }, - }, - - .motor_param = { - Device::RMMotor::Param{ - .id_feedback = 0x204, - .id_control = M3508_M2006_CTRL_ID_BASE, - .model = Device::RMMotor::MOTOR_M2006, - .can = BSP_CAN_2, - .reverse = true, - }, - }, - - .motor_name = { - "pitch_actr" - }, - - .cali_speed = -3000.0f, - - .min_angle = -66.0f / 180.0f * M_PI, - - .max_angle = 95.0f / 180.0f * M_PI, - - .margin_error = 0.0f, - - .zero_angle = -66.0f / 180.0f * M_PI, - - .reduction_ratio = 19.252851377f / M_PI * 180.0f, - - .translation = { - .x = 0.0f, - .y = 0.085f, - .z = 0.0f, - }, - - .axis = Device::AXIS_X - }, - - .pitch_1_actr = { - .limit_param = { - Device::MicroSwitchLimit::Param{ - .id = 15, - }, - }, - .pos_actuator = { - Component::PosActuator::Param{ - .speed = { - .k = 0.0001f, - .p = 1.0f, - .i = 0.5f, - .d = 0.0f, - .i_limit = 0.5f, - .out_limit = 0.5f, - .d_cutoff_freq = -1.0f, - .cycle = false, - }, - - .position = { - .k = 30000.0f, - .p = 1.0f, - .i = 0.3f, - .d = 0.0f, - .i_limit = 1000.0f, - .out_limit = 5000.0f, - .d_cutoff_freq = -1.0f, - .cycle = false, - }, - - .in_cutoff_freq = 10.0f, - - .out_cutoff_freq = 30.0f, - }, - }, - - .motor_param = { - Device::RMMotor::Param{ - .id_feedback = 0x206, - .id_control = M3508_M2006_CTRL_ID_EXTAND, - .model = Device::RMMotor::MOTOR_M3508, - .can = BSP_CAN_2, - }, - }, - - .motor_name = { - "pitch_1_actr" - }, - - .cali_speed = -3000.0f, - - .min_angle = -52.8f / 180.0f * M_PI, - - .max_angle = 177.6f / 180.0f * M_PI, - - .margin_error = 0.0f, - - .zero_angle = -52.8f / 180.0f * M_PI, - - .reduction_ratio = 10.07565889f / M_PI * 180.0f, - - .translation = { - .x = 0.0f, - .y = 0.2f, - .z = 0.055f, - }, - - .axis = Device::AXIS_X - }, - - .yaw_actr = { - .limit_param = { - Device::MicroSwitchLimit::Param{ - .id = 17, - }, - }, - .pos_actuator = { - Component::PosActuator::Param{ - .speed = { - .k = 0.0001f, - .p = 1.0f, - .i = 0.1f, - .d = 0.0f, - .i_limit = 0.5f, - .out_limit = 0.8f, - .d_cutoff_freq = -1.0f, - .cycle = false, - }, - - .position = { - .k = 120000.0f, - .p = 1.0f, - .i = 0.3f, - .d = 0.0f, - .i_limit = 1000.0f, - .out_limit = 6000.0f, - .d_cutoff_freq = -1.0f, - .cycle = false, - }, - - .in_cutoff_freq = 10.0f, - - .out_cutoff_freq = 30.0f, - }, - }, - - .motor_param = { - Device::RMMotor::Param{ - .id_feedback = 0x203, - .id_control = M3508_M2006_CTRL_ID_BASE, - .model = Device::RMMotor::MOTOR_M2006, - .can = BSP_CAN_2, - }, - }, - - .motor_name = { - "yaw_actr" - }, - - .cali_speed = -5000.0f, - - .min_angle = -90.0f / 180.0f * M_PI, - - .max_angle = 90.0f / 180.0f * M_PI, - - .margin_error = 0.0f, - - .zero_angle = -90.0f / 180.0f * M_PI, - - .reduction_ratio = 19.031800126f / M_PI * 180.0f, - - .translation = { - .x = 0.0f, - .y = 0.23f, - .z = 0.0f, - }, - - .axis = Device::AXIS_Z, - }, - - .roll_actr = { - .limit_param = { - Device::MicroSwitchLimit::Param{ - .id = 18, - }, - }, - .pos_actuator = { - Component::PosActuator::Param{ - .speed = { - .k = 0.0001f, - .p = 1.0f, - .i = 0.5f, - .d = 0.0f, - .i_limit = 0.5f, - .out_limit = 0.8f, - .d_cutoff_freq = -1.0f, - .cycle = false, - }, - - .position = { - .k = 3000.0f, - .p = 1.0f, - .i = 0.6f, - .d = 0.0f, - .i_limit = 300.0f, - .out_limit = 5000.0f, - .d_cutoff_freq = -1.0f, - .cycle = false, - }, - - .in_cutoff_freq = 10.0f, - - .out_cutoff_freq = 30.0f, - }, - }, - - .motor_param = { - Device::RMMotor::Param{ - .id_feedback = 0x202, - .id_control = M3508_M2006_CTRL_ID_BASE, - .model = Device::RMMotor::MOTOR_M2006, - .can = BSP_CAN_2, - }, - }, - - .motor_name = { - "roll_actr" - }, - - .cali_speed = 3000.0f, - - .min_angle = -180.0f / 180.0f * M_PI, - - .max_angle = 96.9 / 180.0f * M_PI, - - .margin_error = 0.0f, - - .zero_angle = 96.9 / 180.0f * M_PI, - - .reduction_ratio = 36.0f, - - .translation = { - .x = 0.0f, - .y = 0.0f, - .z = 0.0f, - }, - - .axis = Device::AXIS_Y, - }, - - .y_actr = { - .limit_param = { - Device::MicroSwitchLimit::Param{ - .id = 20, - }, - Device::MicroSwitchLimit::Param{ - .id = 23, - }, - }, - .pos_actuator = { - Component::PosActuator::Param{ - .speed = { - .k = 0.0001f, - .p = 1.0f, - .i = 0.5f, - .d = 0.0f, - .i_limit = 0.5f, - .out_limit = 0.8f, - .d_cutoff_freq = -1.0f, - .cycle = false, - }, - - .position = { - .k = 120000.0f, - .p = 1.0f, - .i = 0.3f, - .d = 0.0f, - .i_limit = 1000.0f, - .out_limit = 5000.0f, - .d_cutoff_freq = -1.0f, - .cycle = false, - }, - - .in_cutoff_freq = 10.0f, - - .out_cutoff_freq = 30.0f, - }, - Component::PosActuator::Param{ - .speed = { - .k = 0.0001f, - .p = 1.0f, - .i = 0.5f, - .d = 0.0f, - .i_limit = 0.5f, - .out_limit = 0.8f, - .d_cutoff_freq = -1.0f, - .cycle = false, - }, - - .position = { - .k = 120000.0f, - .p = 1.0f, - .i = 0.3f, - .d = 0.0f, - .i_limit = 1000.0f, - .out_limit = 3000.0f, - .d_cutoff_freq = -1.0f, - .cycle = false, - }, - - .in_cutoff_freq = 10.0f, - - .out_cutoff_freq = 30.0f, - }, - }, - - .motor_param = { - Device::RMMotor::Param{ - .id_feedback = 0x208, - .id_control = M3508_M2006_CTRL_ID_EXTAND, - .model = Device::RMMotor::MOTOR_M2006, - .can = BSP_CAN_1, - .reverse = true, - }, - Device::RMMotor::Param{ - .id_feedback = 0x207, - .id_control = M3508_M2006_CTRL_ID_EXTAND, - .model = Device::RMMotor::MOTOR_M2006, - .can = BSP_CAN_2, - .reverse = false, - }, - }, - - .motor_name = { - "y_1_actr", - "y_2_actr" - }, - - .cali_speed = -3000.0f, - - .max_distance = 22.0f * 0.01f, - - .margin_error = 0.0f, - - .zero_position = 0.0f, - - .reduction_ratio = 47.579500315f * 100.0f, - - .axis = Device::AXIS_Y, - }, - - .z_actr = { - .limit_param = { - Device::AutoReturnLimit::Param{ - .timeout = 4.0, - }, - Device::AutoReturnLimit::Param{ - .timeout = 4.0, - }, - }, - - .pos_actuator = { - Component::PosActuator::Param{ - .speed = { - .k = 0.00015f, - .p = 1.0f, - .i = 0.3f, - .d = 0.0f, - .i_limit = 0.5f, - .out_limit = 0.8f, - .d_cutoff_freq = -1.0f, - .cycle = false, - }, - - .position = { - .k = 60000.0f, - .p = 1.0f, - .i = 0.1f, - .d = 0.0f, - .i_limit = 1000.0f, - .out_limit = 3000.0f, - .d_cutoff_freq = -1.0f, - .cycle = false, - }, - - .in_cutoff_freq = 10.0f, - - .out_cutoff_freq = 30.0f, - }, - Component::PosActuator::Param{ - .speed = { - .k = 0.00015f, - .p = 1.0f, - .i = 0.3f, - .d = 0.0f, - .i_limit = 0.5f, - .out_limit = 0.8f, - .d_cutoff_freq = -1.0f, - .cycle = false, - }, - - .position = { - .k = 60000.0f, - .p = 1.0f, - .i = 0.1f, - .d = 0.0f, - .i_limit = 1000.0f, - .out_limit = 3000.0f, - .d_cutoff_freq = -1.0f, - .cycle = false, - }, - - .in_cutoff_freq = 10.0f, - - .out_cutoff_freq = 30.0f, - }, - }, - - .motor_param = { - Device::RMMotor::Param{ - .id_feedback = 0x208, - .id_control = M3508_M2006_CTRL_ID_EXTAND, - .model = Device::RMMotor::MOTOR_M3508, - .can = BSP_CAN_2, - .reverse = false, - }, - Device::RMMotor::Param{ - .id_feedback = 0x205, - .id_control = M3508_M2006_CTRL_ID_EXTAND, - .model = Device::RMMotor::MOTOR_M3508, - .can = BSP_CAN_2, - .reverse = true, - }, - }, - - .motor_name = { - "z_1_1_actr", - "z_1_2_actr" - }, - - .cali_speed = -0.0f, - - .max_distance = 17.0 * 0.01f, - - .margin_error = 0.0f, - - .zero_position = 0.0f, - - .reduction_ratio = 9.236020649f * 100.0f, - - .axis = Device::AXIS_Z, - }, - - .z_1_actr = { - .limit_param = { - Device::AutoReturnLimit::Param{ - .timeout = 1.5, - }, - Device::AutoReturnLimit::Param{ - .timeout = 1.5, - }, - }, - - .pos_actuator = { - Component::PosActuator::Param{ - .speed = { - .k = 0.00015f, - .p = 1.0f, - .i = 0.3f, - .d = 0.0f, - .i_limit = 0.5f, - .out_limit = 0.8f, - .d_cutoff_freq = -1.0f, - .cycle = false, - }, - - .position = { - .k = 60000.0f, - .p = 1.0f, - .i = 0.1f, - .d = 0.0f, - .i_limit = 1000.0f, - .out_limit = 3000.0f, - .d_cutoff_freq = -1.0f, - .cycle = false, - }, - - .in_cutoff_freq = 10.0f, - - .out_cutoff_freq = 30.0f, - }, - Component::PosActuator::Param{ - .speed = { - .k = 0.00015f, - .p = 1.0f, - .i = 0.3f, - .d = 0.0f, - .i_limit = 0.5f, - .out_limit = 0.8f, - .d_cutoff_freq = -1.0f, - .cycle = false, - }, - - .position = { - .k = 60000.0f, - .p = 1.0f, - .i = 0.1f, - .d = 0.0f, - .i_limit = 1000.0f, - .out_limit = 3000.0f, - .d_cutoff_freq = -1.0f, - .cycle = false, - }, - - .in_cutoff_freq = 10.0f, - - .out_cutoff_freq = 30.0f, - }, - }, - - .motor_param = { - Device::RMMotor::Param{ - .id_feedback = 0x205, - .id_control = M3508_M2006_CTRL_ID_EXTAND, - .model = Device::RMMotor::MOTOR_M3508, - .can = BSP_CAN_1, - .reverse = true, - }, - Device::RMMotor::Param{ - .id_feedback = 0x206, - .id_control = M3508_M2006_CTRL_ID_EXTAND, - .model = Device::RMMotor::MOTOR_M3508, - .can = BSP_CAN_1, - .reverse = false, - }, - }, - - .motor_name = { - "z_2_1_actr", - "z_2_2_actr" - }, - - .cali_speed = -500.0f, - - .max_distance = 35.0f * 0.01f, - - .margin_error = 0.0f, - - .zero_position = 0.0f, - - .reduction_ratio = 9.923724351f * 100.0f, - - .axis = Device::AXIS_Z, - }, - - .zero_position = { - .x = -0.13f, - .y = -0.08f, - .z = 0.355f, - }, - - - }, - -}; -/* clang-format on */ -Robot::Engineer* Robot::Engineer::self_; -void robot_init() { - System::Start(param, 500.0f); -} diff --git a/src/robot/engineer/robot.hpp b/src/robot/engineer/robot.hpp deleted file mode 100644 index fcd07b56..00000000 --- a/src/robot/engineer/robot.hpp +++ /dev/null @@ -1,47 +0,0 @@ -#include - -#include "dev_blink_led.hpp" -#include "dev_can.hpp" -#include "dev_can_imu.hpp" -#include "dev_dr16.hpp" -#include "dev_referee.hpp" -#include "mod_ore_collect.hpp" - -void robot_init(); - -namespace Robot { -class Engineer { - public: - typedef struct Param { - Device::BlinkLED::Param led{}; - Device::MicroSwitch::Param sw_2{}; - Device::MicroSwitch::Param sw_3{}; - Device::MicroSwitch::Param sw_4{}; - Device::IMU::Param imu{}; - Module::OreCollect::Param ore_collect{}; - } Param; - - Component::CMD cmd_; - Device::BlinkLED led_; - Device::Can can_{}; - Device::IMU imu_; - Device::DR16 dr16_{}; - Device::Referee referee_{}; - Device::MicroSwitch sw_2; - Device::MicroSwitch sw_3; - Device::MicroSwitch sw_4; - Module::OreCollect ore_collect_; - - static Engineer* self_; - - Engineer(Param& param, float control_freq) - : led_(param.led), - imu_(param.imu), - sw_2(param.sw_2), - sw_3(param.sw_3), - sw_4(param.sw_4), - ore_collect_(param.ore_collect, control_freq) { - self_ = this; - } -}; -} // namespace Robot