From 71128a2f429b548e1324c32957b9aad7031974d7 Mon Sep 17 00:00:00 2001 From: llLeo306 Date: Mon, 28 Apr 2025 21:57:29 +0800 Subject: [PATCH 1/2] =?UTF-8?q?=E5=93=A8=E5=85=B5=E6=89=80=E6=9C=89?= =?UTF-8?q?=E6=9B=B4=E6=94=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/device/ai/dev_ai.cpp | 222 ++++++++-------- src/device/ai/dev_ai.hpp | 70 +++-- src/device/referee/dev_referee.cpp | 5 +- src/device/referee/dev_referee.hpp | 98 +++---- src/module/chassis/mod_chassis.cpp | 347 +++++++++++++------------ src/module/chassis/mod_chassis.hpp | 49 ++-- src/module/gimbal/mod_gimbal.cpp | 131 +++++++++- src/module/gimbal/mod_gimbal.hpp | 41 ++- src/module/launcher/mod_launcher.cpp | 115 ++++---- src/module/launcher/mod_launcher.hpp | 38 +-- src/robot/sentry/robot.cpp | 375 +++++++++++---------------- src/robot/sentry/robot.hpp | 9 +- 12 files changed, 791 insertions(+), 709 deletions(-) diff --git a/src/device/ai/dev_ai.cpp b/src/device/ai/dev_ai.cpp index 6acd13f0..f13a0fee 100644 --- a/src/device/ai/dev_ai.cpp +++ b/src/device/ai/dev_ai.cpp @@ -8,8 +8,7 @@ #define AI_CMD_LIMIT (0.08f) #define AI_CTRL_SENSE (1.0f / 90.0f) #define AI_LEN_RX_BUFF (sizeof(Protocol_DownPackage_t)) -#define AI_LEN_TX_BUFF \ - (sizeof(Protocol_UpPackageMCU_t) + sizeof(Protocol_UpPackageReferee_t)) +#define AI_LEN_TX_BUFF (sizeof(Protocol_UpPackage_t)) static uint8_t rxbuf[AI_LEN_RX_BUFF]; static uint8_t txbuf[AI_LEN_TX_BUFF]; @@ -32,11 +31,8 @@ AI::AI(bool autoscan_enable) Component::CMD::RegisterController(this->cmd_tp_); auto ai_thread = [](AI *ai) { - auto quat_sub = - Message::Subscriber("imu_quat"); auto ref_sub = Message::Subscriber("referee"); auto yaw_sub = Message::Subscriber("chassis_yaw"); - auto eulr_sub = Message::Subscriber("imu_eulr"); while (1) { @@ -63,7 +59,7 @@ AI::AI(bool autoscan_enable) ai->ai_tp_.Publish(ai->cmd_for_ref_); /* 发送数据到上位机 */ - quat_sub.DumpData(ai->quat_); + // quat_sub.DumpData(ai->quat_); ai->PackMCU(); ai->StartTrans(); @@ -92,14 +88,12 @@ bool AI::PraseHost() { } bool AI::StartTrans() { - size_t len = sizeof(this->to_host_.mcu); + this->to_host_.crc16 = Component::CRC16::Calculate( + reinterpret_cast(&(this->to_host_)), + sizeof(this->to_host_) - sizeof(uint16_t), CRC16_INIT); + size_t len = sizeof(this->to_host_); void *src = NULL; - if (this->ref_updated_) { - len += sizeof(this->to_host_.ref); - src = &(this->to_host_); - } else { - src = &(this->to_host_.mcu); - } + src = &(to_host_); this->ref_updated_ = false; memcpy(txbuf, src, len); @@ -109,7 +103,6 @@ bool AI::StartTrans() { bool AI::Offline() { /* 离线移交控制权 */ if (bsp_time_get_ms() - this->last_online_time_ > 200) { - memset(&this->cmd_, 0, sizeof(cmd_)); if (!this->autoscan_enable_) { this->cmd_.online = true; } else { @@ -122,30 +115,35 @@ bool AI::Offline() { } bool AI::PackMCU() { - this->to_host_.mcu.id = AI_ID_MCU; - memcpy(&(this->to_host_.mcu.package.data.quat), &(this->quat_), - sizeof(this->quat_)); - this->to_host_.mcu.package.data.notice = this->notice_for_ai_; - this->to_host_.mcu.package.crc16 = Component::CRC16::Calculate( - reinterpret_cast(&(this->to_host_.mcu.package)), - sizeof(this->to_host_.mcu.package) - sizeof(uint16_t), CRC16_INIT); + float temp = 0.0; + + this->to_host_.id = AI_ID_MCU; + + memcpy(&(this->to_host_.data.eulr), &(this->eulr_), sizeof(this->eulr_)); + memcpy(&(this->to_host_.data.yaw), &temp, sizeof(this->to_host_.data.yaw)); + memcpy(&(to_host_.data.pit), &temp, sizeof(this->to_host_.data.pit)); + memcpy(&(to_host_.data.rol), &temp, sizeof(this->to_host_.data.rol)); + + this->to_host_.data.notice = this->notice_for_ai_; + return true; } bool AI::PackRef() { - this->to_host_.ref.id = AI_ID_REF; - this->to_host_.mcu.package.data.ball_speed = - static_cast(this->ref_.ball_speed); - this->to_host_.ref.package.data.arm = this->ref_.robot_id; - this->to_host_.ref.package.data.rfid = this->ref_.robot_buff; - this->to_host_.ref.package.data.team = this->ref_.team; - this->to_host_.ref.package.data.race = this->ref_.game_type; - this->to_host_.ref.package.crc16 = Component::CRC16::Calculate( - reinterpret_cast(&(this->to_host_.ref.package)), - sizeof(this->to_host_.ref.package) - sizeof(uint16_t), CRC16_INIT); - + this->to_host_.data.ball_speed = static_cast(this->ref_.ball_speed); + this->to_host_.data.rfid = this->ref_.rfid; + this->to_host_.data.sentry_hp = this->ref_.hp; + this->to_host_.data.game_progress = this->ref_.game_progress; + this->to_host_.data.ballet_remain = this->ref_.bullet_num; this->ref_updated_ = true; - + this->to_host_.data.hero_x = this->ref_.hero_x; + this->to_host_.data.hero_y = this->ref_.hero_y; + this->to_host_.data.standard_3_x = this->ref_.infantry_3_x; + this->to_host_.data.standard_3_y = this->ref_.infantry_3_y; + this->to_host_.data.standard_4_x = this->ref_.infantry_4_x; + this->to_host_.data.standard_4_y = this->ref_.infantry_4_y; + this->to_host_.data.engineer_x = this->ref_.engineer_x; + this->to_host_.data.engineer_y = this->ref_.engineer_y; return true; } @@ -153,12 +151,9 @@ void AI::DecideAction() { memcpy(&(this->cmd_.gimbal.eulr), &(this->from_host_.data.gimbal), sizeof(this->cmd_.gimbal.eulr)); this->notice_ = this->from_host_.data.notice; - /* AI自瞄数据不更新，重置notice_ */ - // TODO:检测更新方式不合理 - if (this->cmd_.gimbal.eulr.yaw == this->last_auto_aim_eulr_.yaw || - this->cmd_.gimbal.eulr.pit == this->last_auto_aim_eulr_.pit) { - this->notice_ = 0; - } else { + + if (this->cmd_.gimbal.eulr.yaw != this->last_auto_aim_eulr_.yaw || + this->cmd_.gimbal.eulr.pit != this->last_auto_aim_eulr_.pit) { this->aim_time_ = bsp_time_get_ms(); /* 自瞄锁定时刻 */ } memcpy(&(this->last_auto_aim_eulr_), &(this->cmd_.gimbal.eulr), @@ -178,13 +173,7 @@ void AI::DecideAction() { this->damage_.is_damaged_ = false; } - /* 判定是否可以开始导航 */ - if (this->from_host_.data.chassis_move_vec.vx == 0 && damage_.is_damaged_) { - navigation_enable_ = false; - } else { - navigation_enable_ = true; - } - + navigation_enable_ = true; /* 裁判系统行为：确认复活、购买发弹量 */ if (this->ref_.hp == 0) { this->action_.ai_to_referee = CONFIRM_RESURRECTION; @@ -195,42 +184,26 @@ void AI::DecideAction() { } /* 底盘行为*/ - if (navigation_enable_) { - if (this->ref_.outpost_hp > 200) { /* 认为哨兵&基地无敌 */ - this->action_.ai_chassis = AI::Action::TO_HIGHWAY; - this->notice_for_ai_ = AI::Action::TO_HIGHWAY; - } else { - if (this->ref_.hp < 100 || - (this->action_.ai_to_referee == EXCHANGE_BULLETS)) { /* max = 400 */ - this->action_.ai_chassis = AI::Action::TO_SUPPLY; - this->notice_for_ai_ = AI::Action::TO_SUPPLY; - } else { - this->action_.ai_chassis = AI::Action::TO_PATROL_AREA; - this->notice_for_ai_ = AI::Action::TO_PATROL_AREA; - } - } - } else { - this->action_.ai_chassis = AI::Action::ROTOR; + if (navigation_enable_ && damage_.is_damaged_ == false) { + this->action_.ai_chassis = AI::Action::START_AUTO_CONTROL; + } else if (damage_.is_damaged_ == true) { + this->action_.ai_chassis = AI::Action::STOP_AUTO_CONTROL; } /* 云台行为 */ - if (this->notice_ == AI_NOTICE_AUTO_AIM || this->notice_ == AI_NOTICE_FIRE || - bsp_time_get_ms() - this->aim_time_ < 1000) { + if (this->notice_ == 2 || bsp_time_get_ms() - aim_time_ < 1300) { /* 自瞄/丢失目标1000ms内，进行视觉暂留 */ this->action_.ai_gimbal = AI::Action::AUTO_AIM; this->cmd_.gimbal.mode = Component::CMD::GIMBAL_ABSOLUTE_CTRL; - } else if (damage_.is_damaged_) { - this->cmd_.gimbal.mode = Component::CMD::GIMBAL_ABSOLUTE_CTRL; - this->action_.ai_gimbal = AI::Action::AFFECTED; - } else { + } else if (this->notice_ == 5) { this->cmd_.gimbal.mode = Component::CMD::GIMBAL_ABSOLUTE_CTRL; this->action_.ai_gimbal = AI::Action::SCANF; } /* 发射机构行为 */ - if (this->notice_ == AI_NOTICE_FIRE) { /* 开火 */ + if (this->notice_ == 2) { /* 开火 */ this->action_.ai_launcher = AI::Action::FIRE; - } else { + } else if (this->notice_ == 5) { this->action_.ai_launcher = AI::Action::CEASEFIRE; /* 不发弹 */ } } @@ -246,15 +219,12 @@ bool AI::PackCMD() { /* AUTO控制模式，用于全自动机器人 */ if (Component::CMD::GetCtrlMode() == Component::CMD::CMD_AUTO_CTRL) { switch (this->action_.ai_chassis) { - case TO_PATROL_AREA: - case TO_HIGHWAY: - case TO_OUTPOST: - case TO_SUPPLY: + case START_AUTO_CONTROL: memcpy(&(this->cmd_.chassis), &(this->from_host_.data.chassis_move_vec), sizeof(this->from_host_.data.chassis_move_vec)); break; - case ROTOR: + case STOP_AUTO_CONTROL: this->event_.Active(AI_ROTOR); break; } @@ -263,25 +233,46 @@ bool AI::PackCMD() { case AUTO_AIM: memcpy(&(this->cmd_.gimbal.eulr), &(this->from_host_.data.gimbal), sizeof(this->cmd_.gimbal.eulr)); - break; - case AFFECTED: - this->cmd_.gimbal.eulr.yaw = this->damage_.gimbal_yaw_ + - this->damage_.id_ * (M_2PI / 4.0) - - this->damage_.yaw_offset_; - this->cmd_.gimbal.eulr.pit = 0.0f; - this->gimbal_scan_start_angle_ = - this->eulr_.yaw; /* 更新yaw扫描的起始位置 */ - this->target_scan_angle_ = 0.0; /* 置零yaw扫描的位置增量 */ + if (cmd_.gimbal.eulr.pit == 0 && cmd_.gimbal.eulr.yaw == 0) { + cmd_.gimbal.eulr.pit = eulr_.pit; + cmd_.gimbal.eulr.yaw = eulr_.yaw; + } break; case SCANF: - this->target_scan_angle_ += this->scanf_mode_.scanf_yaw_rate; - this->cmd_.gimbal.eulr.yaw = - this->target_scan_angle_ + this->gimbal_scan_start_angle_; + /*yaw轴旋转加入随机数扰动*/ + static float smoothed_random = + 0.0f; // 静态变量，用于存储平滑后的随机值 + float alpha = 0.1f; // 平滑系数，控制新随机值的影响程度 + float raw_random = + static_cast(rand() % 10 - 5) / 1000.0f; // 原始随机扰动 + smoothed_random = alpha * raw_random + + (1.0f - alpha) * smoothed_random; // 低通滤波 + float yaw_rate = this->scanf_mode_.scanf_yaw_rate; + this->target_scan_angle_ += yaw_rate * (1.0f + smoothed_random); + // this->cmd_.gimbal.eulr.yaw = this->target_scan_angle_; + this->cmd_.gimbal.eulr.yaw = this->eulr_.yaw; + + float t = static_cast(bsp_time_get_ms()) / 1000.0f; + // float pit_phase = + // fmodf(t * this->scanf_mode_.scanf_pit_omega, 2.0f * M_PI); + // // 三角波输出范围 [0, 1] + // float triangle_wave = + // (pit_phase < M_PI ? pit_phase / M_PI + // : (2.0f * M_PI - pit_phase) / M_PI); + // float mapped_triangle_wave = 2.0f * triangle_wave - 1.0f; + // // 计算俯仰角 + // this->cmd_.gimbal.eulr.pit = + // this->scanf_mode_.scanf_pit_center + + // this->scanf_mode_.scanf_pit_range * mapped_triangle_wave; + + /*计算三角波*/ + float phase = + fmodf(t * this->scanf_mode_.scanf_pit_omega, 2.0f * M_PI); + float triangle_wave_mapped = + (fabsf(phase / M_PI - 1.0f) * 2.0f) - 1.0f; this->cmd_.gimbal.eulr.pit = this->scanf_mode_.scanf_pit_center + - this->scanf_mode_.scanf_pit_range * - sinf(this->scanf_mode_.scanf_pit_omega * - static_cast(bsp_time_get_ms()) / 1000.0f); + this->scanf_mode_.scanf_pit_range * triangle_wave_mapped; break; } @@ -319,29 +310,23 @@ bool AI::PackCMD() { } this->last_buy_bullet_num_ = cmd_for_ref_.buy_bullet_num; - /* 比赛开始前不运行 */ - if (this->ref_.game_progress == GAMING || - this->ref_.game_progress == PREPARATION || - this->ref_.game_progress == WAITTING) { - this->cmd_tp_.Publish(this->cmd_); - } + this->cmd_tp_.Publish(this->cmd_); + this->cmd_tp_.Publish(this->cmd_); + this->cmd_tp_.Publish(this->cmd_); } /* OP控制模式，用于鼠标右键自瞄 */ - else if (Component::CMD::GetCtrlMode() == Component::CMD::CMD_OP_CTRL) { + if (Component::CMD::GetCtrlMode() == Component::CMD::CMD_OP_CTRL) { memcpy(&(this->cmd_.gimbal.eulr), &(this->from_host_.data.gimbal), sizeof(this->cmd_.gimbal.eulr)); - memcpy(&(this->cmd_.ext.extern_channel), - &(this->from_host_.data.extern_channel), - sizeof(this->cmd_.ext.extern_channel)); + if (cmd_.gimbal.eulr.pit == 0 && cmd_.gimbal.eulr.yaw == 0) { + cmd_.gimbal.eulr.pit = eulr_.pit; + cmd_.gimbal.eulr.yaw = eulr_.yaw; + } memcpy(&(this->cmd_.chassis), &(this->from_host_.data.chassis_move_vec), sizeof(this->from_host_.data.chassis_move_vec)); - memcpy(&(this->cmd_.ext.extern_channel), - &(this->from_host_.data.extern_channel), - sizeof(this->cmd_.ext.extern_channel)); - this->notice_ = this->from_host_.data.notice; this->cmd_.ctrl_source = Component::CMD::CTRL_SOURCE_AI; @@ -373,14 +358,6 @@ void AI::PraseRef() { } this->ref_.status = this->raw_ref_.status; - if (this->raw_ref_.rfid.own_highland_annular == 1 || - this->raw_ref_.rfid.enemy_highland_annular == 1) { - this->ref_.robot_buff |= AI_RFID_SNIP; - } else if (this->raw_ref_.rfid.own_energy_mech_activation == 1) { - this->ref_.robot_buff |= AI_RFID_BUFF; - } else { - this->ref_.robot_buff = 0; - } switch (this->raw_ref_.game_status.game_type) { case Referee::REF_GAME_TYPE_RMUC: this->ref_.game_type = AI_RACE_RMUC; @@ -423,17 +400,24 @@ void AI::PraseRef() { if (this->raw_ref_.robot_status.robot_id < 100) { this->ref_.base_hp = this->raw_ref_.game_robot_hp.red_base; this->ref_.outpost_hp = this->raw_ref_.game_robot_hp.red_outpose; - this->ref_.own_virtual_shield_value = - this->raw_ref_.field_event.virtual_shield_value; + this->ref_.hp = this->raw_ref_.game_robot_hp.red_7; + // this->ref_.hp = 400; + } else { this->ref_.base_hp = this->raw_ref_.game_robot_hp.blue_base; this->ref_.outpost_hp = this->raw_ref_.game_robot_hp.blue_outpose; - this->ref_.own_virtual_shield_value = - this->raw_ref_.field_event.virtual_shield_value; + this->ref_.hp = this->raw_ref_.game_robot_hp.blue_7; } this->ref_.coin_num = this->raw_ref_.bullet_remain.coin_remain; - this->ref_.pos_x = this->raw_ref_.robot_pos.x; - this->ref_.pos_y = this->raw_ref_.robot_pos.y; + this->ref_.bullet_num = this->raw_ref_.bullet_remain.bullet_17_remain; + this->ref_.hero_x = this->raw_ref_.robot_pos_for_snetry.hero_x; + this->ref_.hero_y = this->raw_ref_.robot_pos_for_snetry.hero_y; + this->ref_.infantry_3_x = this->raw_ref_.robot_pos_for_snetry.standard_3_x; + this->ref_.infantry_3_y = this->raw_ref_.robot_pos_for_snetry.standard_3_y; + this->ref_.infantry_4_x = this->raw_ref_.robot_pos_for_snetry.standard_4_x; + this->ref_.infantry_4_y = this->raw_ref_.robot_pos_for_snetry.standard_4_y; + this->ref_.engineer_x = this->raw_ref_.robot_pos_for_snetry.engineer_x; + this->ref_.engineer_y = this->raw_ref_.robot_pos_for_snetry.engineer_y; this->ref_.pos_angle = this->raw_ref_.robot_pos.angle; this->ref_.target_pos_x = this->raw_ref_.client_map.position_x; @@ -442,4 +426,6 @@ void AI::PraseRef() { if (this->raw_ref_.robot_damage.damage_type == 0) { this->ref_.damaged_armor_id = this->raw_ref_.robot_damage.armor_id; } + + std::memcpy(&this->ref_.rfid, &this->raw_ref_.rfid, sizeof(uint32_t)); } diff --git a/src/device/ai/dev_ai.hpp b/src/device/ai/dev_ai.hpp index 8f25773d..1747403d 100644 --- a/src/device/ai/dev_ai.hpp +++ b/src/device/ai/dev_ai.hpp @@ -10,16 +10,6 @@ namespace Device { class AI { public: - typedef struct __attribute__((packed)) { - uint8_t id; - Protocol_UpPackageReferee_t package; - } RefereePckage; - - typedef struct __attribute__((packed)) { - uint8_t id; - Protocol_UpPackageMCU_t package; - } MCUPckage; - typedef struct { uint8_t game_type; Device::Referee::Status status; @@ -29,6 +19,7 @@ class AI { uint32_t ball_speed; uint32_t max_hp; uint32_t hp; + uint32_t rfid; uint8_t game_progress; uint16_t base_hp; @@ -36,8 +27,16 @@ class AI { uint16_t bullet_num; uint16_t coin_num; uint8_t own_virtual_shield_value; - float pos_x; - float pos_y; + + float hero_x; + float hero_y; + float infantry_3_x; + float infantry_3_y; + float infantry_4_x; + float infantry_4_y; + float engineer_x; + float engineer_y; + float pos_angle; float target_pos_x; float target_pos_y; @@ -69,22 +68,19 @@ class AI { GAME_END = 5, } GameProgress; typedef enum { - TO_PATROL_AREA = 0, /* 哨兵巡逻区 */ - TO_SUPPLY = 1, /* 补给区 */ - TO_HIGHWAY = 2, /* 公路区 */ - TO_OUTPOST = 3, /* 前哨站 */ - ROTOR = 4, - - SCANF = 5, - AUTO_AIM = 6, - AFFECTED = 7, /* 反击*/ - - CEASEFIRE = 8, /* 停火 */ - FIRE = 9, - - NOTHING = 10, - CONFIRM_RESURRECTION = 11, /* 确认复活 */ - EXCHANGE_BULLETS = 12, /* 兑换弹丸 */ + START_AUTO_CONTROL = 0, /* 哨兵巡逻区 */ + STOP_AUTO_CONTROL = 1, /* 补给区 */ + + SCANF = 2, + AUTO_AIM = 3, + AFFECTED = 4, /* 反击*/ + + CEASEFIRE = 5, /* 停火 */ + FIRE = 6, + + NOTHING = 7, + CONFIRM_RESURRECTION = 8, /* 确认复活 */ + EXCHANGE_BULLETS = 9, /* 兑换弹丸 */ /* 其他行为暂不考虑 */ } Action; @@ -138,9 +134,9 @@ class AI { /* angle record */ float chassis_yaw_offset_ = 0; - float gimbal_scan_start_angle_; + // float gimbal_scan_start_angle_; Component::Type::Eulr eulr_; - Component::Type::Quaternion quat_; + // Component::Type::Quaternion quat_; Component::Type::CycleValue target_scan_angle_ = 0.0; struct { float yaw; /* 偏航角（Yaw angle） */ @@ -164,11 +160,7 @@ class AI { /* Data */ Protocol_DownPackage_t from_host_{}; - - struct { - RefereePckage ref{}; - MCUPckage mcu{}; - } to_host_; + Protocol_UpPackage_t to_host_{}; RefForAI ref_; @@ -177,10 +169,10 @@ class AI { AICtrlAction action_; ScanfMode scanf_mode_ = { - .scanf_yaw_rate = 0.0025f, - .scanf_pit_center = 0.04f, - .scanf_pit_range = 0.22f, - .scanf_pit_omega = 5.0f, + .scanf_yaw_rate = 0.0053f, + .scanf_pit_center = 0.1f, + .scanf_pit_range = 0.5f, + .scanf_pit_omega = 13.0f, }; SentryDecisionData cmd_for_ref_; diff --git a/src/device/referee/dev_referee.cpp b/src/device/referee/dev_referee.cpp index 29159e96..c76b530f 100644 --- a/src/device/referee/dev_referee.cpp +++ b/src/device/referee/dev_referee.cpp @@ -102,9 +102,10 @@ Referee::Referee() : event_(Message::Event::FindEvent("cmd_event")) { auto ref_trans_thread = [](Referee *ref) { uint32_t last_online_time = bsp_time_get_ms(); - auto ai_sub = Message::Subscriber("sentry_cmd_for_ref"); + // auto ai_sub = + // Message::Subscriber("sentry_cmd_for_ref"); while (1) { - ai_sub.DumpData(ref->data_from_sentry_); + // ai_sub.DumpData(ref->data_from_sentry_); ref->Update(); /* 更新UI */ ref->trans_thread_.SleepUntil(40, last_online_time); } diff --git a/src/device/referee/dev_referee.hpp b/src/device/referee/dev_referee.hpp index 304b509f..70d97703 100644 --- a/src/device/referee/dev_referee.hpp +++ b/src/device/referee/dev_referee.hpp @@ -10,10 +10,10 @@ #include "comp_ui.hpp" #define GAME_HEAT_INCREASE_42MM (100.0f) /* 每发射一颗42mm弹丸增加100热量 */ -#define GAME_HEAT_INCREASE_17MM (10.0f) /* 每发射一颗17mm弹丸增加10热量 */ +#define GAME_HEAT_INCREASE_17MM (10.0f) /* 每发射一颗17mm弹丸增加10热量 */ #define BULLET_SPEED_LIMIT_42MM (16.0) -#define BULLET_SPEED_LIMIT_17MM (30.0) +#define BULLET_SPEED_LIMIT_17MM (25.0) #define GAME_CHASSIS_MAX_POWER_WO_REF 40.0f /* 裁判系统离线时底盘最大功率 */ #define REF_UI_BOX_UP_OFFSET (4) @@ -74,8 +74,8 @@ class Referee { REF_CMD_ID_DART_CLIENT = 0x020A, REF_CMD_ID_ROBOT_POS_TO_SENTRY = 0X020B, REF_CMD_ID_RADAR_MARK = 0X020C, - REF_CMD_ID_SENTRY_DECISION = 0x020D, /* 哨兵自主决策相关信息同步 */ - REF_CMD_ID_RADAR_DECISION = 0x020E, /* 雷达自主决策相关信息同步 */ + REF_CMD_ID_SENTRY_DECISION = 0x020D, /* 哨兵自主决策相关信息同步 */ + REF_CMD_ID_RADAR_DECISION = 0x020E, /* 雷达自主决策相关信息同步 */ REF_CMD_ID_INTER_STUDENT = 0x0301, /* 机器人交互数据 */ REF_CMD_ID_INTER_STUDENT_CUSTOM = 0x0302, /* 自定义控制器和机器人 */ REF_CMD_ID_CLIENT_MAP = 0x0303, @@ -116,7 +116,7 @@ class Referee { uint16_t red_2; uint16_t red_3; uint16_t red_4; - uint16_t red_5; + uint16_t res_1; uint16_t red_7; uint16_t red_outpose; uint16_t red_base; @@ -124,7 +124,7 @@ class Referee { uint16_t blue_2; uint16_t blue_3; uint16_t blue_4; - uint16_t blue_5; + uint16_t res_2; uint16_t blue_7; uint16_t blue_outpose; uint16_t blue_base; @@ -133,17 +133,16 @@ class Referee { uint32_t blood_supply_before_status : 1; uint32_t blood_supply_inner_status : 1; uint32_t blood_supply_status_RMUL : 1; - uint32_t energy_mech_activation_status : 1; + // uint32_t energy_mech_activation_status : 1; uint32_t energy_mech_small_status : 1; uint32_t energy_mech_big_status : 1; - uint32_t highland_annular : 2; + uint32_t highland_center : 2; - uint32_t highland_trapezium_1 : 2; - uint32_t highland_trapezium_2 : 2; - uint32_t virtual_shield_value : 8; + uint32_t highland_trapezium : 2; uint32_t last_hit_time : 9; - uint32_t last_hit_target : 2; - uint32_t res : 1; + uint32_t last_hit_target : 3; + uint32_t highland_center_status : 2; + uint32_t res : 9; } FieldEvents; /* 0x0101 */ typedef struct __attribute__((packed)) { uint8_t res; @@ -161,10 +160,10 @@ class Referee { typedef struct __attribute__((packed)) { uint8_t countdown; - uint16_t dart_last_target : 2; + uint16_t dart_last_target : 3; uint16_t attack_count : 3; uint16_t dart_target : 2; - uint16_t res : 9; + uint16_t res : 8; } DartCountdown; /* 0x0105 */ typedef struct __attribute__((packed)) { @@ -181,9 +180,9 @@ class Referee { } RobotStatus; /* 0x0201 */ typedef struct __attribute__((packed)) { - uint16_t chassis_volt; - uint16_t chassis_amp; - float chassis_watt; + uint16_t res_1; + uint16_t res_2; + float res_3; uint16_t chassis_pwr_buff; uint16_t launcher_id1_17_heat; uint16_t launcher_id2_17_heat; @@ -202,6 +201,11 @@ class Referee { uint8_t defense_buff; uint8_t vulnerability_buff; uint16_t attack_buff; + uint8_t percent_50_remain_energy : 1; + uint8_t percent_30_remain_energy : 1; + uint8_t percent_15_remain_energy : 1; + uint8_t percent_5_remain_energy : 1; + uint8_t percent_1_remain_energy : 1; } RobotBuff; /* 0x0204 */ typedef struct __attribute__((packed)) { @@ -229,25 +233,30 @@ class Referee { typedef struct __attribute__((packed)) { uint32_t own_base : 1; - uint32_t own_highland_annular : 1; - uint32_t enemy_highland_annular : 1; - uint32_t own_trapezium_R3B3 : 1; - uint32_t enemy_trapezium_R3B3 : 1; - uint32_t own_trapezium_R4B4 : 1; - uint32_t enemy_trapezium_R4B4 : 1; - uint32_t own_energy_mech_activation : 1; - uint32_t own_slope_before : 1; - uint32_t own_slope_after : 1; - uint32_t enemy_slope_before : 1; - uint32_t enemy_slope_after : 1; - uint32_t own_outpose : 1; - uint32_t own_blood_supply : 1; - uint32_t own_sentry_area : 1; - uint32_t enemy_sentry_area : 1; + uint32_t own_highland_center : 1; + uint32_t enemy_highland_center : 1; + uint32_t own_trapezium : 1; + uint32_t enemy_trapezium : 1; + uint32_t own_slope_before_R1B1 : 1; + uint32_t own_slope_after_R1B1 : 1; + uint32_t enemy_slope_before_R4B4 : 1; + uint32_t enemy_slope_after_R4B4 : 1; + uint32_t own_terrain_crossing_up_R2B2 : 1; + uint32_t own_terrain_crossing_down_R2B2 : 1; + uint32_t enemy_terrain_corrssing_up_R2B2 : 1; + uint32_t enemy_terrain_corrssing_down_R2B2 : 1; + uint32_t own_terrain_crossing_up_R3B3 : 1; + uint32_t own_terrain_crossing_down_R3B3 : 1; + uint32_t enemy_terrain_corrssing_up_R3B5 : 1; + uint32_t enemy_terrain_corrssing_down_R3B3 : 1; + uint32_t own_fortress : 1; + uint32_t own_outpost : 1; + uint32_t own_blood_supply_unoverlapping : 1; + uint32_t own_blood_supply_overlapping : 1; uint32_t own_resource : 1; uint32_t enemy_resource : 1; - uint32_t own_exchange_area : 1; - uint32_t res : 13; + uint32_t center_resource_RMUL : 1; + uint32_t res : 8; } RFID; /* 0x0209 */ typedef struct __attribute__((packed)) { @@ -265,16 +274,15 @@ class Referee { float standard_3_y; float standard_4_x; float standard_4_y; - float standard_5_x; - float standard_5_y; + float res_1; + float res_2; } RobotPosForSentry; /* 0x020B */ typedef struct __attribute__((packed)) { - uint8_t mark_hero_progress; - uint8_t mark_engineer_progress; - uint8_t mark_standard_3_progress; - uint8_t mark_standard_4_progress; - uint8_t mark_standard_5_progress; - uint8_t mark_sentry_progress; + uint8_t mark_hero_progress : 1; + uint8_t mark_engineer_progress : 1; + uint8_t mark_standard_3_progress : 1; + uint8_t mark_standard_4_progress : 1; + uint8_t mark_sentry_progress : 1; } RadarMarkProgress; /* 0x020C */ typedef struct __attribute__((packed)) { uint32_t exchanged_bullet_num : 11; @@ -494,7 +502,7 @@ class Referee { struct __attribute__((packed)) { Header frame_header; uint16_t cmd_id; - Referee::InterStudentHeader student_header; //字命令、发送者、接受者 + Referee::InterStudentHeader student_header; // 字命令、发送者、接受者 } raw; }; @@ -510,7 +518,7 @@ class Referee { typedef struct __attribute__((packed)) { Header frame_header; // 0x0301 uint16_t cmd_id; - Referee::InterStudentHeader student_header; //含0x0120 + Referee::InterStudentHeader student_header; // 含0x0120 uint32_t data_cmd; uint16_t crc16; } SentryPack; diff --git a/src/module/chassis/mod_chassis.cpp b/src/module/chassis/mod_chassis.cpp index 6e4d02c6..f45aa816 100644 --- a/src/module/chassis/mod_chassis.cpp +++ b/src/module/chassis/mod_chassis.cpp @@ -11,10 +11,6 @@ #include "mod_chassis.hpp" -#include - -#include - #include "bsp_time.h" #define ROTOR_WZ_MIN 0.8f /* 小陀螺旋转位移下界 */ @@ -23,6 +19,7 @@ #define ROTOR_OMEGA 0.0025f /* 小陀螺转动频率 */ #define MOTOR_MAX_SPEED_COFFICIENT 1.2f /* 电机的最大转速 */ +#define MOTOR_MAX_ROTATIONAL_SPEED 9600 /* 电机最大转速 */ #if POWER_LIMIT_WITH_CAP /* 保证电容电量宏定义在正确范围内 */ @@ -49,6 +46,8 @@ Chassis::Chassis(Param& param, float control_freq) mode_(Chassis::RELAX), mixer_(param.type), follow_pid_(param.follow_pid_param, control_freq), + xaccl_pid_(param.xaccl_pid_param, control_freq), + yaccl_pid_(param.yaccl_pid_param, control_freq), ctrl_lock_(true) { memset(&(this->cmd_), 0, sizeof(this->cmd_)); @@ -82,6 +81,12 @@ Chassis::Chassis(Param& param, float control_freq) case SET_MODE_INDENPENDENT: chassis->SetMode(INDENPENDENT); break; + case CHANGE_POWER_UP: + chassis->ChangePowerlim(COMMON); + break; + case CHANGE_POWER_DOWN: + chassis->ChangePowerlim(BEAST); + break; default: break; } @@ -91,14 +96,11 @@ Chassis::Chassis(Param& param, float control_freq) Component::CMD::RegisterEvent(event_callback, this, this->param_.EVENT_MAP); - auto chassis_thread = [](Chassis* chassis) { auto raw_ref_sub = Message::Subscriber("referee"); auto cmd_sub = Message::Subscriber("cmd_chassis"); - auto yaw_sub = Message::Subscriber("chassis_yaw"); - auto cap_sub = Message::Subscriber("cap_info"); uint32_t last_online_time = bsp_time_get_ms(); @@ -139,41 +141,7 @@ void Chassis::UpdateFeedback() { this->motor_feedback_[i] = this->motor_[i]->GetSpeed(); } } -template -uint16_t Chassis::MAXSPEEDGET(float power_limit) { - if (param_.get_speed) { - return param_.get_speed(power_limit); - } else { - return 5000; - } -} -template -bool Chassis::LimitChassisOutPower(float power_limit, - float* motor_out, - float* speed_rpm, - uint32_t len) { - if (power_limit < 0.0f) { - return 0; - } - float sum_motor_power = 0.0f; - float motor_power[4]; - for (size_t i = 0; i < len; i++) { - motor_power[i] = - this->param_.toque_coefficient_ * fabsf(motor_out[i]) * - fabsf(speed_rpm[i]) + - this->param_.speed_2_coefficient_ * speed_rpm[i] * speed_rpm[i] + - this->param_.out_2_coefficient_ * motor_out[i] * motor_out[i]; - sum_motor_power += motor_power[i]; - } - sum_motor_power += this->param_.constant_; - if (sum_motor_power > power_limit) { - for (size_t i = 0; i < len; i++) { - motor_out[i] *= power_limit / sum_motor_power; - } - } - return true; -} template void Chassis::Control() { this->now_ = bsp_time_get(); @@ -182,7 +150,6 @@ void Chassis::Control() { this->last_wakeup_ = this->now_; - max_motor_rotational_speed_ = this->MAXSPEEDGET(ref_.chassis_power_limit); /* ctrl_vec -> move_vec 控制向量和真实的移动向量之间有一个换算关系 */ /* 计算vx、vy */ switch (this->mode_) { @@ -191,21 +158,48 @@ void Chassis::Control() { this->move_vec_.vy = 0.0f; break; - case Chassis::INDENPENDENT: /* 独立模式控制向量与运动向量相等 - */ + case Chassis::INDENPENDENT: /* 独立模式控制向量与运动向量相等*/ this->move_vec_.vx = this->cmd_.x; this->move_vec_.vy = this->cmd_.y; break; case Chassis::RELAX: - case Chassis::FOLLOW_GIMBAL: /* 按照云台方向换算运动向量 - */ + case Chassis::FOLLOW_GIMBAL: /* 按照云台方向换算运动向量 */ + { + float beta = this->yaw_; + float cos_beta = cosf(beta); + float sin_beta = sinf(beta); + /*控制加速度*/ + this->move_vec_.vx = this->xaccl_pid_.Calculate( + (cos_beta * this->cmd_.x - sin_beta * this->cmd_.y), + this->move_vec_.vx, dt_); + if (!cmd_.x) { + this->xaccl_pid_.Reset(); + } + this->move_vec_.vy = this->yaccl_pid_.Calculate( + (sin_beta * this->cmd_.x + cos_beta * this->cmd_.y), + this->move_vec_.vy, dt_); + if (!cmd_.y) { + this->yaccl_pid_.Reset(); + } + float scalar_sum = fabs(this->move_vec_.vx) + fabs(this->move_vec_.vy); + if (scalar_sum > 1.01f) { + this->move_vec_.vx = this->move_vec_.vx / scalar_sum; + this->move_vec_.vy = this->move_vec_.vy / scalar_sum; + } + break; + } 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; + float scalar_sum = fabs(this->move_vec_.vx) + fabs(this->move_vec_.vy); + if (scalar_sum > 1.01f) { + this->move_vec_.vx = this->move_vec_.vx / scalar_sum; + this->move_vec_.vy = this->move_vec_.vy / scalar_sum; + } break; } default: @@ -220,16 +214,39 @@ void Chassis::Control() { this->move_vec_.wz = this->cmd_.z; break; - case Chassis::FOLLOW_GIMBAL: /* 跟随模式通过PID控制使车头跟随云台 - */ + case Chassis::FOLLOW_GIMBAL: /* 跟随模式通过PID控制使车头跟随云台*/ + { + float direction = 0.0f; + // /*双零点*/ + // if (this->yaw_ > M_PI_2) { + // direction = 3.14158f; + // } + // if (this->yaw_ < -M_PI_2) { + // direction = 3.14158f; + // } this->move_vec_.wz = - this->follow_pid_.Calculate(0.0f, this->yaw_, this->dt_); + this->follow_pid_.Calculate(direction, this->yaw_, this->dt_); + clampf(&this->move_vec_.wz, -1.0f, 1.0f); + float move_scal_sum = fabs(this->move_vec_.vx) + + fabs(this->move_vec_.vy) + fabs(this->move_vec_.wz); + if (move_scal_sum > 1.01f) { + this->move_vec_.vx = + this->move_vec_.vx * (1 - fabs(this->move_vec_.wz)); + this->move_vec_.vy = + this->move_vec_.vy * (1 - fabs(this->move_vec_.wz)); + } break; - - case Chassis::ROTOR: { /* 小陀螺模式使底盘以一定速度旋转 - */ - this->move_vec_.wz = - this->wz_dir_mult_ * CalcWz(ROTOR_WZ_MIN, ROTOR_WZ_MAX); + } + case Chassis::ROTOR: /* 小陀螺模式使底盘根据速度大小调整旋转速度*/ + { /* TODO 改成实际底盘速度 */ + this->move_vec_.wz = this->wz_dir_mult_; + float move_scal_sum = fabs(this->move_vec_.vx) + + fabs(this->move_vec_.vy) + fabs(this->move_vec_.wz); + if (move_scal_sum > 1.01f) { + this->move_vec_.wz = this->move_vec_.wz / move_scal_sum; + this->move_vec_.vx = this->move_vec_.vx / move_scal_sum; + this->move_vec_.vy = this->move_vec_.vy / move_scal_sum; + } break; } default: @@ -240,8 +257,6 @@ void Chassis::Control() { /* move_vec -> motor_rpm_set. 通过运动向量计算轮子转速目标值 */ this->mixer_.Apply(this->move_vec_, this->setpoint_.motor_rotational_speed); - /* 根据轮子转速目标值，利用PID计算电机输出值 */ - /* 根据底盘模式计算输出值 */ switch (this->mode_) { case Chassis::BREAK: @@ -252,7 +267,6 @@ void Chassis::Control() { if (ref_.status == Device::Referee::RUNNING) { if (ref_.chassis_pwr_buff > 30) { percentage = 1.0f; - } else { percentage = this->ref_.chassis_pwr_buff / 30.0f; } @@ -262,28 +276,25 @@ void Chassis::Control() { clampf(&percentage, 0.0f, 1.0f); - float max_power_limit = - ref_.chassis_power_limit + - ref_.chassis_power_limit * 0.2 * this->cap_.percentage_; for (unsigned i = 0; i < this->mixer_.len_; i++) { out_.motor_out[i] = this->actuator_[i]->Calculate( this->setpoint_.motor_rotational_speed[i] * - max_motor_rotational_speed_, + MOTOR_MAX_ROTATIONAL_SPEED, this->motor_[i]->GetSpeed(), this->dt_); } for (unsigned i = 0; i < this->mixer_.len_; i++) { if (cap_.online_) { - LimitChassisOutPower(max_power_limit, out_.motor_out, motor_feedback_, - this->mixer_.len_); + LimitChassisOutPower(this->max_power_limit_, out_.motor_out, + motor_feedback_, this->mixer_.len_); this->motor_[i]->Control(out_.motor_out[i]); - } else { + /* 不限功率的兵种使用该底盘时 + * 注意更改chassis_power_limit至电池安全功率 */ LimitChassisOutPower(ref_.chassis_power_limit, out_.motor_out, motor_feedback_, this->mixer_.len_); this->motor_[i]->Control(out_.motor_out[i]); } } - break; } case Chassis::RELAX: /* 放松模式,不输出 */ @@ -297,22 +308,44 @@ void Chassis::Control() { } } +/* 功率限制 */ +template +bool Chassis::LimitChassisOutPower(float power_limit, + float* motor_out, + float* speed_rpm, + uint32_t len) { + if (power_limit < 0.0f) { + return 0; + } + float sum_motor_power = 0.0f; + float motor_power[4]; + for (size_t i = 0; i < len; i++) { + motor_power[i] = + this->param_.toque_coefficient_ * fabsf(motor_out[i]) * + fabsf(speed_rpm[i]) + + this->param_.speed_2_coefficient_ * speed_rpm[i] * speed_rpm[i] + + this->param_.out_2_coefficient_ * motor_out[i] * motor_out[i]; + sum_motor_power += motor_power[i]; + } + sum_motor_power += this->param_.constant_; + if (sum_motor_power > power_limit) { + for (size_t i = 0; i < len; i++) { + motor_out[i] *= power_limit / sum_motor_power; + } + } + return true; +} + +/* 解析裁判系统数据 */ 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; - clampf(&wz_vary, LO, HI); - return wz_vary; -} - +/* 设置底盘模式 */ template void Chassis::SetMode(Chassis::Mode mode) { if (mode == this->mode_) { @@ -330,108 +363,94 @@ void Chassis::SetMode(Chassis::Mode mode) { this->mode_ = mode; } +/* 功率限制切换 */ +template +void Chassis::ChangePowerlim( + Chassis::Power_Mode power_mode) { + // TODO仔细研究正常模式和野兽模式的功率设置 + if (power_mode == this->power_mode_) { + return; + } /* 模式未改变直接返回 */ + if (power_mode == Chassis::COMMON) { + this->max_power_limit_ = ref_.chassis_power_limit; + } else { + this->max_power_limit_ = 100.0f + 100 * 0.2 * this->cap_.percentage_; + } + this->power_mode_ = power_mode; +} + +/* 随机转速小陀螺 结合自身超电与敌方视觉水平使用 */ +template +float Chassis::CalcWz(const float LO, const float HI) { + float wz_vary = fabsf(0.2f * sinf(ROTOR_OMEGA * this->now_)) + LO; + clampf(&wz_vary, LO, HI); + return wz_vary; +} + +/*慢速刷新*/ template void Chassis::DrawUIStatic( Chassis* chassis) { + /* 底盘模式初始化 */ chassis->string_.Draw("CM", Component::UI::UI_GRAPHIC_OP_ADD, - Component::UI::UI_GRAPHIC_LAYER_CONST, - Component::UI::UI_GREEN, UI_DEFAULT_WIDTH * 10, 80, - UI_CHAR_DEFAULT_WIDTH, - static_cast(Device::Referee::UIGetWidth() * - REF_UI_RIGHT_START_W), - static_cast(Device::Referee::UIGetHeight() * - REF_UI_MODE_LINE1_H), - "CHAS FLLW INDT ROTR"); + Component::UI::UI_GRAPHIC_LAYER_CHASSIS, + Component::UI::UI_CYAN, UI_DEFAULT_WIDTH * 20, 80, + UI_CHAR_DEFAULT_WIDTH, 1336, 749, "INIT"); Device::Referee::AddUI(chassis->string_); - - float box_pos_left = 0.0f, box_pos_right = 0.0f; - - /* 更新底盘模式选择框 */ - 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; - break; - case RELAX: - case BREAK: - default: - box_pos_left = 0.0f; - box_pos_right = 0.0f; - break; - } - - if (box_pos_left != 0.0f && box_pos_right != 0.0f) { - chassis->rectange_.Draw( - "CS", Component::UI::UI_GRAPHIC_OP_ADD, - Component::UI::UI_GRAPHIC_LAYER_CHASSIS, Component::UI::UI_GREEN, - UI_DEFAULT_WIDTH, - static_cast(Device::Referee::UIGetWidth() * - REF_UI_RIGHT_START_W + - box_pos_left), - static_cast(Device::Referee::UIGetHeight() * - REF_UI_MODE_LINE1_H + - REF_UI_BOX_UP_OFFSET), - static_cast(Device::Referee::UIGetWidth() * - REF_UI_RIGHT_START_W + - box_pos_right), - static_cast(Device::Referee::UIGetHeight() * - REF_UI_MODE_LINE1_H + - REF_UI_BOX_BOT_OFFSET)); - Device::Referee::AddUI(chassis->rectange_); - } + /* 车头朝向初始化 */ + chassis->cycle_.Draw( + "CS", Component::UI::UI_GRAPHIC_OP_ADD, + Component::UI::UI_GRAPHIC_LAYER_CHASSIS, Component::UI::UI_CYAN, + UI_DEFAULT_WIDTH * 7, + static_cast(Device::Referee::UIGetWidth() * 0.5), + static_cast(Device::Referee::UIGetHeight() * 0.5 + 260), 20); + Device::Referee::AddUI(chassis->cycle_); } +/* 快速刷新 */ template void Chassis::DrawUIDynamic( Chassis* chassis) { - float box_pos_left = 0.0f, box_pos_right = 0.0f; - - /* 更新底盘模式选择框 */ - 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: - - default: - box_pos_left = 0.0f; - box_pos_right = 0.0f; - break; - } - - if (box_pos_left != 0.0f && box_pos_right != 0.0f) { - chassis->rectange_.Draw( + if (chassis->mode_ == chassis->last_mode_) { + /* 车头朝向ui */ + chassis->cycle_.Draw( "CS", Component::UI::UI_GRAPHIC_OP_REWRITE, - Component::UI::UI_GRAPHIC_LAYER_CHASSIS, Component::UI::UI_GREEN, - UI_DEFAULT_WIDTH, - static_cast(Device::Referee::UIGetWidth() * - REF_UI_RIGHT_START_W + - box_pos_left), - static_cast(Device::Referee::UIGetHeight() * - REF_UI_MODE_LINE1_H + - REF_UI_BOX_UP_OFFSET), - static_cast(Device::Referee::UIGetWidth() * - REF_UI_RIGHT_START_W + - box_pos_right), - static_cast(Device::Referee::UIGetHeight() * - REF_UI_MODE_LINE1_H + - REF_UI_BOX_BOT_OFFSET)); - Device::Referee::AddUI(chassis->rectange_); + Component::UI::UI_GRAPHIC_LAYER_CHASSIS, Component::UI::UI_CYAN, + UI_DEFAULT_WIDTH * 7, + static_cast(Device::Referee::UIGetWidth() * 0.5 + + 260 * sinf(chassis->yaw_)), + static_cast(Device::Referee::UIGetHeight() * 0.5 + + 260 * cosf(chassis->yaw_)), + 20); + Device::Referee::AddUI(chassis->cycle_); + } else { + chassis->last_mode_ = chassis->mode_; + char mode_ui[5] = "EROR"; + switch (chassis->mode_) { + case RELAX: + strncpy(mode_ui, "RELX", sizeof(mode_ui)); /*1 5!*/ + break; + case BREAK: + strncpy(mode_ui, "BREK", sizeof(mode_ui)); + break; + case FOLLOW_GIMBAL: + strncpy(mode_ui, "FOLW", sizeof(mode_ui)); + break; + case ROTOR: + strncpy(mode_ui, "ROTO", sizeof(mode_ui)); + break; + case INDENPENDENT: + strncpy(mode_ui, "INDP", sizeof(mode_ui)); + break; + default: + break; + } + chassis->string_.Draw("CM", Component::UI::UI_GRAPHIC_OP_REWRITE, + Component::UI::UI_GRAPHIC_LAYER_CHASSIS, + Component::UI::UI_CYAN, UI_DEFAULT_WIDTH * 20, 80, + UI_CHAR_DEFAULT_WIDTH, 1336, 749, mode_ui); + Device::Referee::AddUI(chassis->string_); } } + template class Module::Chassis; diff --git a/src/module/chassis/mod_chassis.hpp b/src/module/chassis/mod_chassis.hpp index 1ac3b21b..6f1c6dee 100644 --- a/src/module/chassis/mod_chassis.hpp +++ b/src/module/chassis/mod_chassis.hpp @@ -8,7 +8,6 @@ * @copyright Copyright (c) 2021 * */ - #pragma once #include @@ -36,11 +35,18 @@ class Chassis { INDENPENDENT, /* 独立模式。底盘运行不受云台影响 */ } Mode; + typedef enum{ + COMMON, + BEAST, + }Power_Mode; + typedef enum { SET_MODE_RELAX, SET_MODE_FOLLOW, SET_MODE_ROTOR, SET_MODE_INDENPENDENT, + CHANGE_POWER_UP, + CHANGE_POWER_DOWN, } ChassisEvent; /* 底盘参数的结构体，包含所有初始Component化用的参数，通常是const，存好几组 */ @@ -48,26 +54,22 @@ class Chassis { float toque_coefficient_; float speed_2_coefficient_; float out_2_coefficient_; - float constant_; - - Component::Mixer::Mode type = - Component::Mixer::MECANUM; /* 底盘类型，底盘的机械设计和轮子选型 */ + float constant_; //功率参数不能封装一下吗 + Component::Mixer::Mode type = Component::Mixer::MECANUM; /* 底盘类型，底盘的机械设计和轮子选型 */ Component::PID::Param follow_pid_param{}; /* 跟随云台PID的参数 */ - - const std::vector EVENT_MAP; + Component::PID::Param xaccl_pid_param{}; /* 加速跟随PID的参数 */ + Component::PID::Param yaccl_pid_param{}; /* y方向加速跟随PID */ std::array actuator_param{}; - std::array motor_param; - float (*get_speed)(float); + const std::vector EVENT_MAP; } 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); @@ -78,10 +80,10 @@ class Chassis { void SetMode(Mode mode); + void ChangePowerlim(Power_Mode power_mode_); + bool LimitChassisOutPower(float power_limit, float *motor_out, float *speed, uint32_t len); - uint16_t MAXSPEEDGET(float power_limit); - void PraseRef(); static void DrawUIStatic(Chassis *chassis); @@ -93,8 +95,6 @@ class Chassis { private: Param param_; - float max_motor_rotational_speed_ = 0.0f; - float dt_ = 0.0f; float chassis_current_; @@ -106,6 +106,8 @@ class Chassis { RefForChassis ref_; Mode mode_ = RELAX; + Mode last_mode_ = mode_; + Power_Mode power_mode_ = COMMON; Device::Cap::Info cap_; @@ -116,37 +118,36 @@ class Chassis { /* 底盘设计 */ Component::Mixer mixer_; + float wz_dir_mult_; /* 小陀螺模式旋转方向乘数 */ + + float yaw_; + Component::Type::MoveVector move_vec_; /* 底盘实际的运动向量 */ - float wz_dir_mult_; /* 小陀螺模式旋转方向乘数 */ + Component::PID follow_pid_; /* 跟随云台用的PID */ + Component::PID xaccl_pid_; /* x方向加速跟随PID */ + Component::PID yaccl_pid_; /* y方向加速跟随PID */ - /* PID计算的目标值 */ + float max_power_limit_ ; struct { float *motor_rotational_speed; /* 电机转速的动态数组，单位：RPM */ } setpoint_; float motor_feedback_[4]; - struct { float motor_out[4]; } out_; - 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_; - - Component::UI::Line line_; - - Component::UI::Rectangle rectange_; + Component::UI::Cycle cycle_; }; typedef Chassis RMChassis; diff --git a/src/module/gimbal/mod_gimbal.cpp b/src/module/gimbal/mod_gimbal.cpp index b0b494c8..9374a268 100644 --- a/src/module/gimbal/mod_gimbal.cpp +++ b/src/module/gimbal/mod_gimbal.cpp @@ -15,6 +15,8 @@ Gimbal::Gimbal(Param& param, float control_freq) st_(param.st), yaw_actuator_(this->param_.yaw_actr, control_freq), pit_actuator_(this->param_.pit_actr, control_freq), + yaw_ai_actuator_(this->param_.yaw_ai_actr, control_freq), + pit_ai_actuator_(this->param_.pit_ai_actr, control_freq), yaw_motor_(this->param_.yaw_motor, "Gimbal_Yaw"), pit_motor_(this->param_.pit_motor, "Gimbal_Pitch"), ctrl_lock_(true) { @@ -24,14 +26,13 @@ Gimbal::Gimbal(Param& param, float control_freq) switch (event) { case SET_MODE_RELAX: case SET_MODE_ABSOLUTE: + Component::CMD::SetCtrlSource(Component::CMD::CTRL_SOURCE_RC); gimbal->SetMode(static_cast(event)); break; - case START_AUTO_AIM: + case SET_MODE_AUTO_AIM: Component::CMD::SetCtrlSource(Component::CMD::CTRL_SOURCE_AI); - break; - case STOP_AUTO_AIM: - Component::CMD::SetCtrlSource(Component::CMD::CTRL_SOURCE_RC); + gimbal->SetMode((static_cast(event))); break; } gimbal->ctrl_lock_.Post(); @@ -61,6 +62,10 @@ Gimbal::Gimbal(Param& param, float control_freq) gimbal->ctrl_lock_.Post(); gimbal->yaw_tp_.Publish(gimbal->yaw_); + gimbal->pit_tp_.Publish(gimbal->pit_); + gimbal->alpha_tp_.Publish(gimbal->alpha_); + gimbal->eulr_yaw1_tp_.Publish(gimbal->eulr_yaw1_); + gimbal->tan_pit_tp_.Publish(gimbal->tan_pit_); /* 运行结束，等待下一次唤醒 */ gimbal->thread_.SleepUntil(2, last_online_time); @@ -80,6 +85,91 @@ void Gimbal::UpdateFeedback() { this->yaw_motor_.Update(); this->yaw_ = this->yaw_motor_.GetAngle() - this->param_.mech_zero.yaw; + this->eulr_yaw1_ = eulr_.yaw; + this->pit_ = atan( + sqrt(tan(ChangeAngleRange(eulr_.pit) - + (this->pit_motor_.GetAngle() - this->param_.mech_zero.pit)) * + tan(ChangeAngleRange(eulr_.pit) - + (this->pit_motor_.GetAngle() - this->param_.mech_zero.pit)) + + tan(eulr_.rol) * tan(eulr_.rol))); + this->alpha_ = this->GetAlpha(); + this->slope_angle_ = this->pit_ * 180 / M_PI; + this->yaw_motor_value_ = this->yaw_motor_.GetAngle().Value(); + this->pit_motor_value_ = this->pit_motor_.GetAngle().Value(); + + this->tan_pit_ = + tan(ChangeAngleRange(eulr_.pit) - + (this->pit_motor_.GetAngle() - this->param_.mech_zero.pit)); + this->tan_rol_ = tan(ChangeAngleRange(eulr_.rol)); + this->test_angle_3_ = ChangeAngleRange(eulr_.pit); + // this->tan_yaw_ = -(this->yaw_motor_.GetAngle().Value() - + // this->param_.mech_zero.yaw) + + // ChangeAngleRange(eulr_.yaw); + this->test_angle_4_ = RotateVector3D(static_cast(this->test_angle_3_), + static_cast(this->tan_pit_), + static_cast(this->tan_rol_)); +} + +double Gimbal::GetAlpha() { + if (this->tan_rol_ > 0 && this->tan_pit_ > 0) { + this->alpha_ = ((this->tan_rol_ * this->tan_rol_) / + (tan(this->pit_) * tan(this->pit_))) * + M_PI_2; + } else if (this->tan_rol_ > 0 && this->tan_pit_ < 0) { + this->alpha_ = + M_PI - + ((tan_rol_ * tan_rol_) / (tan(this->pit_) * tan(this->pit_))) * M_PI_2; + } else if (this->tan_rol_ < 0 && this->tan_pit_ < 0) { + this->alpha_ = + M_PI + + ((tan_rol_ * tan_rol_) / (tan(this->pit_) * tan(this->pit_))) * M_PI_2; + } else if (this->tan_rol_ < 0 && this->tan_pit_ > 0) { + this->alpha_ = + 2 * M_PI - + ((tan_rol_ * tan_rol_) / (tan(this->pit_) * tan(this->pit_))) * M_PI_2; + } + return this->alpha_; +} + +float Gimbal::ChangeAngleRange(float angle) { + angle = fmod(angle, 2 * M_PI); + if (angle < 0) { + angle += 2 * M_PI; + } + // 将角度从 [0, 2π] 转换到 [-π, π] + if (angle > M_PI) { + angle -= 2 * M_PI; + } + + return angle; +} + +float Gimbal::RotateVector3D(float x, float y, float z) { + float cos_x = cosf(x); + float sin_x = sinf(x); + float cos_y = cosf(y); + float sin_y = sinf(y); + float cos_z = cosf(z); + float sin_z = sinf(z); + float angle = 0.0f; + rotation_mat_[0][0] = cos_x * cos_y; + rotation_mat_[0][1] = -sin_x * cos_z + cos_x * sin_y * sin_z; + rotation_mat_[0][2] = sin_x * cos_z + cos_x * sin_y * cos_z; + rotation_mat_[1][0] = sin_x * cos_y; + rotation_mat_[1][1] = cos_x * cos_z + sin_x * sin_y * sin_z; + rotation_mat_[1][2] = -cos_x * sin_z + sin_x * sin_y * cos_z; + rotation_mat_[2][0] = -sin_y; + rotation_mat_[2][1] = cos_y * sin_z; + rotation_mat_[2][2] = cos_y * cos_z; + angle = atan(-sqrt(rotation_mat_[0][2] * rotation_mat_[0][2] + + rotation_mat_[1][2] * rotation_mat_[1][2]) / + rotation_mat_[2][2]); + return angle; + // return {{{cos_x * cos_y, -sin_x * cos_z + cos_x * sin_y * sin_z, + // sin_x * cos_z + cos_x * sin_y * cos_z}, + // {sin_x * cos_y, cos_x * cos_z + sin_x * sin_y * sin_z, + // -cos_x * sin_z + sin_x * sin_y * cos_z}, + // {-sin_y, cos_y * sin_z, cos_y * cos_z}}}; } void Gimbal::Control() { @@ -95,7 +185,7 @@ void Gimbal::Control() { if (this->cmd_.mode == Component::CMD::GIMBAL_RELATIVE_CTRL) { gimbal_yaw_cmd = this->cmd_.eulr.yaw * this->dt_ * GIMBAL_MAX_SPEED; gimbal_pit_cmd = this->cmd_.eulr.pit * this->dt_ * GIMBAL_MAX_SPEED; - + pit_ = gimbal_pit_cmd; } else { gimbal_yaw_cmd = Component::Type::CycleValue(this->cmd_.eulr.yaw) - this->setpoint_.eulr_.yaw; @@ -148,6 +238,16 @@ void Gimbal::Control() { this->yaw_motor_.Control(yaw_out); this->pit_motor_.Control(pit_out); + break; + case AI_CONTROL: + yaw_out = this->yaw_ai_actuator_.Calculate( + this->setpoint_.eulr_.yaw, this->gyro_.z, this->eulr_.yaw, this->dt_); + + pit_out = this->pit_ai_actuator_.Calculate( + this->setpoint_.eulr_.pit, this->gyro_.x, this->eulr_.pit, this->dt_); + + this->yaw_motor_.Control(yaw_out); + this->pit_motor_.Control(pit_out); break; } } @@ -163,11 +263,22 @@ void Gimbal::SetMode(Mode mode) { memcpy(&(this->setpoint_.eulr_), &(this->eulr_), sizeof(this->setpoint_.eulr_)); /* 切换模式后重置设定值 */ - if (this->mode_ == RELAX) { - if (mode == ABSOLUTE) { - this->setpoint_.eulr_.yaw = this->eulr_.yaw; - } - } + this->setpoint_.eulr_.yaw = this->eulr_.yaw; + // if (this->mode_ == RELAX) { + // if (mode == ABSOLUTE) { + // this->setpoint_.eulr_.yaw = this->eulr_.yaw; + // } + // } + // if (this->mode_ == ABSOLUTE) { + // if (mode == AI_CONTROL) { + // this->setpoint_.eulr_.yaw = this->eulr_.yaw; + // } + // } + // if (this->mode_ == AI_CONTROL) { + // if (mode == ABSOLUTE) { + // this->setpoint_.eulr_.yaw = this->eulr_.yaw; + // } + // } this->mode_ = mode; } diff --git a/src/module/gimbal/mod_gimbal.hpp b/src/module/gimbal/mod_gimbal.hpp index 378c063d..80a4a20d 100644 --- a/src/module/gimbal/mod_gimbal.hpp +++ b/src/module/gimbal/mod_gimbal.hpp @@ -20,7 +20,9 @@ class Gimbal { /* 云台运行模式 */ typedef enum { RELAX, /* 放松模式，电机不输出。一般情况云台初始化之后的模式 */ - ABSOLUTE, /* 绝对坐标系控制，控制在空间内的绝对姿态 */ + ABSOLUTE, /* 绝对坐标系控制，控制在空间内的绝对姿态 */ + AI_CONTROL /*Host control mode, based on absolute coordinate system + control*/ } Mode; enum { @@ -34,8 +36,7 @@ class Gimbal { typedef enum { SET_MODE_RELAX, SET_MODE_ABSOLUTE, - START_AUTO_AIM, - STOP_AUTO_AIM, + SET_MODE_AUTO_AIM, } GimbalEvent; typedef struct { @@ -44,6 +45,8 @@ class Gimbal { Component::PosActuator::Param yaw_actr; Component::PosActuator::Param pit_actr; + Component::PosActuator::Param yaw_ai_actr; + Component::PosActuator::Param pit_ai_actr; Device::RMMotor::Param yaw_motor; Device::RMMotor::Param pit_motor; @@ -67,12 +70,20 @@ class Gimbal { void Control(); + float ChangeAngleRange(float angle); + void SetMode(Mode mode); static void DrawUIStatic(Gimbal *gimbal); static void DrawUIDynamic(Gimbal *gimbal); + void GetSlopeAngle(); + + float RotateVector3D(float x, float y, float z); + + double GetAlpha(); + private: uint64_t last_wakeup_ = 0; @@ -80,6 +91,9 @@ class Gimbal { float dt_ = 0.0f; + float yaw_motor_value_; + float pit_motor_value_; + Param param_; Gimbal::Mode mode_ = RELAX; /* 云台模式 */ @@ -92,6 +106,8 @@ class Gimbal { Component::PosActuator yaw_actuator_; Component::PosActuator pit_actuator_; + Component::PosActuator yaw_ai_actuator_; + Component::PosActuator pit_ai_actuator_; Device::RMMotor yaw_motor_; Device::RMMotor pit_motor_; @@ -101,8 +117,27 @@ class Gimbal { System::Semaphore ctrl_lock_; Message::Topic yaw_tp_ = Message::Topic("chassis_yaw"); + Message::Topic eulr_tp_ = Message::Topic("ahrs_eulr"); + Message::Topic quat_tp_ = Message::Topic("ahrs_quat"); + Message::Topic pit_tp_ = Message::Topic("chassis_pitch"); + Message::Topic alpha_tp_ = Message::Topic("chassis_alpha"); + Message::Topic eulr_yaw1_tp_ = + Message::Topic("chassis_eulr_yaw1"); /* 首次云台偏航角 */ + Message::Topic tan_pit_tp_ = + Message::Topic("chassis_tan_pit"); float yaw_; + float pit_; + double alpha_; + double slope_angle_; + float eulr_yaw1_; + double tan_pit_; + // Component::Type::CycleValue test_angle_2_; + double tan_rol_; + double test_angle_3_; + float test_angle_4_; + + float rotation_mat_[3][3]; Component::UI::String string_; diff --git a/src/module/launcher/mod_launcher.cpp b/src/module/launcher/mod_launcher.cpp index 7df68f5d..cdbede6d 100644 --- a/src/module/launcher/mod_launcher.cpp +++ b/src/module/launcher/mod_launcher.cpp @@ -3,28 +3,28 @@ #include "bsp_pwm.h" #include "bsp_time.h" -#define LAUNCHER_TRIG_SPEED_MAX (8191) +#define LAUNCHER_TRIG_SPEED_MAX (16000) using namespace Module; -Launcher::Launcher(Param& param, float control_freq) +template +Launcher::Launcher(Param& param, + float control_freq) : param_(param), ctrl_lock_(true) { - for (size_t i = 0; i < LAUNCHER_ACTR_TRIG_NUM; i++) { + for (size_t i = 0; i < Trig_num; i++) { this->trig_actuator_.at(i) = new Component::PosActuator(param.trig_actr.at(i), control_freq); - this->trig_motor_.at(i) = - new Device::RMMotor(this->param_.trig_motor.at(i), - ("Launcher_Trig" + std::to_string(i)).c_str()); + this->trig_motor_.at(i) = new Motor( + param.trig_param.at(i), ("Launcher_Trig" + std::to_string(i)).c_str()); } - for (size_t i = 0; i < LAUNCHER_ACTR_FRIC_NUM; i++) { + for (size_t i = 0; i < Fric_num; i++) { this->fric_actuator_.at(i) = new Component::SpeedActuator(param.fric_actr.at(i), control_freq); - this->fric_motor_.at(i) = - new Device::RMMotor(this->param_.fric_motor.at(i), - ("Launcher_Fric" + std::to_string(i)).c_str()); + this->fric_motor_.at(i) = new Motor( + param.fric_param.at(i), ("Launcher_Fric" + std::to_string(i)).c_str()); } auto event_callback = [](LauncherEvent event, Launcher* launcher) { @@ -76,8 +76,8 @@ Launcher::Launcher(Param& param, float control_freq) Component::CMD::RegisterEvent( event_callback, this, this->param_.EVENT_MAP); - bsp_pwm_start(BSP_PWM_LAUNCHER_SERVO); - bsp_pwm_set_comp(BSP_PWM_LAUNCHER_SERVO, this->param_.cover_close_duty); + // bsp_pwm_start(BSP_PWM_LAUNCHER_SERVO); + // bsp_pwm_set_comp(BSP_PWM_LAUNCHER_SERVO, this->param_.cover_close_duty); auto launcher_thread = [](Launcher* launcher) { auto ref_sub = Message::Subscriber("referee"); @@ -109,15 +109,16 @@ Launcher::Launcher(Param& param, float control_freq) System::Timer::Create(this->DrawUIDynamic, this, 100); } -void Launcher::UpdateFeedback() { +template +void Launcher::UpdateFeedback() { const float LAST_TRIG_MOTOR_ANGLE = this->trig_motor_[0]->GetAngle(); - for (size_t i = 0; i < LAUNCHER_ACTR_FRIC_NUM; i++) { + for (size_t i = 0; i < Fric_num; i++) { this->fric_motor_[i]->Update(); speed[i] = fric_motor_[i]->GetSpeed(); } - for (size_t i = 0; i < LAUNCHER_ACTR_TRIG_NUM; i++) { + for (size_t i = 0; i < Trig_num; i++) { this->trig_motor_[i]->Update(); } @@ -126,7 +127,8 @@ void Launcher::UpdateFeedback() { this->trig_angle_ += DELTA_MOTOR_ANGLE / this->param_.trig_gear_ratio; } -void Launcher::Control() { +template +void Launcher::Control() { this->now_ = bsp_time_get(); this->dt_ = TIME_DIFF(this->last_wakeup_, this->now_); @@ -236,19 +238,21 @@ void Launcher::Control() { /* 计算摩擦轮和拨弹盘并输出 */ switch (this->fire_ctrl_.fire_mode_) { case RELAX: - for (size_t i = 0; i < LAUNCHER_ACTR_TRIG_NUM; i++) { + for (size_t i = 0; i < Trig_num; i++) { this->trig_motor_[i]->Relax(); } - for (size_t i = 0; i < LAUNCHER_ACTR_FRIC_NUM; i++) { + for (size_t i = 0; i < Fric_num; i++) { this->fric_motor_[i]->Relax(); } - bsp_pwm_stop(BSP_PWM_LAUNCHER_SERVO); + // sp_pwm_stop(BSP_PWM_LAUNCHER_SERVO); break; case SAFE: case LOADED: - for (int i = 0; i < LAUNCHER_ACTR_TRIG_NUM; i++) { + for (size_t i = 0; i < Trig_num; i++) { /* 控制拨弹电机 */ + float trig_speed = this->trig_motor_[i]->GetSpeed(); + clampf(&trig_speed, 0.f, LAUNCHER_TRIG_SPEED_MAX); float trig_out = this->trig_actuator_[i]->Calculate( this->setpoint_.trig_angle_, this->trig_motor_[i]->GetSpeed() / LAUNCHER_TRIG_SPEED_MAX, @@ -257,7 +261,7 @@ void Launcher::Control() { this->trig_motor_[i]->Control(trig_out); } - for (size_t i = 0; i < LAUNCHER_ACTR_FRIC_NUM; i++) { + for (size_t i = 0; i < Fric_num; i++) { /* 控制摩擦轮 */ float fric_out = this->fric_actuator_[i]->Calculate( this->setpoint_.fric_rpm_[i], this->fric_motor_[i]->GetSpeed(), @@ -267,19 +271,23 @@ void Launcher::Control() { } /* 根据弹仓盖开关状态更新弹舱盖打开时舵机PWM占空比 */ - if (this->cover_mode_ == OPEN) { - bsp_pwm_start(BSP_PWM_LAUNCHER_SERVO); - bsp_pwm_set_comp(BSP_PWM_LAUNCHER_SERVO, this->param_.cover_open_duty); - } else { - bsp_pwm_start(BSP_PWM_LAUNCHER_SERVO); - bsp_pwm_set_comp(BSP_PWM_LAUNCHER_SERVO, this->param_.cover_close_duty); - } + // if (this->cover_mode_ == OPEN) { + // bsp_pwm_start(BSP_PWM_LAUNCHER_SERVO); + // bsp_pwm_set_comp(BSP_PWM_LAUNCHER_SERVO, + // this->param_.cover_open_duty); + // } else { + // bsp_pwm_start(BSP_PWM_LAUNCHER_SERVO); + // bsp_pwm_set_comp(BSP_PWM_LAUNCHER_SERVO, + // this->param_.cover_close_duty); + // } break; } } /* 拨弹盘模式 */ /* SINGLE,BURST,CONTINUED, */ -void Launcher::SetTrigMode(TrigMode mode) { +template +void Launcher::SetTrigMode( + TrigMode mode) { if (mode == this->fire_ctrl_.trig_mode_) { return; } @@ -288,14 +296,16 @@ void Launcher::SetTrigMode(TrigMode mode) { } /* 设置摩擦轮模式 */ /* RELEX SAFE LOADED三种模式可以选择 */ -void Launcher::SetFireMode(FireMode mode) { +template +void Launcher::SetFireMode( + FireMode mode) { if (mode == this->fire_ctrl_.fire_mode_) { /* 未更改，return */ return; } fire_ctrl_.fire = false; - for (size_t i = 0; i < LAUNCHER_ACTR_FRIC_NUM; i++) { + for (size_t i = 0; i < Fric_num; i++) { this->fric_actuator_[i]->Reset(); } /* reset 所有电机执行器PID等参数 */ @@ -306,7 +316,8 @@ void Launcher::SetFireMode(FireMode mode) { this->fire_ctrl_.fire_mode_ = mode; } -void Launcher::HeatLimit() { +template +void Launcher::HeatLimit() { if (this->ref_.status == Device::Referee::RUNNING) { /* 根据机器人型号获得对应数据 */ if (this->param_.model == LAUNCHER_MODEL_42MM) { @@ -340,7 +351,8 @@ void Launcher::HeatLimit() { } } -void Launcher::PraseRef() { +template +void Launcher::PraseRef() { memcpy(&(this->ref_.power_heat), &(this->raw_ref_.power_heat), sizeof(this->ref_.power_heat)); memcpy(&(this->ref_.robot_status), &(this->raw_ref_.robot_status), @@ -350,29 +362,30 @@ void Launcher::PraseRef() { this->ref_.status = this->raw_ref_.status; } -float Launcher::LimitLauncherFreq() { /* 热量限制计算 */ +template +float Launcher::LimitLauncherFreq() { /* 热量限制计算 */ float heat_percent = this->heat_ctrl_.heat / this->heat_ctrl_.heat_limit; float stable_freq = this->heat_ctrl_.cooling_rate / this->heat_ctrl_.heat_increase; /* 每秒可发弹量 */ if (this->param_.model == LAUNCHER_MODEL_42MM) { return stable_freq; + } else if (heat_percent > 0.9f) { + return 0.5f; + } else if (heat_percent > 0.85f) { + return stable_freq * 0.6f; + } else if (heat_percent > 0.8f) { + return 0.6f * 1000 / this->param_.min_launch_delay; + } else if (heat_percent > 0.6f) { + return 0.8f * 1000 / this->param_.min_launch_delay; } else { - if (heat_percent > 0.9f) { - return 0.5f; - } else if (heat_percent > 0.8f) { - return 1.0f; - } else if (heat_percent > 0.6f) { - return 2.0f * stable_freq; - } else if (heat_percent > 0.2f) { - return 3.0f * stable_freq; - } else if (heat_percent > 0.1f) { - return 4.0f * stable_freq; - } else { - return 5.0f; - } + return 1000 / this->param_.min_launch_delay; } } -void Launcher::DrawUIStatic(Launcher* launcher) { + +template +void Launcher::DrawUIStatic( + Launcher* launcher) { launcher->string_.Draw("SM", Component::UI::UI_GRAPHIC_OP_ADD, Component::UI::UI_GRAPHIC_LAYER_CONST, Component::UI::UI_GREEN, UI_DEFAULT_WIDTH * 10, 80, @@ -511,7 +524,9 @@ void Launcher::DrawUIStatic(Launcher* launcher) { Device::Referee::AddUI(launcher->arc_); } -void Launcher::DrawUIDynamic(Launcher* launcher) { +template +void Launcher::DrawUIDynamic( + Launcher* launcher) { float box_pos_left = 0.0f, box_pos_right = 0.0f; /* 更新发射器模式选择框 */ @@ -655,3 +670,5 @@ void Launcher::DrawUIDynamic(Launcher* launcher) { 40, 40); Device::Referee::AddUI(launcher->arc_); } + +template class Module::Launcher; diff --git a/src/module/launcher/mod_launcher.hpp b/src/module/launcher/mod_launcher.hpp index b569c249..138528ac 100644 --- a/src/module/launcher/mod_launcher.hpp +++ b/src/module/launcher/mod_launcher.hpp @@ -10,6 +10,8 @@ #include "dev_rm_motor.hpp" namespace Module { +template class Launcher { public: /* 发射器运行模式 */ @@ -44,24 +46,6 @@ class Launcher { CLOSE_COVER, } LauncherEvent; - enum { - LAUNCHER_ACTR_FRIC1_IDX = 0, /* 1号摩擦轮相关的索引值 */ - LAUNCHER_ACTR_FRIC2_IDX, /* 2号摩擦轮相关的索引值 */ - LAUNCHER_ACTR_FRIC_NUM, /* 总共的动作器数量 */ - }; - - enum { - LAUNCHER_ACTR_TRIG_IDX, /* 拨弹电机相关的索引值 */ - LAUNCHER_ACTR_TRIG_NUM, /* 总共的动作器数量 */ - }; - - enum { - LAUNCHER_CTRL_FRIC1_SPEED_IDX = 0, /* 摩擦轮1控制的速度环控制器的索引值 */ - LAUNCHER_CTRL_FRIC2_SPEED_IDX, /* 摩擦轮2控制的速度环控制器的索引值 */ - LAUNCHER_CTRL_TRIG_SPEED_IDX, /* 拨弹电机控制的速度环控制器的索引值 */ - LAUNCHER_CTRL_NUM, /* 总共的控制器数量 */ - }; - typedef enum { LAUNCHER_MODEL_17MM = 0, /* 17mm发射机构 */ LAUNCHER_MODEL_42MM, /* 42mm发射机构 */ @@ -77,11 +61,10 @@ class Launcher { float default_bullet_speed; /* 默认弹丸初速度 */ uint32_t min_launch_delay; /* 最小发射间隔(1s/最大射频) */ - std::array trig_actr; - std::array - fric_actr; - std::array trig_motor; - std::array fric_motor; + std::array trig_actr; + std::array fric_actr; + std::array trig_param; + std::array fric_param; const std::vector EVENT_MAP; } Param; @@ -165,11 +148,11 @@ class Launcher { HeatControl heat_ctrl_; FireControl fire_ctrl_; - std::array trig_actuator_; - std::array fric_actuator_; + std::array trig_actuator_; + std::array fric_actuator_; - std::array trig_motor_; - std::array fric_motor_; + std::array trig_motor_; + std::array fric_motor_; RefForLauncher ref_; @@ -185,4 +168,5 @@ class Launcher { Component::UI::Arc arc_; }; +typedef class Launcher RMLauncher; } // namespace Module diff --git a/src/robot/sentry/robot.cpp b/src/robot/sentry/robot.cpp index bac02e7c..da50368c 100644 --- a/src/robot/sentry/robot.cpp +++ b/src/robot/sentry/robot.cpp @@ -10,13 +10,13 @@ Robot::Sentry::Param param = { .chassis={ .toque_coefficient_ = 0.0327120418848f, - .speed_2_coefficient_ = 2.300974248103511e-07f, - .out_2_coefficient_ = 2.1455244766462685e-29f, - .constant_ = 0.23958431845825284f, + .speed_2_coefficient_ = 1.227822928729637e-07, + .out_2_coefficient_ = 1.1108430132455055e-24, + .constant_ = 1.8135014050213443, .type = Component::Mixer::OMNICROSS, .follow_pid_param = { - .k = 0.5f, + .k = 1.f, .p = 1.0f, .i = 0.0f, .d = 0.0f, @@ -25,29 +25,29 @@ Robot::Sentry::Param param = { .d_cutoff_freq = -1.0f, .cycle = true, }, - - .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_INDENPENDENT - }, - Component::CMD::EventMapItem{ - Device::DR16::DR16_SW_L_POS_MID, - Module::RMChassis::SET_MODE_FOLLOW - }, - Component::CMD::EventMapItem{ - Device::DR16::DR16_SW_L_POS_BOT, - Module::RMChassis::SET_MODE_ROTOR + .xaccl_pid_param = + { + .k = 1.0f, + .p = 0.6f, + .i = 1.6f, + .d = 0.00f, + .i_limit = 1.0f, + .out_limit = 1.0f, + .d_cutoff_freq = -0.001f, + .cycle = false, + }, + .yaccl_pid_param = + { + .k = 1.0f, + .p = 0.6f, + .i = 1.6f, + .d = 0.00f, + .i_limit = 1.0f, + .out_limit = 1.0f, + .d_cutoff_freq = -0.001f, + .cycle = false, }, - Component::CMD::EventMapItem{ - Device::AI::AIControlData::AI_ROTOR, - Module::RMChassis::SET_MODE_ROTOR - } - }, + .actuator_param = { Component::SpeedActuator::Param{ @@ -119,14 +119,14 @@ Robot::Sentry::Param param = { .motor_param = { Device::RMMotor::Param{ - .id_feedback = 0x201, + .id_feedback = 0x204, .id_control = M3508_M2006_CTRL_ID_BASE, .model = Device::RMMotor::MOTOR_M3508, .can = BSP_CAN_1, .reverse = false }, Device::RMMotor::Param{ - .id_feedback = 0x202, + .id_feedback = 0x201, .id_control = M3508_M2006_CTRL_ID_BASE, .model = Device::RMMotor::MOTOR_M3508, .can = BSP_CAN_1, @@ -140,30 +140,34 @@ Robot::Sentry::Param param = { .reverse = false }, Device::RMMotor::Param{ - .id_feedback = 0x204, + .id_feedback = 0x202, .id_control = M3508_M2006_CTRL_ID_BASE, .model = Device::RMMotor::MOTOR_M3508, .can = BSP_CAN_1, .reverse = false }, }, - - .get_speed = [](float power_limit){ - float speed = 0.0f; - if (power_limit <= 50.0f) { - speed = 0.0f; - } else if (power_limit <= 60.0f) { - speed = 3800; - } else if (power_limit <= 70.0f) { - speed = 5000; - } else if (power_limit <= 80.0f) { - speed = 5500; - } else if (power_limit <= 100.0f) { - speed = 6000; - } else { - speed = 6500; + .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_FOLLOW + }, + Component::CMD::EventMapItem{ + Device::DR16::DR16_SW_L_POS_BOT, + Module::RMChassis::SET_MODE_INDENPENDENT + }, + Component::CMD::EventMapItem{ + Device::AI::AIControlData::AI_ROTOR, + Module::RMChassis::SET_MODE_ROTOR } - return speed; }, }, @@ -191,7 +195,7 @@ Robot::Sentry::Param param = { /* GIMBAL_CTRL_YAW_OMEGA_IDX */ .k = 0.28f, .p = 1.f, - .i = 1.f, + .i = 10.f, .d = 0.f, .i_limit = 0.2f, .out_limit = 1.0f, @@ -201,10 +205,10 @@ Robot::Sentry::Param param = { .position = { /* GIMBAL_CTRL_YAW_ANGLE_IDX */ - .k = 25.0f, - .p = 1.0f, - .i = 0.0f, - .d = 0.0f, + .k = 15.0f, + .p = 1.5f, + .i = 5.f, + .d = 1.0f, .i_limit = 0.0f, .out_limit = 10.0f, .d_cutoff_freq = -1.0f, @@ -230,10 +234,68 @@ Robot::Sentry::Param param = { .position = { /* GIMBAL_CTRL_PIT_ANGLE_IDX */ + .k = 15.0f, + .p = 1.5f, + .i = 5.0f, + .d = 1.0f, + .i_limit = 0.0f, + .out_limit = 10.0f, + .d_cutoff_freq = -1.0f, + .cycle = true, + }, + + .in_cutoff_freq = -1.0f, + + .out_cutoff_freq = -1.0f, + }, + .yaw_ai_actr = { + .speed = { + /* GIMBAL_CTRL_YAW_OMEGA_IDX */ + .k = 0.28f, + .p = 1.1f, + .i = 10.f, + .d = 0.f, + .i_limit = 0.2f, + .out_limit = 1.0f, + .d_cutoff_freq = -1.0f, + .cycle = false, + }, + + .position = { + /* GIMBAL_CTRL_YAW_ANGLE_IDX */ .k = 25.0f, - .p = 1.0f, + .p = 1.5f, + .i = 5.f, + .d = 1.0f, + .i_limit = 0.0f, + .out_limit = 10.0f, + .d_cutoff_freq = -1.0f, + .cycle = true, + }, + + .in_cutoff_freq = -1.0f, + + .out_cutoff_freq = -1.0f, + }, + .pit_ai_actr = { + .speed = { + /* GIMBAL_CTRL_PIT_OMEGA_IDX */ + .k = 0.1f, + .p = 1.2f, .i = 0.0f, - .d = 0.0f, + .d = 0.f, + .i_limit = 0.8f, + .out_limit = 1.0f, + .d_cutoff_freq = -1.0f, + .cycle = false, + }, + + .position = { + /* GIMBAL_CTRL_PIT_ANGLE_IDX */ + .k = 27.0f, + .p = 2.2f, + .i = 6.0f, + .d = 1.0f, .i_limit = 0.0f, .out_limit = 10.0f, .d_cutoff_freq = -1.0f, @@ -249,29 +311,29 @@ Robot::Sentry::Param param = { .id_control = GM6020_CTRL_ID_BASE, .model = Device::RMMotor::MOTOR_GM6020, .can = BSP_CAN_1, - .reverse = false + .reverse = true }, .pit_motor = { - .id_feedback = 0x209, - .id_control = GM6020_CTRL_ID_EXTAND, + .id_feedback = 0x205, + .id_control = GM6020_CTRL_ID_BASE, .model = Device::RMMotor::MOTOR_GM6020, .can = BSP_CAN_2, - .reverse = false + .reverse = true }, .mech_zero = { - .yaw = 1.58f, - .pit = 4.6f, + .yaw = M_2PI-0.46f, + .pit = M_2PI-2.2, .rol = 0.0f, }, .limit = { - .pitch_max = 4.9f, - .pitch_min = 4.46f, + .pitch_max = M_2PI-1.7f, + .pitch_min = M_2PI-2.4f, .yaw_max = 0.0f, - .yaw_min = 0.0f, - }, + .yaw_min = 0.0f + }, .EVENT_MAP = { Component::CMD::EventMapItem{ @@ -284,7 +346,7 @@ Robot::Sentry::Param param = { }, Component::CMD::EventMapItem{ Device::DR16::DR16_SW_R_POS_MID, - Module::Gimbal::SET_MODE_ABSOLUTE + Module::Gimbal::SET_MODE_AUTO_AIM }, Component::CMD::EventMapItem{ Device::DR16::DR16_SW_R_POS_BOT, @@ -294,15 +356,15 @@ Robot::Sentry::Param param = { }, - .launcher1 = { + .launcher = { .num_trig_tooth = 8.0f, .trig_gear_ratio = 36.0f, .fric_radius = 0.03f, .cover_open_duty = 0.125f, .cover_close_duty = 0.075f, - .model = Module::Launcher::LAUNCHER_MODEL_17MM, + .model = Module::RMLauncher::LAUNCHER_MODEL_17MM, .default_bullet_speed = 15.f, - .min_launch_delay = static_cast(1000.0f / 20.0f), + .min_launch_delay = static_cast(1000.0f / 10.0f), .trig_actr = { Component::PosActuator::Param{ @@ -369,23 +431,23 @@ Robot::Sentry::Param param = { }, }, - .trig_motor = { + .trig_param = { Device::RMMotor::Param{ - .id_feedback = 0x207, + .id_feedback = 0x205, .id_control = M3508_M2006_CTRL_ID_EXTAND, .model = Device::RMMotor::MOTOR_M2006, - .can = BSP_CAN_2, - .reverse = true, + .can = BSP_CAN_1, + .reverse = false, } }, - .fric_motor = { + .fric_param = { Device::RMMotor::Param{ - .id_feedback = 0x204, + .id_feedback = 0x202, .id_control = M3508_M2006_CTRL_ID_BASE, .model = Device::RMMotor::MOTOR_M3508, .can = BSP_CAN_2, - .reverse = false + .reverse = false, }, Device::RMMotor::Param{ .id_feedback = 0x203, @@ -396,179 +458,48 @@ Robot::Sentry::Param param = { } }, - .EVENT_MAP = { - Component::CMD::EventMapItem{ - Component::CMD::CMD_EVENT_LOST_CTRL, - Module::Launcher::CHANGE_FIRE_MODE_RELAX - }, - Component::CMD::EventMapItem{ - Device::DR16::DR16_SW_R_POS_TOP, - Module::Launcher::CHANGE_FIRE_MODE_SAFE - }, - Component::CMD::EventMapItem{ - Device::DR16::DR16_SW_R_POS_MID, - Module::Launcher::CHANGE_FIRE_MODE_LOADED - }, - Component::CMD::EventMapItem{ - Device::DR16::DR16_SW_R_POS_BOT, - Module::Launcher::CHANGE_FIRE_MODE_LOADED - }, - Component::CMD::EventMapItem{ - Device::DR16::DR16_SW_R_POS_BOT, - Module::Launcher::LAUNCHER_START_FIRE - }, - Component::CMD::EventMapItem{ - Device::AI::AIControlData::AI_FIRE_COMMAND, - Module::Launcher::CHANGE_TRIG_MODE_BURST - }, - Component::CMD::EventMapItem{ - Device::AI::AIControlData::AI_STOP_FIRE, - Module::Launcher::LAUNCHER_STOP_TRIG - } - - }, - }, /* launcher1 */ -.launcher2 = { - .num_trig_tooth = 8.0f, - .trig_gear_ratio = 36.0f, - .fric_radius = 0.03f, - .cover_open_duty = 0.125f, - .cover_close_duty = 0.075f, - .model = Module::Launcher::LAUNCHER_MODEL_17MM, - .default_bullet_speed = 15.f, - .min_launch_delay = static_cast(1000.0f / 20.0f), - - .trig_actr = { - Component::PosActuator::Param{ - .speed = { - .k = 3.0f, - .p = 1.0f, - .i = 0.0f, - .d = 0.0f, - .i_limit = 0.5f, - .out_limit = 1.0f, - .d_cutoff_freq = -1.0f, - .cycle = false, - }, - - .position = { - .k = 1.5f, - .p = 1.0f, - .i = 0.0f, - .d = 0.0f, - .i_limit = 1.0f, - .out_limit = 1.0f, - .d_cutoff_freq = -1.0f, - .cycle = true, - }, - - .in_cutoff_freq = -1.0f, - .out_cutoff_freq = -1.0f, - }, - }, - - .fric_actr = { - Component::SpeedActuator::Param{ - .speed = { - .k = 0.00035f, - .p = 1.0f, - .i = 0.5f, - .d = 0.0f, - .i_limit = 0.5f, - .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.00035f, - .p = 1.0f, - .i = 0.5f, - .d = 0.0f, - .i_limit = 0.5f, - .out_limit = 1.0f, - .d_cutoff_freq = -1.0f, - .cycle = false, - }, - - .in_cutoff_freq = -1.0f, - - .out_cutoff_freq = -1.0f, - }, - }, - - .trig_motor = { - Device::RMMotor::Param{ - .id_feedback =0x206, - .id_control = M3508_M2006_CTRL_ID_EXTAND, - .model = Device::RMMotor::MOTOR_M2006, - .can = BSP_CAN_2, - .reverse = true, - }, - }, - - .fric_motor = { - Device::RMMotor::Param{ - .id_feedback = 0x202, - .id_control = M3508_M2006_CTRL_ID_BASE, - .model = Device::RMMotor::MOTOR_M3508, - .can = BSP_CAN_2, - .reverse = false, - }, - Device::RMMotor::Param{ - .id_feedback = 0x201, - .id_control = M3508_M2006_CTRL_ID_BASE, - .model = Device::RMMotor::MOTOR_M3508, - .can = BSP_CAN_2, - .reverse = false, - }, - }, - - .EVENT_MAP = { + .EVENT_MAP = { Component::CMD::EventMapItem{ Component::CMD::CMD_EVENT_LOST_CTRL, - Module::Launcher::CHANGE_FIRE_MODE_RELAX + Module::RMLauncher::CHANGE_FIRE_MODE_RELAX }, Component::CMD::EventMapItem{ Device::DR16::DR16_SW_R_POS_TOP, - Module::Launcher::CHANGE_FIRE_MODE_SAFE + Module::RMLauncher::CHANGE_FIRE_MODE_SAFE }, Component::CMD::EventMapItem{ Device::DR16::DR16_SW_R_POS_MID, - Module::Launcher::CHANGE_FIRE_MODE_LOADED + Module::RMLauncher::CHANGE_FIRE_MODE_SAFE }, Component::CMD::EventMapItem{ Device::DR16::DR16_SW_R_POS_BOT, - Module::Launcher::CHANGE_FIRE_MODE_LOADED + Module::RMLauncher::CHANGE_FIRE_MODE_SAFE }, Component::CMD::EventMapItem{ Device::DR16::DR16_SW_R_POS_BOT, - Module::Launcher::LAUNCHER_START_FIRE + Module::RMLauncher::CHANGE_TRIG_MODE_BURST }, Component::CMD::EventMapItem{ Device::AI::AIControlData::AI_FIRE_COMMAND, - Module::Launcher::CHANGE_TRIG_MODE_BURST + Module::RMLauncher::CHANGE_TRIG_MODE_BURST }, Component::CMD::EventMapItem{ Device::AI::AIControlData::AI_STOP_FIRE, - Module::Launcher::LAUNCHER_STOP_TRIG + Module::RMLauncher::LAUNCHER_STOP_TRIG } }, - }, /* launcher */ - .bmi088_rot = { - .rot_mat = { - { +1, +0, +0}, - { +0, +1, +0}, - { +0, +0, +1}, - }, - }, + }, /* launcher*/ + .bmi088_rot = + { + .rot_mat = + { + {+0, +1, +0}, + {-1, +0, +0}, + {+0, +0, +1}, + }, + }, .cap = { .can = BSP_CAN_1, diff --git a/src/robot/sentry/robot.hpp b/src/robot/sentry/robot.hpp index 95e1b8cb..56dc9380 100644 --- a/src/robot/sentry/robot.hpp +++ b/src/robot/sentry/robot.hpp @@ -18,8 +18,7 @@ class Sentry { typedef struct Param { Module::RMChassis::Param chassis; Module::Gimbal::Param gimbal; - Module::Launcher::Param launcher1; - Module::Launcher::Param launcher2; + Module::RMLauncher::Param launcher; Device::BMI088::Rotation bmi088_rot{}; Device::Cap::Param cap{}; } Param; @@ -38,8 +37,7 @@ class Sentry { Module::RMChassis chassis_; Module::Gimbal gimbal_; - Module::Launcher launcher1_; - Module::Launcher launcher2_; + Module::RMLauncher launcher_; Sentry(Param& param, float control_freq) : cmd_(Component::CMD::CMD_AUTO_CTRL), ai_(false), @@ -47,7 +45,6 @@ class Sentry { cap_(param.cap), chassis_(param.chassis, control_freq), gimbal_(param.gimbal, control_freq), - launcher1_(param.launcher1, control_freq), - launcher2_(param.launcher2, control_freq) {} + launcher_(param.launcher, control_freq) {} }; } // namespace Robot From 55501f5f7923723e2ed2f4e2602e24cceef4aec2 Mon Sep 17 00:00:00 2001 From: llLeo306 Date: Mon, 28 Apr 2025 22:10:24 +0800 Subject: [PATCH 2/2] sunmodules changes --- lib/host-protocol | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/host-protocol b/lib/host-protocol index 06f5d56d..5e0a1a9b 160000 --- a/lib/host-protocol +++ b/lib/host-protocol @@ -1 +1 @@ -Subproject commit 06f5d56d62baa44d8265a514cd6e997699289c35 +Subproject commit 5e0a1a9b2c3c3229690a2934d625d33acfa5020b