From 2dd89afff10b45b785e77dd6068813ce0a8e613e Mon Sep 17 00:00:00 2001 From: Marine_zxy <1423266892@qq.com> Date: Wed, 13 Mar 2024 19:42:41 +0800 Subject: [PATCH 01/10] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E5=93=A8=E5=85=B5?= =?UTF-8?q?=E5=B7=A1=E9=80=BB=E6=A8=A1=E5=BC=8F=E8=BF=90=E5=8A=A8=E9=80=BB?= =?UTF-8?q?=E8=BE=91=EF=BC=8C=E4=BF=AE=E6=AD=A3=E9=83=A8=E5=88=86=E5=8F=98?= =?UTF-8?q?=E9=87=8F=E5=91=BD=E5=90=8D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/device/ai/dev_ai.cpp | 51 +++++-- src/device/ai/dev_ai.hpp | 16 ++- src/module/gimbal/mod_gimbal.cpp | 66 ++++++--- src/module/gimbal/mod_gimbal.hpp | 13 +- src/robot/sentry/robot.cpp | 227 +++++++++++++++++++++++++++---- src/robot/sentry/robot.hpp | 14 +- 6 files changed, 321 insertions(+), 66 deletions(-) diff --git a/src/device/ai/dev_ai.cpp b/src/device/ai/dev_ai.cpp index dfa9e091..82992c48 100644 --- a/src/device/ai/dev_ai.cpp +++ b/src/device/ai/dev_ai.cpp @@ -16,7 +16,10 @@ static uint8_t txbuf[AI_LEN_TX_BUFF]; using namespace Device; -AI::AI() : data_ready_(false), cmd_tp_("cmd_ai") { +AI::AI() + : data_ready_(false), + event_(Message::Event::FindEvent("cmd_event")), + cmd_tp_("cmd_ai") { auto rx_cplt_callback = [](void *arg) { AI *ai = static_cast(arg); ai->data_ready_.Post(); @@ -69,10 +72,10 @@ bool AI::StartRecv() { } bool AI::PraseHost() { - if (Component::CRC16::Verify(rxbuf, sizeof(this->form_host_))) { + if (Component::CRC16::Verify(rxbuf, sizeof(this->from_host_))) { this->cmd_.online = true; this->last_online_time_ = bsp_time_get_ms(); - memcpy(&(this->form_host_), rxbuf, sizeof(this->form_host_)); + memcpy(&(this->from_host_), rxbuf, sizeof(this->from_host_)); memset(rxbuf, 0, AI_LEN_RX_BUFF); return true; } @@ -132,23 +135,53 @@ bool AI::PackRef() { return true; } -bool AI::PackCMD() { +bool AI::PackCMD() { /* 将原始数据进行copy转移 */ this->cmd_.gimbal.mode = Component::CMD::GIMBAL_ABSOLUTE_CTRL; - memcpy(&(this->cmd_.gimbal.eulr), &(this->form_host_.data.gimbal), + memcpy(&(this->cmd_.gimbal.eulr), &(this->from_host_.data.gimbal), sizeof(this->cmd_.gimbal.eulr)); memcpy(&(this->cmd_.ext.extern_channel), - &(this->form_host_.data.extern_channel), + &(this->from_host_.data.extern_channel), sizeof(this->cmd_.ext.extern_channel)); - memcpy(&(this->cmd_.chassis), &(this->form_host_.data.chassis_move_vec), - sizeof(this->form_host_.data.chassis_move_vec)); + memcpy(&(this->cmd_.chassis), &(this->from_host_.data.chassis_move_vec), + sizeof(this->from_host_.data.chassis_move_vec)); + // memcpy(&(this->fire_command_), &(this->from_host_.data.notice), + // sizeof(this->from_host_.data.notice)); + + /* 判断是否为有效数据,即是否看到目标 */ + if (fabs(this->cmd_.gimbal.last_eulr.pit - this->cmd_.gimbal.last_eulr.pit) < + 0.01 && + fabs(this->cmd_.gimbal.last_eulr.yaw - this->cmd_.gimbal.last_eulr.yaw) < + 0.01) { + this->ai_data_ = this->IS_INVALID_DATA; + } else { + this->ai_data_ = this->IS_USEFUL_DATA; + } + + /*AI数据无效，云台采用巡逻模式 */ + // if (this->ai_data_ == this->IS_INVALID_DATA) { + // this->event_.Active(AI_AUTOPATROL); + // this->fire_command_ = 0; + // } + + /*AI数据有效，采用自动瞄准模式，也就是云台ABSOLUTE模式 */ + // if (this->ai_data_ == IS_USEFUL_DATA) { + // this->event_.Active(AI_FIND_TARGET); + // this->fire_command_ = this->from_host_.data.notice; /* 0或者2 */ + // } + + /* AI开火发弹指令 */ + if (this->ai_data_ == IS_USEFUL_DATA && this->fire_command_ != 0) { + this->event_.Active(AI_FIRE_COMMAND); + } + + this->cmd_.gimbal.last_eulr = this->cmd_.gimbal.eulr; this->cmd_.ctrl_source = Component::CMD::CTRL_SOURCE_AI; this->cmd_tp_.Publish(this->cmd_); - return true; } diff --git a/src/device/ai/dev_ai.hpp b/src/device/ai/dev_ai.hpp index 97b75136..39107bca 100644 --- a/src/device/ai/dev_ai.hpp +++ b/src/device/ai/dev_ai.hpp @@ -31,6 +31,14 @@ class AI { uint32_t hp; } RefForAI; + typedef enum { + AI_FIRE_COMMAND = 128, + AI_FIND_TARGET, + AI_AUTOPATROL, + IS_INVALID_DATA, + IS_USEFUL_DATA + } AI_DATA; + /* 这个变量如何跟notice建立联系 */ AI(); bool StartRecv(); @@ -53,7 +61,11 @@ class AI { bool ref_updated_ = false; uint32_t last_online_time_ = 0; - Protocol_DownPackage_t form_host_{}; + Protocol_DownPackage_t from_host_{}; /* 从ai拿到的原始数据数组 */ + + uint8_t ai_data_; + + uint8_t fire_command_; struct { RefereePckage ref{}; @@ -66,6 +78,8 @@ class AI { System::Semaphore data_ready_; + Message::Event event_; /* 为了上面那个功能，尝试中 */ + Message::Topic cmd_tp_; Component::CMD::Data cmd_{}; diff --git a/src/module/gimbal/mod_gimbal.cpp b/src/module/gimbal/mod_gimbal.cpp index 0d1613b6..009e83eb 100644 --- a/src/module/gimbal/mod_gimbal.cpp +++ b/src/module/gimbal/mod_gimbal.cpp @@ -1,7 +1,3 @@ -/* - * 云台模组 - */ - #include "mod_gimbal.hpp" #include @@ -30,12 +26,16 @@ Gimbal::Gimbal(Param& param, float control_freq) case SET_MODE_ABSOLUTE: gimbal->SetMode(static_cast(event)); break; + case START_AUTO_AIM: Component::CMD::SetCtrlSource(Component::CMD::CTRL_SOURCE_AI); break; case STOP_AUTO_AIM: Component::CMD::SetCtrlSource(Component::CMD::CTRL_SOURCE_RC); break; + case SET_AUTOPATROL: /* 自动巡逻 */ + gimbal->SetMode(static_cast(AUTOPATROL)); + break; } gimbal->ctrl_lock_.Post(); }; @@ -54,16 +54,16 @@ Gimbal::Gimbal(Param& param, float control_freq) while (1) { /* 读取控制指令、姿态、IMU、电机反馈 */ - eulr_sub.DumpData(gimbal->eulr_); - gyro_sub.DumpData(gimbal->gyro_); - cmd_sub.DumpData(gimbal->cmd_); + eulr_sub.DumpData(gimbal->eulr_); /* imu */ + gyro_sub.DumpData(gimbal->gyro_); /* imu */ + cmd_sub.DumpData(gimbal->cmd_); /* cmd */ gimbal->ctrl_lock_.Wait(UINT32_MAX); gimbal->UpdateFeedback(); gimbal->Control(); gimbal->ctrl_lock_.Post(); - gimbal->yaw_tp_.Publish(gimbal->yaw_); + gimbal->yaw_tp_.Publish(gimbal->yaw_); /* 底盘用的yaw_ */ /* 运行结束，等待下一次唤醒 */ gimbal->thread_.SleepUntil(2, last_online_time); @@ -79,10 +79,16 @@ Gimbal::Gimbal(Param& param, float control_freq) } void Gimbal::UpdateFeedback() { - this->pit_motor_.Update(); + this->pit_motor_.Update(); /* 电机*/ this->yaw_motor_.Update(); - - this->yaw_ = this->yaw_motor_.GetAngle() - this->param_.mech_zero.yaw; + switch (this->mode_) { + case RELAX: + case ABSOLUTE: + this->yaw_ = this->yaw_motor_.GetAngle() - this->param_.mech_zero.yaw; + case AUTOPATROL: + this->yaw_ = this->yaw_virtual_; + break; + } } void Gimbal::Control() { @@ -121,23 +127,43 @@ void Gimbal::Control() { this->setpoint_.eulr_.pit += gimbal_pit_cmd; /* 控制相关逻辑 */ + float yaw_out = 0; + float pit_out = 0; switch (this->mode_) { case RELAX: this->yaw_motor_.Relax(); this->pit_motor_.Relax(); break; - case ABSOLUTE: + + case ABSOLUTE: //作为瞄准模式使用 /* Yaw轴角速度环参数计算 */ - float yaw_out = this->yaw_actuator_.Calculate( + yaw_out = this->yaw_actuator_.Calculate( this->setpoint_.eulr_.yaw, this->gyro_.z, this->eulr_.yaw, this->dt_); - float pit_out = this->pit_actuator_.Calculate( + pit_out = this->pit_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; + + case AUTOPATROL: + /* 以sin变化左右摆头 */ + + this->t_ = this->t_ + 0.01; + this->setpoint_.eulr_.yaw = + this->eulr_.yaw + this->param_.patrol_rate * sin(this->t_); + + yaw_out = this->yaw_actuator_.Calculate( + this->setpoint_.eulr_.yaw, this->gyro_.z, this->eulr_.yaw, this->dt_); + + pit_out = this->pit_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; } } @@ -152,9 +178,17 @@ void Gimbal::SetMode(Mode mode) { memcpy(&(this->setpoint_.eulr_), &(this->eulr_), sizeof(this->setpoint_.eulr_)); /* 切换模式后重置设定值 */ - if (this->mode_ == RELAX) { + if (this->mode_ != ABSOLUTE) { if (mode == ABSOLUTE) { - this->setpoint_.eulr_.yaw = this->eulr_.yaw; + this->setpoint_.eulr_.yaw = this->eulr_.yaw; /* imu的eulr */ + } + } + if (this->mode_ != AUTOPATROL) { + if (mode == AUTOPATROL) { /* 切换到AUTOPATROL */ + this->t_ = 0; /* t_置零，保证sin从0开始 */ + this->yaw_virtual_ = + this->yaw_motor_.GetAngle() - this->param_.mech_zero.yaw; + /* 锁定切换模式时的yaw角度作为巡逻基准角度 */ } } diff --git a/src/module/gimbal/mod_gimbal.hpp b/src/module/gimbal/mod_gimbal.hpp index 0c65e0d9..4dfe2b0d 100644 --- a/src/module/gimbal/mod_gimbal.hpp +++ b/src/module/gimbal/mod_gimbal.hpp @@ -1,7 +1,3 @@ -/* - * 云台模组 - */ - #pragma once #include @@ -25,6 +21,7 @@ class Gimbal { typedef enum { RELAX, /* 放松模式，电机不输出。一般情况云台初始化之后的模式 */ ABSOLUTE, /* 绝对坐标系控制，控制在空间内的绝对姿态 */ + AUTOPATROL, } Mode; enum { @@ -39,7 +36,8 @@ class Gimbal { SET_MODE_RELAX, SET_MODE_ABSOLUTE, START_AUTO_AIM, - STOP_AUTO_AIM + STOP_AUTO_AIM, + SET_AUTOPATROL, } GimbalEvent; typedef struct { @@ -54,6 +52,8 @@ class Gimbal { Component::Type::Eulr mech_zero; + double patrol_rate; + struct { Component::Type::CycleValue pitch_max; Component::Type::CycleValue pitch_min; @@ -104,7 +104,10 @@ class Gimbal { Message::Topic yaw_tp_ = Message::Topic("chassis_yaw"); + double t_; + float yaw_; + float yaw_virtual_; /* 用于标定哨兵小陀螺摆头巡航模式下的正方向 */ Component::UI::String string_; diff --git a/src/robot/sentry/robot.cpp b/src/robot/sentry/robot.cpp index 45e586b7..d7bf09eb 100644 --- a/src/robot/sentry/robot.cpp +++ b/src/robot/sentry/robot.cpp @@ -7,7 +7,7 @@ #include "system.hpp" /* clang-format off */ -Robot::Infantry::Param param = { +Robot::Sentry::Param param = { .chassis={ .type = Component::Mixer::MECANUM, @@ -29,14 +29,22 @@ Robot::Infantry::Param param = { }, Component::CMD::EventMapItem{ Device::DR16::DR16_SW_L_POS_TOP, - Module::RMChassis::SET_MODE_RELAX + Module::RMChassis::SET_MODE_RELAX, + }, + Component::CMD::EventMapItem{ + Device::DR16::DR16_SW_L_POS_MID,/* 模拟找到目标模式，云台绝对 */ + Module::RMChassis::SET_MODE_ROTOR, + }, + Component::CMD::EventMapItem{ + Device::DR16::DR16_SW_L_POS_BOT,/* 模拟未找到目标，巡逻模式 */ + Module::RMChassis::SET_MODE_ROTOR }, Component::CMD::EventMapItem{ - Device::DR16::DR16_SW_L_POS_MID, - Module::RMChassis::SET_MODE_INDENPENDENT + Device::AI::AI_FIND_TARGET, + Module::RMChassis::SET_MODE_ROTOR }, Component::CMD::EventMapItem{ - Device::DR16::DR16_SW_L_POS_BOT, + Device::AI::AI_AUTOPATROL, Module::RMChassis::SET_MODE_ROTOR } }, @@ -159,7 +167,7 @@ Robot::Infantry::Param param = { .yaw_actr = { .speed = { /* GIMBAL_CTRL_YAW_OMEGA_IDX */ - .k = 0.14f, + .k = 0.6f, .p = 1.f, .i = 3.f, .d = 0.f, @@ -171,10 +179,10 @@ Robot::Infantry::Param param = { .position = { /* GIMBAL_CTRL_YAW_ANGLE_IDX */ - .k = 20.0f, + .k = 10.0f, .p = 1.0f, .i = 0.0f, - .d = 0.0f, + .d = 1.4f, .i_limit = 0.0f, .out_limit = 10.0f, .d_cutoff_freq = -1.0f, @@ -203,7 +211,7 @@ Robot::Infantry::Param param = { .k = 20.0f, .p = 1.0f, .i = 0.0f, - .d = 0.0f, + .d = 0.8f, .i_limit = 0.0f, .out_limit = 10.0f, .d_cutoff_freq = -1.0f, @@ -216,14 +224,14 @@ Robot::Infantry::Param param = { }, .yaw_motor = { - .id_feedback = 0x209, - .id_control = GM6020_CTRL_ID_EXTAND, + .id_feedback = 0x206, + .id_control = GM6020_CTRL_ID_BASE, .model = Device::RMMotor::MOTOR_GM6020, .can = BSP_CAN_1, }, .pit_motor = { - .id_feedback = 0x20A, + .id_feedback = 0x209, .id_control = GM6020_CTRL_ID_EXTAND, .model = Device::RMMotor::MOTOR_GM6020, .can = BSP_CAN_2, @@ -234,10 +242,11 @@ Robot::Infantry::Param param = { .pit = 4.0f, .rol = 0.0f, }, + .patrol_rate = 0.25, .limit = { - .pitch_max = 3.8f, - .pitch_min = 3.0f, + .pitch_max = 0.57f, + .pitch_min = 0.23f, }, .EVENT_MAP = { @@ -250,25 +259,33 @@ Robot::Infantry::Param param = { Module::Gimbal::SET_MODE_ABSOLUTE }, Component::CMD::EventMapItem{ - Device::DR16::DR16_SW_R_POS_MID, + Device::DR16::DR16_SW_L_POS_MID,/* 模拟找到目标模式，云台绝对 */ Module::Gimbal::SET_MODE_ABSOLUTE }, Component::CMD::EventMapItem{ - Device::DR16::DR16_SW_R_POS_BOT, + Device::DR16::DR16_SW_L_POS_BOT,/* 模拟未找到目标，巡逻模式 */ + Module::Gimbal::SET_AUTOPATROL + }, + Component::CMD::EventMapItem{ + Device::AI::AI_FIND_TARGET, Module::Gimbal::SET_MODE_ABSOLUTE + }, + Component::CMD::EventMapItem{ + Device::AI::AI_AUTOPATROL, + Module::Gimbal::SET_AUTOPATROL } }, }, - .launcher = { + .launcher1 = { .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 = 30.f, + .default_bullet_speed = 15.f, .min_launch_delay = static_cast(1000.0f / 20.0f), .trig_actr = { @@ -302,7 +319,7 @@ Robot::Infantry::Param param = { }, .fric_actr = { - Component::SpeedActuator::Param{ + Component::SpeedActuator::Param{ .speed = { .k = 0.00015f, .p = 1.0f, @@ -338,7 +355,7 @@ Robot::Infantry::Param param = { .trig_motor = { Device::RMMotor::Param{ - .id_feedback = 0x207, + .id_feedback = 0x206, .id_control = M3508_M2006_CTRL_ID_EXTAND, .model = Device::RMMotor::MOTOR_M2006, .can = BSP_CAN_2, @@ -347,14 +364,154 @@ Robot::Infantry::Param param = { .fric_motor = { Device::RMMotor::Param{ - .id_feedback = 0x205, - .id_control = M3508_M2006_CTRL_ID_EXTAND, + .id_feedback = 0x204, + .id_control = M3508_M2006_CTRL_ID_BASE, .model = Device::RMMotor::MOTOR_M3508, .can = BSP_CAN_2, }, Device::RMMotor::Param{ - .id_feedback = 0x206, - .id_control = M3508_M2006_CTRL_ID_EXTAND, + .id_feedback = 0x203, + .id_control = M3508_M2006_CTRL_ID_BASE, + .model = Device::RMMotor::MOTOR_M3508, + .can = BSP_CAN_2, + } + }, + + .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::DR16::KEY_L_PRESS, + Module::Launcher::LAUNCHER_START_FIRE + }, + Component::CMD::EventMapItem{ + Device::AI::AI_DATA::AI_FIRE_COMMAND, + Module::Launcher::LAUNCHER_START_FIRE + }, + Component::CMD::EventMapItem{ + Device::AI::AI_DATA::AI_FIRE_COMMAND, + Module::Launcher::CHANGE_FIRE_MODE_LOADED + }, + Component::CMD::EventMapItem{ + Device::AI::AI_DATA::AI_FIRE_COMMAND, + Module::Launcher::CHANGE_TRIG_MODE_SINGLE + } + + }, + }, /* 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 = 1.0f, + .p = 1.0f, + .i = 0.0f, + .d = 0.03f, + .i_limit = 0.5f, + .out_limit = 0.5f, + .d_cutoff_freq = -1.0f, + .cycle = false, + }, + + .position = { + .k = 1.0f, + .p = 1.0f, + .i = 0.0f, + .d = 0.012f, + .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.00015f, + .p = 1.0f, + .i = 0.4f, + .d = 0.01f, + .i_limit = 0.5f, + .out_limit = 0.5f, + .d_cutoff_freq = -1.0f, + .cycle = false, + }, + + .in_cutoff_freq = -1.0f, + + .out_cutoff_freq = -1.0f, + }, + Component::SpeedActuator::Param{ + .speed = { + .k = 0.00015f, + .p = 1.0f, + .i = 0.4f, + .d = 0.01f, + .i_limit = 0.5f, + .out_limit = 0.5f, + .d_cutoff_freq = -1.0f, + .cycle = false, + }, + + .in_cutoff_freq = -1.0f, + + .out_cutoff_freq = -1.0f, + }, + }, + + .trig_motor = { + Device::RMMotor::Param{ + .id_feedback =0x207, + .id_control = M3508_M2006_CTRL_ID_EXTAND, + .model = Device::RMMotor::MOTOR_M2006, + .can = BSP_CAN_2, + .reverse = false, + }, + }, + + .fric_motor = { + Device::RMMotor::Param{ + .id_feedback = 0x202, + .id_control = M3508_M2006_CTRL_ID_BASE, + .model = Device::RMMotor::MOTOR_M3508, + .can = BSP_CAN_2, + }, + Device::RMMotor::Param{ + .id_feedback = 0x201, + .id_control = M3508_M2006_CTRL_ID_BASE, .model = Device::RMMotor::MOTOR_M3508, .can = BSP_CAN_2, }, @@ -370,24 +527,36 @@ Robot::Infantry::Param param = { Module::Launcher::CHANGE_FIRE_MODE_SAFE }, Component::CMD::EventMapItem{ - Device::DR16::DR16_SW_R_POS_MID, + Device::DR16::DR16_SW_R_POS_MID,/* 模拟找到目标模式，云台绝对 */ Module::Launcher::CHANGE_FIRE_MODE_LOADED }, Component::CMD::EventMapItem{ - Device::DR16::DR16_SW_R_POS_BOT, + Device::DR16::DR16_SW_R_POS_BOT,/* 模拟未找到目标，巡逻模式 */ Module::Launcher::CHANGE_FIRE_MODE_LOADED }, Component::CMD::EventMapItem{ - Device::DR16::DR16_SW_R_POS_BOT, + Device::DR16::DR16_SW_R_POS_BOT,/* 模拟未找到目标，巡逻模式 */ Module::Launcher::LAUNCHER_START_FIRE }, Component::CMD::EventMapItem{ Device::DR16::KEY_L_PRESS, Module::Launcher::LAUNCHER_START_FIRE + }, + Component::CMD::EventMapItem{ + Device::AI::AI_DATA::AI_FIRE_COMMAND, + Module::Launcher::LAUNCHER_START_FIRE + }, + Component::CMD::EventMapItem{ + Device::AI::AI_DATA::AI_FIRE_COMMAND, + Module::Launcher::CHANGE_FIRE_MODE_LOADED + }, + Component::CMD::EventMapItem{ + Device::AI::AI_DATA::AI_FIRE_COMMAND, + Module::Launcher::CHANGE_TRIG_MODE_SINGLE } + }, }, /* launcher */ - .bmi088_rot = { .rot_mat = { { +1, +0, +0}, @@ -405,5 +574,5 @@ Robot::Infantry::Param param = { /* clang-format on */ void robot_init() { - System::Start(param, 500.0f); + System::Start(param, 500.0f); } diff --git a/src/robot/sentry/robot.hpp b/src/robot/sentry/robot.hpp index 2c8444ce..29b5d8a6 100644 --- a/src/robot/sentry/robot.hpp +++ b/src/robot/sentry/robot.hpp @@ -13,12 +13,13 @@ void robot_init(); namespace Robot { -class Infantry { +class Sentry { public: typedef struct Param { Module::RMChassis::Param chassis; Module::Gimbal::Param gimbal; - Module::Launcher::Param launcher; + Module::Launcher::Param launcher1; + Module::Launcher::Param launcher2; Device::BMI088::Rotation bmi088_rot{}; Device::Cap::Param cap{}; } Param; @@ -36,14 +37,15 @@ class Infantry { Module::RMChassis chassis_; Module::Gimbal gimbal_; - Module::Launcher launcher_; - - Infantry(Param& param, float control_freq) + Module::Launcher launcher1_; + Module::Launcher launcher2_; + Sentry(Param& param, float control_freq) : cmd_(Component::CMD::CMD_AUTO_CTRL), bmi088_(param.bmi088_rot), cap_(param.cap), chassis_(param.chassis, control_freq), gimbal_(param.gimbal, control_freq), - launcher_(param.launcher, control_freq) {} + launcher1_(param.launcher1, control_freq), + launcher2_(param.launcher2, control_freq) {} }; } // namespace Robot -- Gitee From 6b9f789c1c9e71fc2fb28526224269f080d177a2 Mon Sep 17 00:00:00 2001 From: Marine_zxy <1423266892@qq.com> Date: Thu, 21 Mar 2024 22:51:15 +0800 Subject: [PATCH 02/10] =?UTF-8?q?=E6=9B=B4=E6=96=B0=E8=A3=81=E5=88=A4?= =?UTF-8?q?=E7=B3=BB=E7=BB=9F=EF=BC=8C=E5=8D=8F=E8=AE=AE1.61?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/device/ai/dev_ai.cpp | 55 +++-- src/device/ai/dev_ai.hpp | 9 +- src/device/referee/dev_referee.cpp | 27 ++- src/device/referee/dev_referee.hpp | 351 +++++++++++++++++------------ 4 files changed, 256 insertions(+), 186 deletions(-) diff --git a/src/device/ai/dev_ai.cpp b/src/device/ai/dev_ai.cpp index 82992c48..72ab28b3 100644 --- a/src/device/ai/dev_ai.cpp +++ b/src/device/ai/dev_ai.cpp @@ -135,7 +135,7 @@ bool AI::PackRef() { return true; } -bool AI::PackCMD() { /* 将原始数据进行copy转移 */ +bool AI::PackCMD() { this->cmd_.gimbal.mode = Component::CMD::GIMBAL_ABSOLUTE_CTRL; memcpy(&(this->cmd_.gimbal.eulr), &(this->from_host_.data.gimbal), @@ -147,33 +147,39 @@ bool AI::PackCMD() { /* 将原始数据进行copy转移 */ memcpy(&(this->cmd_.chassis), &(this->from_host_.data.chassis_move_vec), sizeof(this->from_host_.data.chassis_move_vec)); - // memcpy(&(this->fire_command_), &(this->from_host_.data.notice), - // sizeof(this->from_host_.data.notice)); - - /* 判断是否为有效数据,即是否看到目标 */ - if (fabs(this->cmd_.gimbal.last_eulr.pit - this->cmd_.gimbal.last_eulr.pit) < - 0.01 && - fabs(this->cmd_.gimbal.last_eulr.yaw - this->cmd_.gimbal.last_eulr.yaw) < - 0.01) { - this->ai_data_ = this->IS_INVALID_DATA; + + /* AI在线的情况下，根据AI的数据来判定此时的状态 */ + /* 因为如果收不到AI的数据，那么from_host的值就不会更新 */ + if (this->cmd_.online) { + if (this->cmd_.gimbal.last_eulr.yaw == this->cmd_.gimbal.eulr.yaw && + this->cmd_.gimbal.last_eulr.pit == this->cmd_.gimbal.eulr.pit) { + this->ai_data_status_ = this->IS_INVALID_DATA; + } else { + this->ai_data_status_ = this->IS_USEFUL_DATA; + } } else { - this->ai_data_ = this->IS_USEFUL_DATA; + this->ai_data_status_ = this->AI_OFFLINE; + } + + /* AI离线，底盘RELEX模式 */ + if (!this->cmd_.online) { + this->event_.Active(AI_OFFLINE); } /*AI数据无效，云台采用巡逻模式 */ - // if (this->ai_data_ == this->IS_INVALID_DATA) { - // this->event_.Active(AI_AUTOPATROL); - // this->fire_command_ = 0; - // } + if (this->ai_data_status_ == this->IS_INVALID_DATA) { + this->event_.Active(AI_AUTOPATROL); + this->fire_command_ = 0; + } /*AI数据有效，采用自动瞄准模式，也就是云台ABSOLUTE模式 */ - // if (this->ai_data_ == IS_USEFUL_DATA) { - // this->event_.Active(AI_FIND_TARGET); - // this->fire_command_ = this->from_host_.data.notice; /* 0或者2 */ - // } + if (this->ai_data_status_ == IS_USEFUL_DATA) { + this->event_.Active(AI_FIND_TARGET); + this->fire_command_ = this->from_host_.data.notice; + } /* AI开火发弹指令 */ - if (this->ai_data_ == IS_USEFUL_DATA && this->fire_command_ != 0) { + if (this->ai_data_status_ == IS_USEFUL_DATA && this->fire_command_ != 0) { this->event_.Active(AI_FIRE_COMMAND); } @@ -189,8 +195,8 @@ void AI::PraseRef() { #if RB_HERO this->ref_.ball_speed = this->ref_.data_.robot_status.launcher_42_speed_limit; #else - this->ref_.ball_speed = - this->raw_ref_.robot_status.launcher_id1_17_speed_limit; + // this->ref_.ball_speed = + // this->raw_ref_.robot_status.launcher_id1_17_speed_limit; #endif this->ref_.max_hp = this->raw_ref_.robot_status.max_hp; @@ -204,9 +210,10 @@ void AI::PraseRef() { } this->ref_.status = this->raw_ref_.status; - if (this->raw_ref_.rfid.high_ground == 1) { + 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.energy_mech == 1) { + } else if (this->raw_ref_.rfid.own_energy_mech_activation == 1) { this->ref_.robot_buff |= AI_RFID_BUFF; } else { this->ref_.robot_buff = 0; diff --git a/src/device/ai/dev_ai.hpp b/src/device/ai/dev_ai.hpp index 39107bca..6b1ad201 100644 --- a/src/device/ai/dev_ai.hpp +++ b/src/device/ai/dev_ai.hpp @@ -32,11 +32,12 @@ class AI { } RefForAI; typedef enum { - AI_FIRE_COMMAND = 128, + AI_OFFLINE = 128, + IS_INVALID_DATA, + IS_USEFUL_DATA, AI_FIND_TARGET, AI_AUTOPATROL, - IS_INVALID_DATA, - IS_USEFUL_DATA + AI_FIRE_COMMAND, } AI_DATA; /* 这个变量如何跟notice建立联系 */ AI(); @@ -63,7 +64,7 @@ class AI { Protocol_DownPackage_t from_host_{}; /* 从ai拿到的原始数据数组 */ - uint8_t ai_data_; + uint8_t ai_data_status_; uint8_t fire_command_; diff --git a/src/device/referee/dev_referee.cpp b/src/device/referee/dev_referee.cpp index e7d2125f..2904834b 100644 --- a/src/device/referee/dev_referee.cpp +++ b/src/device/referee/dev_referee.cpp @@ -166,14 +166,6 @@ void Referee::Prase() { destination = &(this->ref_data_.game_robot_hp); size = sizeof(this->ref_data_.game_robot_hp); break; - case REF_CMD_ID_DART_STATUS: - destination = &(this->ref_data_.dart_status); - size = sizeof(this->ref_data_.dart_status); - break; - case REF_CMD_ID_ICRA_ZONE_STATUS: - destination = &(this->ref_data_.icra_zone); - size = sizeof(this->ref_data_.icra_zone); - break; case REF_CMD_ID_FIELD_EVENTS: destination = &(this->ref_data_.field_event); size = sizeof(this->ref_data_.field_event); @@ -238,6 +230,18 @@ void Referee::Prase() { destination = &(this->ref_data_.radar_mark_progress); size = sizeof(this->ref_data_.radar_mark_progress); break; + case REF_CMD_ID_SENTRY_DECISION: + destination = &(this->ref_data_.sentry_decision); + size = sizeof(this->ref_data_.sentry_decision); + break; + case REF_CMD_ID_RADAR_DECISION: + destination = &(this->ref_data_.radar_decision); + size = sizeof(this->ref_data_.radar_decision); + break; + case REF_CMD_ID_INTER_STUDENT: + destination = &(this->ref_data_.robot_ineraction_data); + size = sizeof(this->ref_data_.robot_ineraction_data); + break; case REF_CMD_ID_INTER_STUDENT_CUSTOM: destination = &(this->ref_data_.custom_controller); size = sizeof(this->ref_data_.custom_controller); @@ -258,6 +262,10 @@ void Referee::Prase() { destination = &(this->ref_data_.sentry_postion); size = sizeof(this->ref_data_.sentry_postion); break; + case REF_CMD_ID_ROBOT_POS_DATA: + destination = &(this->ref_data_.robot_position); + size = sizeof(this->ref_data_.robot_position); + break; default: return; @@ -290,7 +298,8 @@ void Referee::Prase() { #endif this->ref_data_.power_heat.launcher_id1_17_heat = REF_HEAT_LIMIT_17; this->ref_data_.power_heat.launcher_42_heat = REF_HEAT_LIMIT_42; - this->ref_data_.robot_status.launcher_id1_17_speed_limit = REF_LAUNCH_SPEED; + // this->ref_data_.robot_status.launcher_id1_17_speed_limit = + // REF_LAUNCH_SPEED; this->ref_data_.robot_status.launcher_42_speed_limit = REF_LAUNCH_SPEED; this->ref_data_.robot_status.chassis_power_limit = REF_POWER_LIMIT; this->ref_data_.power_heat.chassis_pwr_buff = REF_POWER_BUFF; diff --git a/src/device/referee/dev_referee.hpp b/src/device/referee/dev_referee.hpp index 41e75180..bb6b190a 100644 --- a/src/device/referee/dev_referee.hpp +++ b/src/device/referee/dev_referee.hpp @@ -1,5 +1,6 @@ /* 裁判系统抽象。 + 2024_03_21 已修改。 */ #pragma once @@ -50,11 +51,10 @@ class Referee { } Status; typedef enum { + /* DRONE空中机器人,DART飞镖,RADAR雷达 */ REF_CMD_ID_GAME_STATUS = 0x0001, REF_CMD_ID_GAME_RESULT = 0x0002, REF_CMD_ID_GAME_ROBOT_HP = 0x0003, - REF_CMD_ID_DART_STATUS = 0x0004, - REF_CMD_ID_ICRA_ZONE_STATUS = 0x0005, REF_CMD_ID_FIELD_EVENTS = 0x0101, REF_CMD_ID_SUPPLY_ACTION = 0x0102, REF_CMD_ID_WARNING = 0x0104, @@ -71,12 +71,16 @@ 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_INTER_STUDENT = 0x0301, REF_CMD_ID_INTER_STUDENT_CUSTOM = 0x0302, REF_CMD_ID_CLIENT_MAP = 0x0303, REF_CMD_ID_KEYBOARD_MOUSE = 0x0304, + // 0x0305 REF_CMD_ID_CUSTOM_KEYBOARD_MOUSE = 0X0306, REF_CMD_ID_SENTRY_POS_DATA = 0x0307, + REF_CMD_ID_ROBOT_POS_DATA = 0x0308, /* 选手端小地图接受机器人消息 */ } CommandID; typedef enum { @@ -98,11 +102,11 @@ class Referee { uint8_t game_progress : 4; uint16_t stage_remain_time; uint64_t sync_time_stamp; - } GameStatus; + } GameStatus; /* 0x0001 */ typedef struct __attribute__((packed)) { uint8_t winner; - } GameResult; + } GameResult; /* 0x0002 */ typedef struct __attribute__((packed)) { uint16_t red_1; @@ -110,7 +114,6 @@ class Referee { uint16_t red_3; uint16_t red_4; uint16_t red_5; - uint16_t red_6; uint16_t red_7; uint16_t red_outpose; uint16_t red_base; @@ -119,78 +122,85 @@ class Referee { uint16_t blue_3; uint16_t blue_4; uint16_t blue_5; - uint16_t blue_6; uint16_t blue_7; uint16_t blue_outpose; uint16_t blue_base; - } RobotHP; - - typedef struct __attribute__((packed)) { - uint8_t dart_belong; - uint16_t stage_remain_time; - } DartStatus; - - typedef struct __attribute__((packed)) { - uint8_t f1_status : 1; - uint8_t f1_buff_status : 3; - uint8_t f2_status : 1; - uint8_t f2_buff_status : 3; - uint8_t f3_status : 1; - uint8_t f3_buff_status : 3; - uint8_t f4_status : 1; - uint8_t f4_buff_status : 3; - uint8_t f5_status : 1; - uint8_t f5_buff_status : 3; - uint8_t f6_status : 1; - uint8_t f6_buff_status : 3; - uint16_t red1_bullet_remain; - uint16_t red2_bullet_remain; - uint16_t blue1_bullet_remain; - uint16_t blue2_bullet_remain; - } IcraZoneStatus; - - typedef struct __attribute__((packed)) { - uint8_t copter_pad : 2; - uint8_t energy_mech : 2; - uint8_t virtual_shield : 1; - uint32_t res : 27; - } FieldEvents; - - typedef struct __attribute__((packed)) { - uint8_t supply_id; - uint8_t robot_id; + } RobotHP; /* 0x0003 */ + /* + typedef struct __attribute__((packed)) { + uint8_t dart_belong; + uint16_t stage_remain_time; + } DartStatus;//0x0105已经有代替 + + typedef struct __attribute__((packed)) { + uint8_t f1_status : 1; + uint8_t f1_buff_status : 3; + uint8_t f2_status : 1; + uint8_t f2_buff_status : 3; + uint8_t f3_status : 1; + uint8_t f3_buff_status : 3; + uint8_t f4_status : 1; + uint8_t f4_buff_status : 3; + uint8_t f5_status : 1; + uint8_t f5_buff_status : 3; + uint8_t f6_status : 1; + uint8_t f6_buff_status : 3; + uint16_t red1_bullet_remain; + uint16_t red2_bullet_remain; + uint16_t blue1_bullet_remain; + uint16_t blue2_bullet_remain; + } IcraZoneStatus; + */ + typedef struct __attribute__((packed)) { + 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_small_status : 1; + uint32_t energy_mech_big_status : 1; + uint32_t highland_annular : 2; + + uint32_t highland_trapezium_1 : 2; + uint32_t highland_trapezium_2 : 2; + uint32_t virtual_shield_value : 8; + uint32_t last_hit_time : 9; + uint32_t last_hit_target : 2; + uint32_t res : 1; + } FieldEvents; /* 0x0101 */ + typedef struct __attribute__((packed)) { + uint8_t res; + uint8_t supply_robot_id; uint8_t supply_step; uint8_t supply_sum; - } SupplyAction; + } SupplyAction; /* 0x0102 */ typedef struct __attribute__((packed)) { uint8_t level; uint8_t robot_id; - } Warning; + uint8_t count; + } Warning; /* 0x0104 */ typedef struct __attribute__((packed)) { uint8_t countdown; - } DartCountdown; + + uint16_t dart_last_target : 2; + uint16_t attack_count : 3; + uint16_t dart_target : 2; + uint16_t res : 9; + } DartCountdown; /* 0x0105 */ typedef struct __attribute__((packed)) { uint8_t robot_id; uint8_t robot_level; uint16_t remain_hp; uint16_t max_hp; - uint16_t launcher_id1_17_cooling_rate; - uint16_t launcher_id1_17_heat_limit; - uint16_t launcher_id1_17_speed_limit; - uint16_t launcher_id2_17_cooling_rate; - uint16_t launcher_id2_17_heat_limit; - uint16_t launcher_id2_17_speed_limit; - uint16_t launcher_42_cooling_rate; - uint16_t launcher_42_heat_limit; - uint16_t launcher_42_speed_limit; + uint16_t shooter_cooling_value; + uint16_t shooter_heat_limit; uint16_t chassis_power_limit; uint8_t power_gimbal_output : 1; uint8_t power_chassis_output : 1; uint8_t power_launcher_output : 1; - } RobotStatus; + } RobotStatus; /* 0x0201 */ typedef struct __attribute__((packed)) { uint16_t chassis_volt; @@ -200,96 +210,74 @@ class Referee { uint16_t launcher_id1_17_heat; uint16_t launcher_id2_17_heat; uint16_t launcher_42_heat; - } PowerHeat; + } PowerHeat; /* 0x0202 */ typedef struct __attribute__((packed)) { float x; float y; - float z; - float yaw; - } RobotPOS; + float angle; + } RobotPOS; /* 0x0203 */ typedef struct __attribute__((packed)) { - uint8_t healing : 1; - uint8_t cooling_acc : 1; - uint8_t defense_buff : 1; - uint8_t attack_buff : 1; - uint8_t res : 4; - } RobotBuff; + uint8_t healing_buff; + uint8_t cooling_acc; + uint8_t defense_buff; + uint8_t vulnerability_buff; + uint16_t attack_buff; + } RobotBuff; /* 0x0204 */ typedef struct __attribute__((packed)) { + uint8_t status; uint8_t attack_countdown; - } DroneEnergy; + } DroneEnergy; /* 0x0205 */ typedef struct __attribute__((packed)) { uint8_t armor_id : 4; uint8_t damage_type : 4; - } RobotDamage; + } RobotDamage; /* 0x0206 */ typedef struct __attribute__((packed)) { uint8_t bullet_type; uint8_t launcherer_id; uint8_t bullet_freq; float bullet_speed; - } LauncherData; + } LauncherData; /* 0x0207 */ typedef struct __attribute__((packed)) { uint16_t bullet_17_remain; uint16_t bullet_42_remain; uint16_t coin_remain; - } BulletRemain; - - typedef struct __attribute__((packed)) { - uint8_t base : 1; - uint8_t high_ground : 1; - uint8_t energy_mech : 1; - uint8_t slope : 1; - uint8_t outpose : 1; - uint8_t resource : 1; - uint8_t healing_card : 1; - uint32_t res : 24; - } RFID; - - typedef struct __attribute__((packed)) { - uint8_t opening; + } BulletRemain; /* 0x0208 */ + + 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_resource : 1; + uint32_t enemy_resource : 1; + uint32_t own_exchange_area : 1; + uint32_t res : 13; + } RFID; /* 0x0209 */ + + typedef struct __attribute__((packed)) { + uint8_t opening_status; uint8_t target; - uint8_t target_changable_countdown; - uint8_t dart1_speed; - uint8_t dart2_speed; - uint8_t dart3_speed; - uint8_t dart4_speed; - uint16_t last_dart_launch_time; + uint16_t target_changable_countdown; uint16_t operator_cmd_launch_time; - } DartClient; - - typedef struct __attribute__((packed)) { - float position_x; - float position_y; - float position_z; - uint8_t commd_keyboard; - uint16_t robot_id; - } ClientMap; - - typedef struct __attribute__((packed)) { - int16_t mouse_x; - int16_t mouse_y; - int16_t mouse_wheel; - int8_t button_l; - int8_t button_r; - uint16_t keyboard_value; - uint16_t res; - } KeyboardMouse; - typedef struct __attribute__((packed)) { - uint8_t intention; - uint16_t start_position_x; - uint16_t start_position_y; - std::array delta_x; - std::array delta_y; - } SentryPosition; - - typedef struct __attribute__((packed)) { - std::array data; - } CustomController; + } DartClient; /* 0x020A */ typedef struct __attribute__((packed)) { float hero_x; float hero_y; @@ -301,7 +289,7 @@ class Referee { float standard_4_y; float standard_5_x; float standard_5_y; - } RobotPosForSentry; + } RobotPosForSentry; /* 0x020B */ typedef struct __attribute__((packed)) { uint8_t mark_hero_progress; uint8_t mark_engineer_progress; @@ -309,15 +297,75 @@ class Referee { uint8_t mark_standard_4_progress; uint8_t mark_standard_5_progress; uint8_t mark_sentry_progress; - } RadarMarkProgress; + } RadarMarkProgress; /* 0x020C */ + typedef struct __attribute__((packed)) { + uint32_t exchanged_bullet_num : 11; + uint32_t exchanged_bullet_times : 4; + uint32_t exchanged_blood_times : 4; + uint32_t res : 13; + } SentryInfo; /* 0x020D */ + typedef struct __attribute__((packed)) { + uint8_t qualification : 2; + uint8_t enemy_status : 1; + uint8_t res : 5; + } RadarInfo; /* 0x020E */ + typedef struct __attribute__((packed)) { + uint16_t data_cmd_id; + uint16_t sender_id; + uint16_t receiver_id; + std::array user_data; + /*最大值113*/ + } RobotInteractionData; /* 0x0301 */ + + typedef struct __attribute__((packed)) { + std::array data; + } CustomController; /* 0x0302 */ + + typedef struct __attribute__((packed)) { + float position_x; + float position_y; + uint8_t commd_keyboard; + uint8_t robot_id; + uint8_t cmd_source; + } ClientMap; /* 0x0303 */ + + typedef struct __attribute__((packed)) { + int16_t mouse_x; + int16_t mouse_y; + int16_t mouse_wheel; + int8_t button_l; + int8_t button_r; + uint16_t keyboard_value; + uint16_t res; + } KeyboardMouse; /* 0x0304 */ + typedef struct __attribute__((packed)) { + uint16_t target_robot_id; + float target_position_x; + float target_position_y; + } MapRobotData; /* 0x0305 */ + typedef struct __attribute__((packed)) { uint16_t key_value; uint16_t x_position : 12; uint16_t mouse_left : 4; uint16_t y_position : 12; uint16_t mouse_right : 4; - uint16_t reserved; - } CustomKeyMouseData; + uint16_t res; + } CustomKeyMouseData; /* 0x0306 */ + + typedef struct __attribute__((packed)) { + uint8_t intention; + uint16_t start_position_x; + uint16_t start_position_y; + std::array delta_x; + std::array delta_y; + uint16_t sender_id; + } SentryPosition; /* 0x0307 */ + typedef struct __attribute__((packed)) { + uint16_t sender_id; + uint16_t receiver_id; + std::array user_data; + } RobotPosition; /* 0x0308 */ typedef uint16_t Tail; @@ -363,6 +411,7 @@ class Referee { REF_STDNT_CMD_ID_UI_DRAW7 = 0x0104, REF_STDNT_CMD_ID_UI_STR = 0x0110, REF_STDNT_CMD_ID_CUSTOM = 0x0200, + // 新的 0x0120 0x0121 未写入 } CMDID; typedef struct __attribute__((packed)) { @@ -373,32 +422,36 @@ class Referee { typedef struct { Status status; - GameStatus game_status; - GameResult game_result; - RobotHP game_robot_hp; - DartStatus dart_status; - IcraZoneStatus icra_zone; - FieldEvents field_event; - SupplyAction supply_action; - Warning warning; - DartCountdown dart_countdown; - RobotStatus robot_status; - PowerHeat power_heat; - RobotPOS robot_pos; - RobotBuff robot_buff; - DroneEnergy drone_energy; - RobotDamage robot_damage; - LauncherData launcher_data; - BulletRemain bullet_remain; - RFID rfid; - DartClient dart_client; - ClientMap client_map; - KeyboardMouse keyboard_mouse; - SentryPosition sentry_postion; - CustomController custom_controller; - RobotPosForSentry robot_pos_for_snetry; - RadarMarkProgress radar_mark_progress; - CustomKeyMouseData custom_key_mouse_data; + GameStatus game_status; /* 0x0001 */ + GameResult game_result; /* 0x0002 */ + RobotHP game_robot_hp; /* 0x0003 */ + // DartStatus dart_status; + // IcraZoneStatus icra_zone; + FieldEvents field_event; /* 0x0101 */ + SupplyAction supply_action; /* 0x0102 */ + Warning warning; /* 0x0104 */ + DartCountdown dart_countdown; /* 0x0105 */ + RobotStatus robot_status; /* 0x0201 */ + PowerHeat power_heat; /* 0x0202 */ + RobotPOS robot_pos; /* 0x0203 */ + RobotBuff robot_buff; /* 0x0204 */ + DroneEnergy drone_energy; /* 0x0205 */ + RobotDamage robot_damage; /* 0x0206 */ + LauncherData launcher_data; /* 0x0207 */ + BulletRemain bullet_remain; /* 0x0208 */ + RFID rfid; /* 0x0209 */ + DartClient dart_client; /* 0x020A */ + ClientMap client_map; /* 0x0303 */ + KeyboardMouse keyboard_mouse; /* 0x0304 */ + SentryPosition sentry_postion; /* 0x0307 */ + RobotPosition robot_position; /* 0x0308 */ + CustomController custom_controller; /* 0x0302 */ + RobotPosForSentry robot_pos_for_snetry; /* 0x020B */ + RadarMarkProgress radar_mark_progress; /* 0x020C */ + SentryInfo sentry_decision; /* 0x020D */ + RadarInfo radar_decision; /* 0x020E */ + RobotInteractionData robot_ineraction_data; /* 0x0301 */ + CustomKeyMouseData custom_key_mouse_data; /* 0x0306 */ } Data; typedef struct __attribute__((packed)) { -- Gitee From b006b11bcad8335992b00381067479492c9c84c4 Mon Sep 17 00:00:00 2001 From: Marine_zxy <1423266892@qq.com> Date: Thu, 21 Mar 2024 22:52:03 +0800 Subject: [PATCH 03/10] =?UTF-8?q?=E5=93=A8=E5=85=B5ai=E5=AE=8C=E5=96=84?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/component/comp_cmd.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/component/comp_cmd.hpp b/src/component/comp_cmd.hpp index c43fda26..53f98143 100644 --- a/src/component/comp_cmd.hpp +++ b/src/component/comp_cmd.hpp @@ -29,7 +29,7 @@ class CMD { float yaw; /* 偏航角（Yaw angle） */ float pit; /* 俯仰角（Pitch angle） */ float rol; /* 翻滚角（Roll angle） */ - } eulr; + } eulr, last_eulr; GimbalCommandMode mode; } GimbalCMD; @@ -100,6 +100,7 @@ class CMD { static void SetCtrlSource(ControlSource source) { self_->ctrl_source_ = source; } + static uint8_t GetCtrlMode() { return self_->mode_; } private: bool online_ = false; -- Gitee From 23155ceb0ef87b4184e9c0458981bfab61a8603b Mon Sep 17 00:00:00 2001 From: Marine_zxy <1423266892@qq.com> Date: Thu, 21 Mar 2024 22:54:17 +0800 Subject: [PATCH 04/10] =?UTF-8?q?=E8=A3=81=E5=88=A4=E7=B3=BB=E7=BB=9F?= =?UTF-8?q?=E6=9B=B4=E6=96=B0=EF=BC=8C=E5=8D=8F=E8=AE=AE1.61?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/module/launcher/mod_launcher.cpp | 61 +++++++++++----------------- src/module/launcher/mod_launcher.hpp | 16 +------- 2 files changed, 26 insertions(+), 51 deletions(-) diff --git a/src/module/launcher/mod_launcher.cpp b/src/module/launcher/mod_launcher.cpp index 6d10de4e..489595e1 100644 --- a/src/module/launcher/mod_launcher.cpp +++ b/src/module/launcher/mod_launcher.cpp @@ -1,14 +1,3 @@ -/** - * @file launcher.c - * @author Qu Shen (503578404@qq.com) - * @brief 弹丸发射器模块 - * @version 1.0.0 - * @date 2021-05-04 - * - * @copyright Copyright (c) 2021 - * - */ - #include "mod_launcher.hpp" #include "bsp_pwm.h" @@ -40,13 +29,18 @@ Launcher::Launcher(Param& param, float control_freq) auto event_callback = [](LauncherEvent event, Launcher* launcher) { launcher->ctrl_lock_.Wait(UINT32_MAX); - - switch (event) { + switch (event) { /* 根据event设置模式 */ case CHANGE_FIRE_MODE_RELAX: case CHANGE_FIRE_MODE_SAFE: case CHANGE_FIRE_MODE_LOADED: launcher->SetFireMode(static_cast(event)); break; + case LAUNCHER_START_FIRE: /* 摩擦轮开启条件下，开火控制fire为ture */ + if (launcher->fire_ctrl_.fire_mode_ == LOADED) { + launcher->fire_ctrl_.fire = true; + } + break; + case CHANGE_TRIG_MODE_SINGLE: case CHANGE_TRIG_MODE_BURST: launcher->SetTrigMode(static_cast(event)); @@ -55,11 +49,6 @@ Launcher::Launcher(Param& param, float control_freq) launcher->SetTrigMode(static_cast( (launcher->fire_ctrl_.trig_mode_ + 1) % CONTINUED)); break; - case LAUNCHER_START_FIRE: - if (launcher->fire_ctrl_.fire_mode_ == LOADED) { - launcher->fire_ctrl_.fire = true; - } - break; case OPEN_COVER: launcher->cover_mode_ = OPEN; break; @@ -101,8 +90,7 @@ Launcher::Launcher(Param& param, float control_freq) } }; - this->thread_.Create(launcher_thread, this, "launcher_thread", - MODULE_LAUNCHER_TASK_STACK_DEPTH, + this->thread_.Create(launcher_thread, this, "launcher_thread", 512, System::Thread::MEDIUM); System::Timer::Create(this->DrawUIStatic, this, 2200); @@ -146,7 +134,7 @@ void Launcher::Control() { max_burst = 1; break; } - + /* 发弹量设置 */ switch (this->fire_ctrl_.trig_mode_) { case SINGLE: /* 点射开火模式 */ case BURST: /* 爆发开火模式 */ @@ -224,7 +212,7 @@ void Launcher::Control() { this->fire_ctrl_.last_launch = bsp_time_get_ms(); } } - + /* 计算摩擦轮和拨弹盘并输出 */ switch (this->fire_ctrl_.fire_mode_) { case RELAX: for (size_t i = 0; i < LAUNCHER_ACTR_TRIG_NUM; i++) { @@ -268,7 +256,8 @@ void Launcher::Control() { break; } } - +/* 拨弹盘模式 */ +/* SINGLE,BURST,CONTINUED, */ void Launcher::SetTrigMode(TrigMode mode) { if (mode == this->fire_ctrl_.trig_mode_) { return; @@ -276,9 +265,10 @@ void Launcher::SetTrigMode(TrigMode mode) { this->fire_ctrl_.trig_mode_ = mode; } - +/* 设置摩擦轮模式 */ +/* RELEX SAFE LOADED三种模式可以选择 */ void Launcher::SetFireMode(FireMode mode) { - if (mode == this->fire_ctrl_.fire_mode_) { + if (mode == this->fire_ctrl_.fire_mode_) { /* 未更改，return */ return; } @@ -286,7 +276,7 @@ void Launcher::SetFireMode(FireMode mode) { for (size_t i = 0; i < LAUNCHER_ACTR_FRIC_NUM; i++) { this->fric_actuator_[i]->Reset(); - } + } /* reset 所有电机执行器PID等参数 */ if (mode == LOADED) { this->fire_ctrl_.to_launch = 0; @@ -300,24 +290,21 @@ void Launcher::HeatLimit() { /* 根据机器人型号获得对应数据 */ if (this->param_.model == LAUNCHER_MODEL_42MM) { this->heat_ctrl_.heat = this->ref_.power_heat.launcher_42_heat; - this->heat_ctrl_.heat_limit = - this->ref_.robot_status.launcher_42_heat_limit; - this->heat_ctrl_.speed_limit = - this->ref_.robot_status.launcher_42_speed_limit; + this->heat_ctrl_.heat_limit = this->ref_.robot_status.shooter_heat_limit; + this->heat_ctrl_.speed_limit = 16; this->heat_ctrl_.cooling_rate = - this->ref_.robot_status.launcher_42_cooling_rate; + this->ref_.robot_status.shooter_cooling_value; this->heat_ctrl_.heat_increase = GAME_HEAT_INCREASE_42MM; } else if (this->param_.model == LAUNCHER_MODEL_17MM) { this->heat_ctrl_.heat = this->ref_.power_heat.launcher_id1_17_heat; - this->heat_ctrl_.heat_limit = - this->ref_.robot_status.launcher_id1_17_heat_limit; - this->heat_ctrl_.speed_limit = - this->ref_.robot_status.launcher_id1_17_speed_limit; + this->heat_ctrl_.heat_limit = this->ref_.robot_status.shooter_heat_limit; + this->heat_ctrl_.speed_limit = 30; this->heat_ctrl_.cooling_rate = - this->ref_.robot_status.launcher_id1_17_cooling_rate; + this->ref_.robot_status.shooter_cooling_value; this->heat_ctrl_.heat_increase = GAME_HEAT_INCREASE_17MM; } /* 检测热量更新后,计算可发射弹丸 */ + if ((this->heat_ctrl_.heat != this->heat_ctrl_.last_heat) || this->heat_ctrl_.available_shot == 0 || (this->heat_ctrl_.heat == 0)) { this->heat_ctrl_.available_shot = static_cast( @@ -343,7 +330,7 @@ void Launcher::PraseRef() { this->ref_.status = this->raw_ref_.status; } -float Launcher::LimitLauncherFreq() { +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; diff --git a/src/module/launcher/mod_launcher.hpp b/src/module/launcher/mod_launcher.hpp index 9f8a1230..8c2ec2fe 100644 --- a/src/module/launcher/mod_launcher.hpp +++ b/src/module/launcher/mod_launcher.hpp @@ -1,14 +1,3 @@ -/** - * @file launcher.h - * @author Qu Shen (503578404@qq.com) - * @brief 弹丸发射器模块 - * @version 1.0.0 - * @date 2021-05-04 - * - * @copyright Copyright (c) 2021 - * - */ - #pragma once #include @@ -48,7 +37,7 @@ class Launcher { CHANGE_TRIG_MODE, OPEN_COVER, CLOSE_COVER, - LAUNCHER_START_FIRE, + LAUNCHER_START_FIRE, /* 开火，拨弹盘开始发弹 */ } LauncherEvent; enum { @@ -66,7 +55,6 @@ class Launcher { LAUNCHER_CTRL_FRIC1_SPEED_IDX = 0, /* 摩擦轮1控制的速度环控制器的索引值 */ LAUNCHER_CTRL_FRIC2_SPEED_IDX, /* 摩擦轮2控制的速度环控制器的索引值 */ LAUNCHER_CTRL_TRIG_SPEED_IDX, /* 拨弹电机控制的速度环控制器的索引值 */ - LAUNCHER_CTRL_TRIG_ANGLE_IDX, /* 拨弹电机控制的角度环控制器的索引值 */ LAUNCHER_CTRL_NUM, /* 总共的控制器数量 */ }; @@ -91,7 +79,7 @@ class Launcher { std::array trig_motor; std::array fric_motor; - const std::vector EVENT_MAP; + std::vector EVENT_MAP; } Param; /* 热量控制 */ -- Gitee From 147782a61f5c9f028ca65825e57d3f4d1929bac7 Mon Sep 17 00:00:00 2001 From: Marine_zxy <1423266892@qq.com> Date: Thu, 21 Mar 2024 22:54:58 +0800 Subject: [PATCH 05/10] =?UTF-8?q?=E5=93=A8=E5=85=B5ai=E5=AE=8C=E5=96=84?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/module/gimbal/mod_gimbal.cpp | 27 +++++++++++++++------------ src/module/gimbal/mod_gimbal.hpp | 9 ++++----- 2 files changed, 19 insertions(+), 17 deletions(-) diff --git a/src/module/gimbal/mod_gimbal.cpp b/src/module/gimbal/mod_gimbal.cpp index 009e83eb..d4da1147 100644 --- a/src/module/gimbal/mod_gimbal.cpp +++ b/src/module/gimbal/mod_gimbal.cpp @@ -85,8 +85,12 @@ void Gimbal::UpdateFeedback() { case RELAX: case ABSOLUTE: this->yaw_ = this->yaw_motor_.GetAngle() - this->param_.mech_zero.yaw; + break; case AUTOPATROL: - this->yaw_ = this->yaw_virtual_; + this->yaw_ = this->yaw_motor_.GetAngle() - this->param_.mech_zero.yaw - + this->param_.patrol_range * + sin(this->param_.patrol_omega * bsp_time_get_ms()); + break; } } @@ -150,14 +154,14 @@ void Gimbal::Control() { case AUTOPATROL: /* 以sin变化左右摆头 */ - - this->t_ = this->t_ + 0.01; - this->setpoint_.eulr_.yaw = - this->eulr_.yaw + this->param_.patrol_rate * sin(this->t_); - - yaw_out = this->yaw_actuator_.Calculate( - this->setpoint_.eulr_.yaw, this->gyro_.z, this->eulr_.yaw, this->dt_); - + float autopatrol_yaw = 0; + this->setpoint_.eulr_.yaw = this->eulr_.yaw; /* 巡逻中间值 */ + autopatrol_yaw = this->setpoint_.eulr_.yaw + + this->param_.patrol_range * + sin(this->param_.patrol_omega * bsp_time_get_ms()); + + yaw_out = this->yaw_actuator_.Calculate(autopatrol_yaw, this->gyro_.z, + this->eulr_.yaw, this->dt_); pit_out = this->pit_actuator_.Calculate( this->setpoint_.eulr_.pit, this->gyro_.x, this->eulr_.pit, this->dt_); @@ -185,9 +189,8 @@ void Gimbal::SetMode(Mode mode) { } if (this->mode_ != AUTOPATROL) { if (mode == AUTOPATROL) { /* 切换到AUTOPATROL */ - this->t_ = 0; /* t_置零，保证sin从0开始 */ - this->yaw_virtual_ = - this->yaw_motor_.GetAngle() - this->param_.mech_zero.yaw; + /* t_置零，保证sin从0开始 */ + // this->yaw_virtual_ = this->eulr_.yaw; /* 锁定切换模式时的yaw角度作为巡逻基准角度 */ } } diff --git a/src/module/gimbal/mod_gimbal.hpp b/src/module/gimbal/mod_gimbal.hpp index 4dfe2b0d..6c716361 100644 --- a/src/module/gimbal/mod_gimbal.hpp +++ b/src/module/gimbal/mod_gimbal.hpp @@ -52,14 +52,15 @@ class Gimbal { Component::Type::Eulr mech_zero; - double patrol_rate; + double patrol_range; + double patrol_omega; struct { Component::Type::CycleValue pitch_max; Component::Type::CycleValue pitch_min; } limit; - const std::vector EVENT_MAP; + std::vector EVENT_MAP; } Param; @@ -104,10 +105,8 @@ class Gimbal { Message::Topic yaw_tp_ = Message::Topic("chassis_yaw"); - double t_; - float yaw_; - float yaw_virtual_; /* 用于标定哨兵小陀螺摆头巡航模式下的正方向 */ + // float yaw_virtual_; /* 用于标定哨兵小陀螺摆头巡航模式下的正方向 */ Component::UI::String string_; -- Gitee From a8cd7d0c946471bb6e0a1f6c4b0818ce8dbb365f Mon Sep 17 00:00:00 2001 From: Marine_zxy <1423266892@qq.com> Date: Sat, 23 Mar 2024 22:08:00 +0800 Subject: [PATCH 06/10] =?UTF-8?q?=E8=A7=A3=E5=86=B3ai=E5=92=8Cdr16?= =?UTF-8?q?=E7=9A=84event=E5=86=B2=E7=AA=81=E9=97=AE=E9=A2=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/device/ai/dev_ai.cpp | 57 +++++++++++++++++++++------------------- 1 file changed, 30 insertions(+), 27 deletions(-) diff --git a/src/device/ai/dev_ai.cpp b/src/device/ai/dev_ai.cpp index 72ab28b3..41601506 100644 --- a/src/device/ai/dev_ai.cpp +++ b/src/device/ai/dev_ai.cpp @@ -148,39 +148,42 @@ bool AI::PackCMD() { memcpy(&(this->cmd_.chassis), &(this->from_host_.data.chassis_move_vec), sizeof(this->from_host_.data.chassis_move_vec)); - /* AI在线的情况下，根据AI的数据来判定此时的状态 */ - /* 因为如果收不到AI的数据，那么from_host的值就不会更新 */ - if (this->cmd_.online) { - if (this->cmd_.gimbal.last_eulr.yaw == this->cmd_.gimbal.eulr.yaw && - this->cmd_.gimbal.last_eulr.pit == this->cmd_.gimbal.eulr.pit) { - this->ai_data_status_ = this->IS_INVALID_DATA; + /* 判断控制权是否在AI，以保证AI的event不会干扰dr16的event */ + if (Component::CMD::GetCtrlMode() == Component::CMD::CMD_AUTO_CTRL) { + /* AI在线的情况下，根据AI的数据来判定此时的状态 */ + /* 如果收不到AI的数据，那么from_host的值就不会更新 */ + if (this->cmd_.online) { + if (this->cmd_.gimbal.last_eulr.yaw == this->cmd_.gimbal.eulr.yaw && + this->cmd_.gimbal.last_eulr.pit == this->cmd_.gimbal.eulr.pit) { + this->ai_data_status_ = this->IS_INVALID_DATA; + } else { + this->ai_data_status_ = this->IS_USEFUL_DATA; + } } else { - this->ai_data_status_ = this->IS_USEFUL_DATA; + this->ai_data_status_ = this->AI_OFFLINE; } - } else { - this->ai_data_status_ = this->AI_OFFLINE; - } - /* AI离线，底盘RELEX模式 */ - if (!this->cmd_.online) { - this->event_.Active(AI_OFFLINE); - } + /* AI离线，底盘RELEX模式 */ + if (!this->cmd_.online) { + this->event_.Active(AI_OFFLINE); + } - /*AI数据无效，云台采用巡逻模式 */ - if (this->ai_data_status_ == this->IS_INVALID_DATA) { - this->event_.Active(AI_AUTOPATROL); - this->fire_command_ = 0; - } + /*AI数据无效，云台采用巡逻模式 */ + if (this->ai_data_status_ == this->IS_INVALID_DATA) { + this->event_.Active(AI_AUTOPATROL); + this->fire_command_ = 0; + } - /*AI数据有效，采用自动瞄准模式，也就是云台ABSOLUTE模式 */ - if (this->ai_data_status_ == IS_USEFUL_DATA) { - this->event_.Active(AI_FIND_TARGET); - this->fire_command_ = this->from_host_.data.notice; - } + /*AI数据有效，采用自动瞄准模式，也就是云台ABSOLUTE模式 */ + if (this->ai_data_status_ == IS_USEFUL_DATA) { + this->event_.Active(AI_FIND_TARGET); + this->fire_command_ = this->from_host_.data.notice; + } - /* AI开火发弹指令 */ - if (this->ai_data_status_ == IS_USEFUL_DATA && this->fire_command_ != 0) { - this->event_.Active(AI_FIRE_COMMAND); + /* AI开火发弹指令 */ + if (this->ai_data_status_ == IS_USEFUL_DATA && this->fire_command_ != 0) { + this->event_.Active(AI_FIRE_COMMAND); + } } this->cmd_.gimbal.last_eulr = this->cmd_.gimbal.eulr; -- Gitee From 05719959986c0e81e410a267abc121c0707d69ef Mon Sep 17 00:00:00 2001 From: Marine_zxy <1423266892@qq.com> Date: Sun, 24 Mar 2024 13:48:46 +0800 Subject: [PATCH 07/10] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E5=93=A8=E5=85=B5AI?= =?UTF-8?q?=E8=BD=AC=E5=90=91?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/device/ai/dev_ai.cpp | 28 ++++++++++++++++++++-------- src/device/ai/dev_ai.hpp | 8 +++++--- src/module/gimbal/mod_gimbal.cpp | 14 +++++++++++++- src/module/gimbal/mod_gimbal.hpp | 7 +++++-- src/module/launcher/mod_launcher.hpp | 2 +- 5 files changed, 44 insertions(+), 15 deletions(-) diff --git a/src/device/ai/dev_ai.cpp b/src/device/ai/dev_ai.cpp index 41601506..cff7e9fe 100644 --- a/src/device/ai/dev_ai.cpp +++ b/src/device/ai/dev_ai.cpp @@ -153,35 +153,47 @@ bool AI::PackCMD() { /* AI在线的情况下，根据AI的数据来判定此时的状态 */ /* 如果收不到AI的数据，那么from_host的值就不会更新 */ if (this->cmd_.online) { + this->ai_data_status_ = this->AI_ONLINE; if (this->cmd_.gimbal.last_eulr.yaw == this->cmd_.gimbal.eulr.yaw && this->cmd_.gimbal.last_eulr.pit == this->cmd_.gimbal.eulr.pit) { - this->ai_data_status_ = this->IS_INVALID_DATA; + this->ai_data_status_ = this->IS_INVALID_AMING_DATA; } else { - this->ai_data_status_ = this->IS_USEFUL_DATA; + this->ai_data_status_ = this->IS_USEFUL_AMING_DATA; } } else { this->ai_data_status_ = this->AI_OFFLINE; } /* AI离线，底盘RELEX模式 */ - if (!this->cmd_.online) { + if (this->cmd_.online) { + this->event_.Active(AI_ONLINE); + } else { this->event_.Active(AI_OFFLINE); } - /*AI数据无效，云台采用巡逻模式 */ - if (this->ai_data_status_ == this->IS_INVALID_DATA) { + /*AI云台数据无效，采用巡逻模式 */ + if (this->ai_data_status_ == this->IS_INVALID_AMING_DATA) { this->event_.Active(AI_AUTOPATROL); this->fire_command_ = 0; } - /*AI数据有效，采用自动瞄准模式，也就是云台ABSOLUTE模式 */ - if (this->ai_data_status_ == IS_USEFUL_DATA) { + /* AI转向 */ + /* 自瞄优先级高于转向 */ + if (this->ai_data_status_ == IS_USEFUL_AMING_DATA && + this->cmd_.chassis.z != 0) { + this->event_.Active(AI_TURN); + this->cmd_.gimbal.eulr.yaw = this->cmd_.chassis.z; + } + + /*AI云台数据有效，采用自动瞄准模式，也就是云台ABSOLUTE模式 */ + if (this->ai_data_status_ == IS_USEFUL_AMING_DATA) { this->event_.Active(AI_FIND_TARGET); this->fire_command_ = this->from_host_.data.notice; } /* AI开火发弹指令 */ - if (this->ai_data_status_ == IS_USEFUL_DATA && this->fire_command_ != 0) { + if (this->ai_data_status_ == IS_USEFUL_AMING_DATA && + this->fire_command_ != 0) { this->event_.Active(AI_FIRE_COMMAND); } } diff --git a/src/device/ai/dev_ai.hpp b/src/device/ai/dev_ai.hpp index 6b1ad201..b5595d47 100644 --- a/src/device/ai/dev_ai.hpp +++ b/src/device/ai/dev_ai.hpp @@ -33,10 +33,12 @@ class AI { typedef enum { AI_OFFLINE = 128, - IS_INVALID_DATA, - IS_USEFUL_DATA, + AI_ONLINE, + IS_INVALID_AMING_DATA, + IS_USEFUL_AMING_DATA, AI_FIND_TARGET, - AI_AUTOPATROL, + AI_AUTOPATROL, /* 直线巡逻，不含转弯（可能也不需要单独写个模式） */ + AI_TURN, AI_FIRE_COMMAND, } AI_DATA; /* 这个变量如何跟notice建立联系 */ diff --git a/src/module/gimbal/mod_gimbal.cpp b/src/module/gimbal/mod_gimbal.cpp index d4da1147..73b41a0a 100644 --- a/src/module/gimbal/mod_gimbal.cpp +++ b/src/module/gimbal/mod_gimbal.cpp @@ -15,6 +15,7 @@ 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_speed_actuator_(this->param_.yaw_speed_actr, control_freq), yaw_motor_(this->param_.yaw_motor, "Gimbal_Yaw"), pit_motor_(this->param_.pit_motor, "Gimbal_Pitch"), ctrl_lock_(true) { @@ -36,6 +37,9 @@ Gimbal::Gimbal(Param& param, float control_freq) case SET_AUTOPATROL: /* 自动巡逻 */ gimbal->SetMode(static_cast(AUTOPATROL)); break; + case SET_AI_TURN: /* AI转向 */ + gimbal->SetMode(static_cast(TURN)); + break; } gimbal->ctrl_lock_.Post(); }; @@ -84,6 +88,7 @@ void Gimbal::UpdateFeedback() { switch (this->mode_) { case RELAX: case ABSOLUTE: + case TURN: this->yaw_ = this->yaw_motor_.GetAngle() - this->param_.mech_zero.yaw; break; case AUTOPATROL: @@ -133,6 +138,7 @@ void Gimbal::Control() { /* 控制相关逻辑 */ float yaw_out = 0; float pit_out = 0; + float autopatrol_yaw = 0; switch (this->mode_) { case RELAX: this->yaw_motor_.Relax(); @@ -154,7 +160,6 @@ void Gimbal::Control() { case AUTOPATROL: /* 以sin变化左右摆头 */ - float autopatrol_yaw = 0; this->setpoint_.eulr_.yaw = this->eulr_.yaw; /* 巡逻中间值 */ autopatrol_yaw = this->setpoint_.eulr_.yaw + this->param_.patrol_range * @@ -168,6 +173,13 @@ void Gimbal::Control() { this->yaw_motor_.Control(yaw_out); this->pit_motor_.Control(pit_out); break; + + case TURN: /* 转向的时候不进行摆头扫描动作 */ + this->setpoint_.eulr_.yaw = this->eulr_.yaw; + yaw_out = + this->yaw_speed_actuator_.Calculate(0.0f, this->eulr_.yaw, this->dt_); + this->yaw_motor_.Control(yaw_out); + break; } } diff --git a/src/module/gimbal/mod_gimbal.hpp b/src/module/gimbal/mod_gimbal.hpp index 6c716361..21e2c28d 100644 --- a/src/module/gimbal/mod_gimbal.hpp +++ b/src/module/gimbal/mod_gimbal.hpp @@ -22,6 +22,7 @@ class Gimbal { RELAX, /* 放松模式，电机不输出。一般情况云台初始化之后的模式 */ ABSOLUTE, /* 绝对坐标系控制，控制在空间内的绝对姿态 */ AUTOPATROL, + TURN, } Mode; enum { @@ -38,6 +39,7 @@ class Gimbal { START_AUTO_AIM, STOP_AUTO_AIM, SET_AUTOPATROL, + SET_AI_TURN, } GimbalEvent; typedef struct { @@ -46,6 +48,7 @@ class Gimbal { Component::PosActuator::Param yaw_actr; Component::PosActuator::Param pit_actr; + Component::SpeedActuator::Param yaw_speed_actr; Device::RMMotor::Param yaw_motor; Device::RMMotor::Param pit_motor; @@ -60,7 +63,7 @@ class Gimbal { Component::Type::CycleValue pitch_min; } limit; - std::vector EVENT_MAP; + const std::vector EVENT_MAP; } Param; @@ -95,6 +98,7 @@ class Gimbal { Component::PosActuator yaw_actuator_; Component::PosActuator pit_actuator_; + Component::SpeedActuator yaw_speed_actuator_; Device::RMMotor yaw_motor_; Device::RMMotor pit_motor_; @@ -106,7 +110,6 @@ class Gimbal { Message::Topic yaw_tp_ = Message::Topic("chassis_yaw"); float yaw_; - // float yaw_virtual_; /* 用于标定哨兵小陀螺摆头巡航模式下的正方向 */ Component::UI::String string_; diff --git a/src/module/launcher/mod_launcher.hpp b/src/module/launcher/mod_launcher.hpp index 8c2ec2fe..b7e0c773 100644 --- a/src/module/launcher/mod_launcher.hpp +++ b/src/module/launcher/mod_launcher.hpp @@ -79,7 +79,7 @@ class Launcher { std::array trig_motor; std::array fric_motor; - std::vector EVENT_MAP; + const std::vector EVENT_MAP; } Param; /* 热量控制 */ -- Gitee From ffb828d35e37f67d2e553d984f632d6be767c1e0 Mon Sep 17 00:00:00 2001 From: Marine_zxy <1423266892@qq.com> Date: Sun, 24 Mar 2024 21:32:29 +0800 Subject: [PATCH 08/10] =?UTF-8?q?=E8=A3=81=E5=88=A4=E7=B3=BB=E7=BB=9F?= =?UTF-8?q?=E5=AE=8C=E5=96=84?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/component/comp_cmd.hpp | 12 +++++---- src/device/referee/dev_referee.cpp | 7 ++--- src/device/referee/dev_referee.hpp | 42 +++++++----------------------- 3 files changed, 21 insertions(+), 40 deletions(-) diff --git a/src/component/comp_cmd.hpp b/src/component/comp_cmd.hpp index 53f98143..3db2763d 100644 --- a/src/component/comp_cmd.hpp +++ b/src/component/comp_cmd.hpp @@ -16,6 +16,12 @@ class CMD { CTRL_SOURCE_NUM } ControlSource; + typedef struct { + float yaw; /* 偏航角（Yaw angle） */ + float pit; /* 俯仰角（Pitch angle） */ + float rol; /* 翻滚角（Roll angle） */ + } Eulr; + typedef enum { CMD_OP_CTRL, CMD_AUTO_CTRL, @@ -25,11 +31,7 @@ class CMD { typedef Type::Vector3 ChassisCMD; typedef struct { - struct { - float yaw; /* 偏航角（Yaw angle） */ - float pit; /* 俯仰角（Pitch angle） */ - float rol; /* 翻滚角（Roll angle） */ - } eulr, last_eulr; + Eulr eulr; GimbalCommandMode mode; } GimbalCMD; diff --git a/src/device/referee/dev_referee.cpp b/src/device/referee/dev_referee.cpp index 2904834b..9abf7258 100644 --- a/src/device/referee/dev_referee.cpp +++ b/src/device/referee/dev_referee.cpp @@ -254,6 +254,10 @@ void Referee::Prase() { destination = &(this->ref_data_.keyboard_mouse); size = sizeof(this->ref_data_.keyboard_mouse); break; + case REF_CMD_ID_MAP_ROBOT_DATA: + destination = &(this->ref_data_.map_robot_data); + size = sizeof(this->ref_data_.map_robot_data); + break; case REF_CMD_ID_CUSTOM_KEYBOARD_MOUSE: destination = &(this->ref_data_.custom_key_mouse_data); size = sizeof(this->ref_data_.custom_key_mouse_data); @@ -298,9 +302,6 @@ void Referee::Prase() { #endif this->ref_data_.power_heat.launcher_id1_17_heat = REF_HEAT_LIMIT_17; this->ref_data_.power_heat.launcher_42_heat = REF_HEAT_LIMIT_42; - // this->ref_data_.robot_status.launcher_id1_17_speed_limit = - // REF_LAUNCH_SPEED; - this->ref_data_.robot_status.launcher_42_speed_limit = REF_LAUNCH_SPEED; this->ref_data_.robot_status.chassis_power_limit = REF_POWER_LIMIT; this->ref_data_.power_heat.chassis_pwr_buff = REF_POWER_BUFF; #endif diff --git a/src/device/referee/dev_referee.hpp b/src/device/referee/dev_referee.hpp index bb6b190a..a7a76e12 100644 --- a/src/device/referee/dev_referee.hpp +++ b/src/device/referee/dev_referee.hpp @@ -12,6 +12,9 @@ #define GAME_HEAT_INCREASE_42MM (100.0f) /* 每发射一颗42mm弹丸增加100热量 */ #define GAME_HEAT_INCREASE_17MM (10.0f) /* 每发射一颗17mm弹丸增加10热量 */ +#define BULLET_SPEED_LIMIT_42MM (16.0) +#define BULLET_SPEED_LIMIT_17MM (30.0) + #define GAME_CHASSIS_MAX_POWER_WO_REF 40.0f /* 裁判系统离线时底盘最大功率 */ #define REF_UI_BOX_UP_OFFSET (4) #define REF_UI_BOX_BOT_OFFSET (-14) @@ -77,7 +80,7 @@ class Referee { REF_CMD_ID_INTER_STUDENT_CUSTOM = 0x0302, REF_CMD_ID_CLIENT_MAP = 0x0303, REF_CMD_ID_KEYBOARD_MOUSE = 0x0304, - // 0x0305 + REF_CMD_ID_MAP_ROBOT_DATA = 0x0305, // 0x0305 REF_CMD_ID_CUSTOM_KEYBOARD_MOUSE = 0X0306, REF_CMD_ID_SENTRY_POS_DATA = 0x0307, REF_CMD_ID_ROBOT_POS_DATA = 0x0308, /* 选手端小地图接受机器人消息 */ @@ -126,31 +129,6 @@ class Referee { uint16_t blue_outpose; uint16_t blue_base; } RobotHP; /* 0x0003 */ - /* - typedef struct __attribute__((packed)) { - uint8_t dart_belong; - uint16_t stage_remain_time; - } DartStatus;//0x0105已经有代替 - - typedef struct __attribute__((packed)) { - uint8_t f1_status : 1; - uint8_t f1_buff_status : 3; - uint8_t f2_status : 1; - uint8_t f2_buff_status : 3; - uint8_t f3_status : 1; - uint8_t f3_buff_status : 3; - uint8_t f4_status : 1; - uint8_t f4_buff_status : 3; - uint8_t f5_status : 1; - uint8_t f5_buff_status : 3; - uint8_t f6_status : 1; - uint8_t f6_buff_status : 3; - uint16_t red1_bullet_remain; - uint16_t red2_bullet_remain; - uint16_t blue1_bullet_remain; - uint16_t blue2_bullet_remain; - } IcraZoneStatus; - */ typedef struct __attribute__((packed)) { uint32_t blood_supply_before_status : 1; uint32_t blood_supply_inner_status : 1; @@ -411,7 +389,8 @@ class Referee { REF_STDNT_CMD_ID_UI_DRAW7 = 0x0104, REF_STDNT_CMD_ID_UI_STR = 0x0110, REF_STDNT_CMD_ID_CUSTOM = 0x0200, - // 新的 0x0120 0x0121 未写入 + REF_STDNT_CMD_ID_SENTRY_COMMAND = 0X0120, + REF_STDNT_CMD_ID_RADAR_COMMAND = 0X0121, } CMDID; typedef struct __attribute__((packed)) { @@ -422,11 +401,9 @@ class Referee { typedef struct { Status status; - GameStatus game_status; /* 0x0001 */ - GameResult game_result; /* 0x0002 */ - RobotHP game_robot_hp; /* 0x0003 */ - // DartStatus dart_status; - // IcraZoneStatus icra_zone; + GameStatus game_status; /* 0x0001 */ + GameResult game_result; /* 0x0002 */ + RobotHP game_robot_hp; /* 0x0003 */ FieldEvents field_event; /* 0x0101 */ SupplyAction supply_action; /* 0x0102 */ Warning warning; /* 0x0104 */ @@ -451,6 +428,7 @@ class Referee { SentryInfo sentry_decision; /* 0x020D */ RadarInfo radar_decision; /* 0x020E */ RobotInteractionData robot_ineraction_data; /* 0x0301 */ + MapRobotData map_robot_data; /* 0x0305 */ CustomKeyMouseData custom_key_mouse_data; /* 0x0306 */ } Data; -- Gitee From f86afeaab6db6f707b6bd766f494f8cecf019531 Mon Sep 17 00:00:00 2001 From: Marine_zxy <1423266892@qq.com> Date: Sun, 24 Mar 2024 22:37:51 +0800 Subject: [PATCH 09/10] =?UTF-8?q?=E5=AE=8C=E5=96=84=E5=93=A8=E5=85=B5?= =?UTF-8?q?=E8=BF=90=E5=8A=A8=E6=8E=A7=E5=88=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/device/ai/dev_ai.cpp | 50 ++++++++------------ src/device/ai/dev_ai.hpp | 20 ++++---- src/module/gimbal/mod_gimbal.cpp | 27 +++-------- src/module/gimbal/mod_gimbal.hpp | 8 ++-- src/module/launcher/mod_launcher.cpp | 4 +- src/robot/sentry/robot.cpp | 68 ++++++++++++++++------------ 6 files changed, 81 insertions(+), 96 deletions(-) diff --git a/src/device/ai/dev_ai.cpp b/src/device/ai/dev_ai.cpp index cff7e9fe..bfee2d89 100644 --- a/src/device/ai/dev_ai.cpp +++ b/src/device/ai/dev_ai.cpp @@ -136,8 +136,6 @@ bool AI::PackRef() { } bool AI::PackCMD() { - this->cmd_.gimbal.mode = Component::CMD::GIMBAL_ABSOLUTE_CTRL; - memcpy(&(this->cmd_.gimbal.eulr), &(this->from_host_.data.gimbal), sizeof(this->cmd_.gimbal.eulr)); @@ -148,14 +146,14 @@ bool AI::PackCMD() { memcpy(&(this->cmd_.chassis), &(this->from_host_.data.chassis_move_vec), sizeof(this->from_host_.data.chassis_move_vec)); - /* 判断控制权是否在AI，以保证AI的event不会干扰dr16的event */ + memcpy(&(this->fire_command_), &(this->from_host_.data.notice), + sizeof(this->from_host_.data.notice)); + + /* 控制权在AI */ if (Component::CMD::GetCtrlMode() == Component::CMD::CMD_AUTO_CTRL) { - /* AI在线的情况下，根据AI的数据来判定此时的状态 */ - /* 如果收不到AI的数据，那么from_host的值就不会更新 */ if (this->cmd_.online) { - this->ai_data_status_ = this->AI_ONLINE; - if (this->cmd_.gimbal.last_eulr.yaw == this->cmd_.gimbal.eulr.yaw && - this->cmd_.gimbal.last_eulr.pit == this->cmd_.gimbal.eulr.pit) { + if (this->last_eulr_.yaw == this->cmd_.gimbal.eulr.yaw && + this->last_eulr_.pit == this->cmd_.gimbal.eulr.pit) { this->ai_data_status_ = this->IS_INVALID_AMING_DATA; } else { this->ai_data_status_ = this->IS_USEFUL_AMING_DATA; @@ -164,41 +162,29 @@ bool AI::PackCMD() { this->ai_data_status_ = this->AI_OFFLINE; } - /* AI离线，底盘RELEX模式 */ - if (this->cmd_.online) { - this->event_.Active(AI_ONLINE); - } else { - this->event_.Active(AI_OFFLINE); - } - - /*AI云台数据无效，采用巡逻模式 */ + /*AI云台数据无效，巡逻模式 */ if (this->ai_data_status_ == this->IS_INVALID_AMING_DATA) { this->event_.Active(AI_AUTOPATROL); - this->fire_command_ = 0; } /* AI转向 */ /* 自瞄优先级高于转向 */ - if (this->ai_data_status_ == IS_USEFUL_AMING_DATA && + if (this->ai_data_status_ == IS_INVALID_AMING_DATA && this->cmd_.chassis.z != 0) { this->event_.Active(AI_TURN); - this->cmd_.gimbal.eulr.yaw = this->cmd_.chassis.z; } - /*AI云台数据有效，采用自动瞄准模式，也就是云台ABSOLUTE模式 */ + /*AI云台数据有效，自动瞄准模式 */ if (this->ai_data_status_ == IS_USEFUL_AMING_DATA) { this->event_.Active(AI_FIND_TARGET); - this->fire_command_ = this->from_host_.data.notice; - } - - /* AI开火发弹指令 */ - if (this->ai_data_status_ == IS_USEFUL_AMING_DATA && - this->fire_command_ != 0) { - this->event_.Active(AI_FIRE_COMMAND); + if (fire_command_ == AI_NOTICE_FIRE) { + this->event_.Active(AI_FIRE_COMMAND); /* AI开火发弹指令 */ + } } } - this->cmd_.gimbal.last_eulr = this->cmd_.gimbal.eulr; + memcpy(&(this->last_eulr_), &(this->cmd_.gimbal.eulr), + sizeof(this->cmd_.gimbal.eulr)); this->cmd_.ctrl_source = Component::CMD::CTRL_SOURCE_AI; @@ -208,13 +194,13 @@ bool AI::PackCMD() { void AI::PraseRef() { #if RB_HERO - this->ref_.ball_speed = this->ref_.data_.robot_status.launcher_42_speed_limit; + this->ref_.ball_speed = BULLET_SPEED_LIMIT_42MM #else - // this->ref_.ball_speed = - // this->raw_ref_.robot_status.launcher_id1_17_speed_limit; + this->ref_.ball_speed = BULLET_SPEED_LIMIT_17MM; #endif - this->ref_.max_hp = this->raw_ref_.robot_status.max_hp; + this->ref_.max_hp = + this->raw_ref_.robot_status.max_hp; this->ref_.hp = this->raw_ref_.robot_status.remain_hp; diff --git a/src/device/ai/dev_ai.hpp b/src/device/ai/dev_ai.hpp index b5595d47..426dcbb9 100644 --- a/src/device/ai/dev_ai.hpp +++ b/src/device/ai/dev_ai.hpp @@ -32,16 +32,17 @@ class AI { } RefForAI; typedef enum { - AI_OFFLINE = 128, - AI_ONLINE, IS_INVALID_AMING_DATA, IS_USEFUL_AMING_DATA, - AI_FIND_TARGET, - AI_AUTOPATROL, /* 直线巡逻，不含转弯（可能也不需要单独写个模式） */ + AI_OFFLINE, + } AIDataStatus; + typedef enum { + AI_FIND_TARGET = 128, + AI_AUTOPATROL, AI_TURN, AI_FIRE_COMMAND, - } AI_DATA; - /* 这个变量如何跟notice建立联系 */ + } AIControlData; + AI(); bool StartRecv(); @@ -62,9 +63,10 @@ class AI { private: bool ref_updated_ = false; + uint32_t last_online_time_ = 0; - Protocol_DownPackage_t from_host_{}; /* 从ai拿到的原始数据数组 */ + Protocol_DownPackage_t from_host_{}; uint8_t ai_data_status_; @@ -81,12 +83,14 @@ class AI { System::Semaphore data_ready_; - Message::Event event_; /* 为了上面那个功能，尝试中 */ + Message::Event event_; Message::Topic cmd_tp_; Component::CMD::Data cmd_{}; + Component::CMD::Eulr last_eulr_; + Component::Type::Quaternion quat_{}; Device::Referee::Data raw_ref_{}; }; diff --git a/src/module/gimbal/mod_gimbal.cpp b/src/module/gimbal/mod_gimbal.cpp index 73b41a0a..fdf7dfc2 100644 --- a/src/module/gimbal/mod_gimbal.cpp +++ b/src/module/gimbal/mod_gimbal.cpp @@ -15,7 +15,6 @@ 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_speed_actuator_(this->param_.yaw_speed_actr, control_freq), yaw_motor_(this->param_.yaw_motor, "Gimbal_Yaw"), pit_motor_(this->param_.pit_motor, "Gimbal_Pitch"), ctrl_lock_(true) { @@ -34,10 +33,10 @@ Gimbal::Gimbal(Param& param, float control_freq) case STOP_AUTO_AIM: Component::CMD::SetCtrlSource(Component::CMD::CTRL_SOURCE_RC); break; - case SET_AUTOPATROL: /* 自动巡逻 */ + case SET_AUTOPATROL: gimbal->SetMode(static_cast(AUTOPATROL)); break; - case SET_AI_TURN: /* AI转向 */ + case SET_AI_TURN: gimbal->SetMode(static_cast(TURN)); break; } @@ -144,8 +143,8 @@ void Gimbal::Control() { this->yaw_motor_.Relax(); this->pit_motor_.Relax(); break; - - case ABSOLUTE: //作为瞄准模式使用 + case ABSOLUTE: + case TURN: /* Yaw轴角速度环参数计算 */ yaw_out = this->yaw_actuator_.Calculate( this->setpoint_.eulr_.yaw, this->gyro_.z, this->eulr_.yaw, this->dt_); @@ -173,13 +172,6 @@ void Gimbal::Control() { this->yaw_motor_.Control(yaw_out); this->pit_motor_.Control(pit_out); break; - - case TURN: /* 转向的时候不进行摆头扫描动作 */ - this->setpoint_.eulr_.yaw = this->eulr_.yaw; - yaw_out = - this->yaw_speed_actuator_.Calculate(0.0f, this->eulr_.yaw, this->dt_); - this->yaw_motor_.Control(yaw_out); - break; } } @@ -194,16 +186,9 @@ void Gimbal::SetMode(Mode mode) { memcpy(&(this->setpoint_.eulr_), &(this->eulr_), sizeof(this->setpoint_.eulr_)); /* 切换模式后重置设定值 */ - if (this->mode_ != ABSOLUTE) { + if (this->mode_ == RELAX) { if (mode == ABSOLUTE) { - this->setpoint_.eulr_.yaw = this->eulr_.yaw; /* imu的eulr */ - } - } - if (this->mode_ != AUTOPATROL) { - if (mode == AUTOPATROL) { /* 切换到AUTOPATROL */ - /* t_置零，保证sin从0开始 */ - // this->yaw_virtual_ = this->eulr_.yaw; - /* 锁定切换模式时的yaw角度作为巡逻基准角度 */ + this->setpoint_.eulr_.yaw = this->eulr_.yaw; } } diff --git a/src/module/gimbal/mod_gimbal.hpp b/src/module/gimbal/mod_gimbal.hpp index 21e2c28d..15c28a69 100644 --- a/src/module/gimbal/mod_gimbal.hpp +++ b/src/module/gimbal/mod_gimbal.hpp @@ -48,15 +48,14 @@ class Gimbal { Component::PosActuator::Param yaw_actr; Component::PosActuator::Param pit_actr; - Component::SpeedActuator::Param yaw_speed_actr; Device::RMMotor::Param yaw_motor; Device::RMMotor::Param pit_motor; Component::Type::Eulr mech_zero; - double patrol_range; - double patrol_omega; + float patrol_range; + float patrol_omega; struct { Component::Type::CycleValue pitch_max; @@ -88,7 +87,7 @@ class Gimbal { Param param_; - Gimbal::Mode mode_ = RELAX; /* 云台模式 */ + Gimbal::Mode mode_ = ABSOLUTE; /* 云台模式 */ struct { Component::Type::Eulr eulr_; /* 表示云台姿态的欧拉角 */ @@ -98,7 +97,6 @@ class Gimbal { Component::PosActuator yaw_actuator_; Component::PosActuator pit_actuator_; - Component::SpeedActuator yaw_speed_actuator_; Device::RMMotor yaw_motor_; Device::RMMotor pit_motor_; diff --git a/src/module/launcher/mod_launcher.cpp b/src/module/launcher/mod_launcher.cpp index 489595e1..a0b14fb1 100644 --- a/src/module/launcher/mod_launcher.cpp +++ b/src/module/launcher/mod_launcher.cpp @@ -291,14 +291,14 @@ void Launcher::HeatLimit() { if (this->param_.model == LAUNCHER_MODEL_42MM) { this->heat_ctrl_.heat = this->ref_.power_heat.launcher_42_heat; this->heat_ctrl_.heat_limit = this->ref_.robot_status.shooter_heat_limit; - this->heat_ctrl_.speed_limit = 16; + this->heat_ctrl_.speed_limit = BULLET_SPEED_LIMIT_42MM; this->heat_ctrl_.cooling_rate = this->ref_.robot_status.shooter_cooling_value; this->heat_ctrl_.heat_increase = GAME_HEAT_INCREASE_42MM; } else if (this->param_.model == LAUNCHER_MODEL_17MM) { this->heat_ctrl_.heat = this->ref_.power_heat.launcher_id1_17_heat; this->heat_ctrl_.heat_limit = this->ref_.robot_status.shooter_heat_limit; - this->heat_ctrl_.speed_limit = 30; + this->heat_ctrl_.speed_limit = BULLET_SPEED_LIMIT_17MM; this->heat_ctrl_.cooling_rate = this->ref_.robot_status.shooter_cooling_value; this->heat_ctrl_.heat_increase = GAME_HEAT_INCREASE_17MM; diff --git a/src/robot/sentry/robot.cpp b/src/robot/sentry/robot.cpp index d7bf09eb..70dbaacb 100644 --- a/src/robot/sentry/robot.cpp +++ b/src/robot/sentry/robot.cpp @@ -37,7 +37,7 @@ Robot::Sentry::Param param = { }, Component::CMD::EventMapItem{ Device::DR16::DR16_SW_L_POS_BOT,/* 模拟未找到目标，巡逻模式 */ - Module::RMChassis::SET_MODE_ROTOR + Module::RMChassis::SET_MODE_ROTOR, }, Component::CMD::EventMapItem{ Device::AI::AI_FIND_TARGET, @@ -46,13 +46,17 @@ Robot::Sentry::Param param = { Component::CMD::EventMapItem{ Device::AI::AI_AUTOPATROL, Module::RMChassis::SET_MODE_ROTOR + }, + Component::CMD::EventMapItem{ + Device::AI::AI_TURN, + Module::RMChassis::SET_MODE_ROTOR } }, .actuator_param = { Component::SpeedActuator::Param{ .speed = { - .k = 0.00015f, + .k = 0.00020f, .p = 1.0f, .i = 0.0f, .d = 0.0f, @@ -85,7 +89,7 @@ Robot::Sentry::Param param = { }, Component::SpeedActuator::Param{ .speed = { - .k = 0.00015f, + .k = 0.00020f, .p = 1.0f, .i = 0.0f, .d = 0.0f, @@ -101,7 +105,7 @@ Robot::Sentry::Param param = { }, Component::SpeedActuator::Param{ .speed = { - .k = 0.00015f, + .k = 0.00022f, .p = 1.0f, .i = 0.0f, .d = 0.0f, @@ -179,7 +183,7 @@ Robot::Sentry::Param param = { .position = { /* GIMBAL_CTRL_YAW_ANGLE_IDX */ - .k = 10.0f, + .k = 5.0f, .p = 1.0f, .i = 0.0f, .d = 1.4f, @@ -208,8 +212,8 @@ Robot::Sentry::Param param = { .position = { /* GIMBAL_CTRL_PIT_ANGLE_IDX */ - .k = 20.0f, - .p = 1.0f, + .k = 8.0f, + .p = 0.5f, .i = 0.0f, .d = 0.8f, .i_limit = 0.0f, @@ -242,7 +246,9 @@ Robot::Sentry::Param param = { .pit = 4.0f, .rol = 0.0f, }, - .patrol_rate = 0.25, + + .patrol_range = 0.25, + .patrol_omega = 0.005, .limit = { .pitch_max = 0.57f, @@ -252,7 +258,7 @@ Robot::Sentry::Param param = { .EVENT_MAP = { Component::CMD::EventMapItem{ Component::CMD::CMD_EVENT_LOST_CTRL, - Module::Gimbal::SET_MODE_RELAX + Module::Gimbal::SET_MODE_ABSOLUTE }, Component::CMD::EventMapItem{ Device::DR16::DR16_SW_R_POS_TOP, @@ -272,7 +278,11 @@ Robot::Sentry::Param param = { }, Component::CMD::EventMapItem{ Device::AI::AI_AUTOPATROL, - Module::Gimbal::SET_AUTOPATROL + Module::Gimbal::SET_MODE_ABSOLUTE + }, + Component::CMD::EventMapItem{ + Device::AI::AI_TURN, + Module::Gimbal::SET_AI_TURN } }, @@ -399,19 +409,19 @@ Robot::Sentry::Param param = { Module::Launcher::LAUNCHER_START_FIRE }, Component::CMD::EventMapItem{ - Device::DR16::KEY_L_PRESS, - Module::Launcher::LAUNCHER_START_FIRE - }, - Component::CMD::EventMapItem{ - Device::AI::AI_DATA::AI_FIRE_COMMAND, - Module::Launcher::LAUNCHER_START_FIRE + Device::AI::AIControlData::AI_FIND_TARGET, + Module::Launcher::CHANGE_FIRE_MODE_LOADED }, Component::CMD::EventMapItem{ - Device::AI::AI_DATA::AI_FIRE_COMMAND, + Device::AI::AIControlData::AI_FIRE_COMMAND, Module::Launcher::CHANGE_FIRE_MODE_LOADED + }, + Component::CMD::EventMapItem{ + Device::AI::AIControlData::AI_FIRE_COMMAND, + Module::Launcher::LAUNCHER_START_FIRE }, Component::CMD::EventMapItem{ - Device::AI::AI_DATA::AI_FIRE_COMMAND, + Device::AI::AIControlData::AI_FIRE_COMMAND, Module::Launcher::CHANGE_TRIG_MODE_SINGLE } @@ -498,7 +508,7 @@ Robot::Sentry::Param param = { .id_control = M3508_M2006_CTRL_ID_EXTAND, .model = Device::RMMotor::MOTOR_M2006, .can = BSP_CAN_2, - .reverse = false, + .reverse = true, }, }, @@ -508,17 +518,19 @@ Robot::Sentry::Param param = { .id_control = M3508_M2006_CTRL_ID_BASE, .model = Device::RMMotor::MOTOR_M3508, .can = BSP_CAN_2, + .reverse = true, }, Device::RMMotor::Param{ .id_feedback = 0x201, .id_control = M3508_M2006_CTRL_ID_BASE, .model = Device::RMMotor::MOTOR_M3508, .can = BSP_CAN_2, + .reverse = true, }, }, .EVENT_MAP = { - Component::CMD::EventMapItem{ + Component::CMD::EventMapItem{ Component::CMD::CMD_EVENT_LOST_CTRL, Module::Launcher::CHANGE_FIRE_MODE_RELAX }, @@ -539,19 +551,19 @@ Robot::Sentry::Param param = { Module::Launcher::LAUNCHER_START_FIRE }, Component::CMD::EventMapItem{ - Device::DR16::KEY_L_PRESS, - Module::Launcher::LAUNCHER_START_FIRE - }, - Component::CMD::EventMapItem{ - Device::AI::AI_DATA::AI_FIRE_COMMAND, - Module::Launcher::LAUNCHER_START_FIRE + Device::AI::AIControlData::AI_FIND_TARGET, + Module::Launcher::CHANGE_FIRE_MODE_LOADED }, Component::CMD::EventMapItem{ - Device::AI::AI_DATA::AI_FIRE_COMMAND, + Device::AI::AIControlData::AI_FIRE_COMMAND, Module::Launcher::CHANGE_FIRE_MODE_LOADED + }, + Component::CMD::EventMapItem{ + Device::AI::AIControlData::AI_FIRE_COMMAND, + Module::Launcher::LAUNCHER_START_FIRE }, Component::CMD::EventMapItem{ - Device::AI::AI_DATA::AI_FIRE_COMMAND, + Device::AI::AIControlData::AI_FIRE_COMMAND, Module::Launcher::CHANGE_TRIG_MODE_SINGLE } -- Gitee From f2c69cfc1af94164c9661265081213262602a5f2 Mon Sep 17 00:00:00 2001 From: Marine_zxy <1423266892@qq.com> Date: Mon, 25 Mar 2024 21:40:18 +0800 Subject: [PATCH 10/10] =?UTF-8?q?=E5=AE=8C=E5=96=84=E5=93=A8=E5=85=B5wz?= =?UTF-8?q?=E8=BD=AC=E5=BC=AF=E6=8E=A7=E5=88=B6=E9=80=BB=E8=BE=91?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/component/comp_mixer.cpp | 6 ++++++ src/device/ai/dev_ai.cpp | 4 ++++ .../dev_custom_controller.hpp | 2 +- src/robot/sentry/robot.cpp | 18 +++++++++--------- 4 files changed, 20 insertions(+), 10 deletions(-) diff --git a/src/component/comp_mixer.cpp b/src/component/comp_mixer.cpp index 35d18a50..3f9ec3b4 100644 --- a/src/component/comp_mixer.cpp +++ b/src/component/comp_mixer.cpp @@ -71,6 +71,12 @@ bool Mixer::Apply(Component::Type::MoveVector &move_vec, float *out) { break; case OMNICROSS: + ASSERT(this->len_ == 4); + out[0] = move_vec.vx - move_vec.vy + move_vec.wz; + out[1] = move_vec.vx + move_vec.vy + move_vec.wz; + out[2] = -move_vec.vx + move_vec.vy + move_vec.wz; + out[3] = -move_vec.vx - move_vec.vy + move_vec.wz; + break; case OMNIPLUS: break; diff --git a/src/device/ai/dev_ai.cpp b/src/device/ai/dev_ai.cpp index bfee2d89..86ad8b1a 100644 --- a/src/device/ai/dev_ai.cpp +++ b/src/device/ai/dev_ai.cpp @@ -171,11 +171,15 @@ bool AI::PackCMD() { /* 自瞄优先级高于转向 */ if (this->ai_data_status_ == IS_INVALID_AMING_DATA && this->cmd_.chassis.z != 0) { + /* 将底盘wz复制给yaw,实现AI间接控制云台进而控制底盘 */ + this->cmd_.gimbal.eulr.yaw = this->cmd_.chassis.z; + this->cmd_.gimbal.mode = Component::CMD::GIMBAL_RELATIVE_CTRL; this->event_.Active(AI_TURN); } /*AI云台数据有效，自动瞄准模式 */ if (this->ai_data_status_ == IS_USEFUL_AMING_DATA) { + this->cmd_.gimbal.mode = Component::CMD::GIMBAL_ABSOLUTE_CTRL; this->event_.Active(AI_FIND_TARGET); if (fire_command_ == AI_NOTICE_FIRE) { this->event_.Active(AI_FIRE_COMMAND); /* AI开火发弹指令 */ diff --git a/src/device/custom_controller/dev_custom_controller.hpp b/src/device/custom_controller/dev_custom_controller.hpp index e1a7e942..e12eb453 100644 --- a/src/device/custom_controller/dev_custom_controller.hpp +++ b/src/device/custom_controller/dev_custom_controller.hpp @@ -6,7 +6,7 @@ class CustomController { public: CustomController(); - typedef enum { NUM = 31 } ControllerEvent; + typedef enum { NUM = 144 } ControllerEvent; bool StartRecv(); diff --git a/src/robot/sentry/robot.cpp b/src/robot/sentry/robot.cpp index 70dbaacb..4a9e64cf 100644 --- a/src/robot/sentry/robot.cpp +++ b/src/robot/sentry/robot.cpp @@ -9,10 +9,10 @@ /* clang-format off */ Robot::Sentry::Param param = { .chassis={ - .type = Component::Mixer::MECANUM, + .type = Component::Mixer::OMNICROSS, .follow_pid_param = { - .k = 0.5f, + .k = 0.8f, .p = 1.0f, .i = 0.0f, .d = 0.0f, @@ -200,7 +200,7 @@ Robot::Sentry::Param param = { .pit_actr = { .speed = { /* GIMBAL_CTRL_PIT_OMEGA_IDX */ - .k = 0.1f, + .k = 0.25f, .p = 1.0f, .i = 0.f, .d = 0.f, @@ -212,10 +212,10 @@ Robot::Sentry::Param param = { .position = { /* GIMBAL_CTRL_PIT_ANGLE_IDX */ - .k = 8.0f, - .p = 0.5f, + .k = 16.0f, + .p = 1.0f, .i = 0.0f, - .d = 0.8f, + .d = 0.0f, .i_limit = 0.0f, .out_limit = 10.0f, .d_cutoff_freq = -1.0f, @@ -242,7 +242,7 @@ Robot::Sentry::Param param = { }, .mech_zero = { - .yaw = 1.3f, + .yaw = 3.12f, .pit = 4.0f, .rol = 0.0f, }, @@ -258,7 +258,7 @@ Robot::Sentry::Param param = { .EVENT_MAP = { Component::CMD::EventMapItem{ Component::CMD::CMD_EVENT_LOST_CTRL, - Module::Gimbal::SET_MODE_ABSOLUTE + Module::Gimbal::SET_MODE_RELAX }, Component::CMD::EventMapItem{ Device::DR16::DR16_SW_R_POS_TOP, @@ -278,7 +278,7 @@ Robot::Sentry::Param param = { }, Component::CMD::EventMapItem{ Device::AI::AI_AUTOPATROL, - Module::Gimbal::SET_MODE_ABSOLUTE + Module::Gimbal::SET_AUTOPATROL }, Component::CMD::EventMapItem{ Device::AI::AI_TURN, -- Gitee