diff --git a/src/component/comp_cmd.cpp b/src/component/comp_cmd.cpp index 29edad79dcc68b563ff6afcf20479f653a4bfd10..5414467571d08b1021027244f59cfe9bd9ba3abf 100644 --- a/src/component/comp_cmd.cpp +++ b/src/component/comp_cmd.cpp @@ -45,7 +45,7 @@ CMD::CMD(Mode mode) if (!cmd->data_[CTRL_SOURCE_RC].online && cmd->online_) { cmd->event_.Active(CMD_EVENT_LOST_CTRL); cmd->online_ = false; - } else { + } else if (cmd->data_[CTRL_SOURCE_RC].online) { cmd->online_ = true; } diff --git a/src/component/comp_cmd.hpp b/src/component/comp_cmd.hpp index 47b9b989c9e28953a87463d045a88c1d6c0d8d49..b96c9c6d7dba6fd68a48ec689d583e36b41a9544 100644 --- a/src/component/comp_cmd.hpp +++ b/src/component/comp_cmd.hpp @@ -25,7 +25,11 @@ class CMD { typedef Type::Vector3 ChassisCMD; typedef struct { - Component::Type::Eulr eulr; + struct { + float yaw; /* 偏航角（Yaw angle） */ + float pit; /* 俯仰角（Pitch angle） */ + float rol; /* 翻滚角（Roll angle） */ + } eulr; GimbalCommandMode mode; } GimbalCMD; @@ -96,7 +100,9 @@ class CMD { static void SetCtrlSource(ControlSource source) { self_->ctrl_source_ = source; } - static uint8_t GetCtrlMode() { return self_->mode_; } + static ControlSource GetCtrlSource() { return self_->ctrl_source_; } + + static bool Online() { return self_->online_; } private: bool online_ = false; diff --git a/src/device/ai/dev_ai.cpp b/src/device/ai/dev_ai.cpp index 2cf8d72a21303fa9db1370d853b86826e59e140d..4e2fb2a8d67dccb938f51f6b1d844a74aa219b55 100644 --- a/src/device/ai/dev_ai.cpp +++ b/src/device/ai/dev_ai.cpp @@ -148,47 +148,37 @@ bool AI::PackCMD() { this->notice_ = this->from_host_.data.notice; - /* 控制权在AI */ - if (Component::CMD::GetCtrlMode() == Component::CMD::CMD_AUTO_CTRL) { - if (this->cmd_.online) { - if (this->last_eulr_.yaw == this->cmd_.gimbal.eulr.yaw && - this->last_eulr_.pit == this->cmd_.gimbal.eulr.pit) { - this->auto_aim_enable_ = false; - } else { - this->auto_aim_enable_ = true; - } - } + memcpy(&(this->last_eulr_), &(this->cmd_.gimbal.eulr), + sizeof(this->cmd_.gimbal.eulr)); - /*AI云台数据无效，巡逻模式 */ - if (!this->auto_aim_enable_) { - this->event_.Active(AI_AUTOPATROL); - } + this->cmd_.ctrl_source = Component::CMD::CTRL_SOURCE_AI; - /* AI转向 */ - /* 自瞄优先级高于转向 */ - if (!this->auto_aim_enable_ && 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); - } + this->cmd_tp_.Publish(this->cmd_); - /*AI云台数据有效，自动瞄准模式 */ - if (this->auto_aim_enable_) { - this->cmd_.gimbal.mode = Component::CMD::GIMBAL_ABSOLUTE_CTRL; - this->event_.Active(AI_FIND_TARGET); - if (notice_ == AI_NOTICE_FIRE) { - this->event_.Active(AI_FIRE_COMMAND); /* AI开火发弹指令 */ + if (!Component::CMD::Online()) { + return false; + } + + /* 控制权在AI */ + if (Component::CMD::GetCtrlSource() == Component::CMD::CTRL_SOURCE_AI) { + if (this->cmd_.online) { + if (this->notice_ == AI_NOTICE_AUTO_AIM) { + this->cmd_.gimbal.mode = Component::CMD::GIMBAL_ABSOLUTE_CTRL; + this->event_.Active(AI_FIND_TARGET); + } else if (this->notice_ == AI_NOTICE_FIRE) { + this->cmd_.gimbal.mode = Component::CMD::GIMBAL_ABSOLUTE_CTRL; + this->event_.Active(AI_FIND_TARGET); + this->event_.Active(AI_FIRE_COMMAND); + } else { + this->event_.Active(AI_AUTOPATROL); } + } else { + this->event_.Active(AI_OFFLINE); } } - memcpy(&(this->last_eulr_), &(this->cmd_.gimbal.eulr), - sizeof(this->cmd_.gimbal.eulr)); + this->notice_ = 0; - 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 07b17a5f672a0f068cccc4afe65d79e05df77d4e..bdbc56a69537efb1d55634d62e27f8317b101c40 100644 --- a/src/device/ai/dev_ai.hpp +++ b/src/device/ai/dev_ai.hpp @@ -32,7 +32,8 @@ class AI { } RefForAI; typedef enum { - AI_FIND_TARGET = 128, + AI_OFFLINE = 128, + AI_FIND_TARGET, AI_AUTOPATROL, AI_TURN, AI_FIRE_COMMAND, @@ -63,8 +64,6 @@ class AI { Protocol_DownPackage_t from_host_{}; - bool auto_aim_enable_; - uint8_t notice_; struct { @@ -84,7 +83,11 @@ class AI { Component::CMD::Data cmd_{}; - Component::Type::Eulr last_eulr_; + struct { + float yaw; /* 偏航角（Yaw angle） */ + float pit; /* 俯仰角（Pitch angle） */ + float rol; /* 翻滚角（Roll angle） */ + } last_eulr_; Component::Type::Quaternion quat_{}; Device::Referee::Data raw_ref_{}; diff --git a/src/module/chassis/mod_chassis.cpp b/src/module/chassis/mod_chassis.cpp index 74fb3e0759fa03dddb37a81def69d1e5b19849ba..524a4abbbfbbacaa57c5ef047df659de9e626239 100644 --- a/src/module/chassis/mod_chassis.cpp +++ b/src/module/chassis/mod_chassis.cpp @@ -214,7 +214,7 @@ void Chassis::Control() { if (cap_.online_) { percentage = cap_.percentage_; } else if (ref_.status == Device::Referee::RUNNING) { - percentage = this->ref_.chassis_pwr_buff / 30.0f; + percentage = this->ref_.chassis_pwr_buff / 60.0f; } else { percentage = 1.0f; } diff --git a/src/module/gimbal/mod_gimbal.cpp b/src/module/gimbal/mod_gimbal.cpp index 9ac70ee2bbae9eef3fc3c6d3c716eee6df37dd59..f715cbdf73fb2f2725fb12f436613c8a94d660ed 100644 --- a/src/module/gimbal/mod_gimbal.cpp +++ b/src/module/gimbal/mod_gimbal.cpp @@ -36,9 +36,6 @@ Gimbal::Gimbal(Param& param, float control_freq) case SET_AUTOPATROL: gimbal->SetMode(static_cast(AUTOPATROL)); break; - case SET_AI_TURN: - gimbal->SetMode(static_cast(TURN)); - break; } gimbal->ctrl_lock_.Post(); }; @@ -87,13 +84,15 @@ 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: this->yaw_ = this->yaw_motor_.GetAngle() - this->param_.mech_zero.yaw - this->param_.patrol_range * - sin(this->param_.patrol_omega * bsp_time_get_ms()); + sinf(this->param_.patrol_omega * + static_cast(bsp_time_get_ms() - + autopatrol_start_time_) / + 1000.0f); break; } @@ -144,7 +143,6 @@ void Gimbal::Control() { this->pit_motor_.Relax(); break; case ABSOLUTE: - case TURN: /* Yaw轴角速度环参数计算 */ yaw_out = this->yaw_actuator_.Calculate( this->setpoint_.eulr_.yaw, this->gyro_.z, this->eulr_.yaw, this->dt_); @@ -159,15 +157,17 @@ void Gimbal::Control() { case AUTOPATROL: /* 以sin变化左右摆头 */ - 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()); + sinf(this->param_.patrol_omega * + static_cast(bsp_time_get_ms() - + autopatrol_start_time_) / + 1000.0f); 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_); + this->param_.patrol_hight, this->gyro_.x, this->eulr_.pit, this->dt_); this->yaw_motor_.Control(yaw_out); this->pit_motor_.Control(pit_out); @@ -191,6 +191,11 @@ void Gimbal::SetMode(Mode mode) { this->setpoint_.eulr_.yaw = this->eulr_.yaw; } } + + if (mode == AUTOPATROL) { + autopatrol_start_time_ = bsp_time_get_ms(); + } + this->mode_ = mode; } diff --git a/src/module/gimbal/mod_gimbal.hpp b/src/module/gimbal/mod_gimbal.hpp index c78621be577f6e731fbbeb152b691af17f262c76..be0abefd5306b455399acc3f60974b594e12bdce 100644 --- a/src/module/gimbal/mod_gimbal.hpp +++ b/src/module/gimbal/mod_gimbal.hpp @@ -22,7 +22,6 @@ class Gimbal { RELAX, /* 放松模式，电机不输出。一般情况云台初始化之后的模式 */ ABSOLUTE, /* 绝对坐标系控制，控制在空间内的绝对姿态 */ AUTOPATROL, /* 自动巡逻模式，云台yaw轴按sin曲线进行扫描 */ - TURN, /* AI转弯模式，采用相对控制 */ } Mode; enum { @@ -39,7 +38,6 @@ class Gimbal { START_AUTO_AIM, STOP_AUTO_AIM, SET_AUTOPATROL, - SET_AI_TURN, } GimbalEvent; typedef struct { @@ -56,6 +54,7 @@ class Gimbal { float patrol_range; float patrol_omega; + float patrol_hight; struct { Component::Type::CycleValue pitch_max; @@ -85,6 +84,8 @@ class Gimbal { float dt_ = 0.0f; + uint32_t autopatrol_start_time_ = 0; + Param param_; Gimbal::Mode mode_ = RELAX; /* 云台模式 */ diff --git a/src/module/launcher/mod_launcher.cpp b/src/module/launcher/mod_launcher.cpp index a0f48d3dd37bb007cc069079476be1ec4752a0a0..5fb4c8f195b93be0d844dcd39ab9d3927bafe608 100644 --- a/src/module/launcher/mod_launcher.cpp +++ b/src/module/launcher/mod_launcher.cpp @@ -29,11 +29,15 @@ Launcher::Launcher(Param& param, float control_freq) auto event_callback = [](LauncherEvent event, Launcher* launcher) { launcher->ctrl_lock_.Wait(UINT32_MAX); - switch (event) { /* 根据event设置模式 */ + switch (event) { case CHANGE_FIRE_MODE_RELAX: + launcher->SetFireMode(static_cast(RELAX)); + break; case CHANGE_FIRE_MODE_SAFE: + launcher->SetFireMode(static_cast(SAFE)); + break; case CHANGE_FIRE_MODE_LOADED: - launcher->SetFireMode(static_cast(event)); + launcher->SetFireMode(static_cast(LOADED)); break; case LAUNCHER_START_FIRE: /* 摩擦轮开启条件下，开火控制fire为ture */ if (launcher->fire_ctrl_.fire_mode_ == LOADED) { @@ -42,8 +46,10 @@ Launcher::Launcher(Param& param, float control_freq) break; case CHANGE_TRIG_MODE_SINGLE: + launcher->SetTrigMode(static_cast(SINGLE)); + break; case CHANGE_TRIG_MODE_BURST: - launcher->SetTrigMode(static_cast(event)); + launcher->SetTrigMode(static_cast(CONTINUED)); break; case CHANGE_TRIG_MODE: launcher->SetTrigMode(static_cast( diff --git a/src/robot/sentry/robot.cpp b/src/robot/sentry/robot.cpp index 4a9e64cf83d9dbbd7912674d92a725328c7b2741..c3548a8d5ff134c56c5e35d3fbcee49efdc3840f 100644 --- a/src/robot/sentry/robot.cpp +++ b/src/robot/sentry/robot.cpp @@ -33,12 +33,16 @@ Robot::Sentry::Param param = { }, Component::CMD::EventMapItem{ Device::DR16::DR16_SW_L_POS_MID,/* 模拟找到目标模式，云台绝对 */ - Module::RMChassis::SET_MODE_ROTOR, + Module::RMChassis::SET_MODE_FOLLOW, }, Component::CMD::EventMapItem{ Device::DR16::DR16_SW_L_POS_BOT,/* 模拟未找到目标，巡逻模式 */ Module::RMChassis::SET_MODE_ROTOR, }, + Component::CMD::EventMapItem{ + Device::AI::AI_OFFLINE, + Module::RMChassis::SET_MODE_ROTOR + }, Component::CMD::EventMapItem{ Device::AI::AI_FIND_TARGET, Module::RMChassis::SET_MODE_ROTOR @@ -171,9 +175,9 @@ Robot::Sentry::Param param = { .yaw_actr = { .speed = { /* GIMBAL_CTRL_YAW_OMEGA_IDX */ - .k = 0.6f, - .p = 1.f, - .i = 3.f, + .k = 0.7f, + .p = 1.0f, + .i = 0.3f, .d = 0.f, .i_limit = 0.2f, .out_limit = 1.0f, @@ -183,12 +187,12 @@ Robot::Sentry::Param param = { .position = { /* GIMBAL_CTRL_YAW_ANGLE_IDX */ - .k = 5.0f, + .k = 20.0f, .p = 1.0f, .i = 0.0f, - .d = 1.4f, + .d = 0.0f, .i_limit = 0.0f, - .out_limit = 10.0f, + .out_limit = 20.0f, .d_cutoff_freq = -1.0f, .cycle = true, }, @@ -200,7 +204,7 @@ Robot::Sentry::Param param = { .pit_actr = { .speed = { /* GIMBAL_CTRL_PIT_OMEGA_IDX */ - .k = 0.25f, + .k = 0.2f, .p = 1.0f, .i = 0.f, .d = 0.f, @@ -212,7 +216,7 @@ Robot::Sentry::Param param = { .position = { /* GIMBAL_CTRL_PIT_ANGLE_IDX */ - .k = 16.0f, + .k = 20.0f, .p = 1.0f, .i = 0.0f, .d = 0.0f, @@ -243,16 +247,17 @@ Robot::Sentry::Param param = { .mech_zero = { .yaw = 3.12f, - .pit = 4.0f, + .pit = 0.50f, .rol = 0.0f, }, - .patrol_range = 0.25, - .patrol_omega = 0.005, + .patrol_range = 0.4f, + .patrol_omega = 2.0f, + .patrol_hight = 6.0, .limit = { - .pitch_max = 0.57f, - .pitch_min = 0.23f, + .pitch_max = 0.60f, + .pitch_min = 0.19f, }, .EVENT_MAP = { @@ -272,6 +277,10 @@ Robot::Sentry::Param param = { Device::DR16::DR16_SW_L_POS_BOT,/* 模拟未找到目标，巡逻模式 */ Module::Gimbal::SET_AUTOPATROL }, + Component::CMD::EventMapItem{ + Device::AI::AI_OFFLINE, + Module::Gimbal::SET_AUTOPATROL + }, Component::CMD::EventMapItem{ Device::AI::AI_FIND_TARGET, Module::Gimbal::SET_MODE_ABSOLUTE @@ -279,10 +288,6 @@ Robot::Sentry::Param param = { Component::CMD::EventMapItem{ Device::AI::AI_AUTOPATROL, Module::Gimbal::SET_AUTOPATROL - }, - Component::CMD::EventMapItem{ - Device::AI::AI_TURN, - Module::Gimbal::SET_AI_TURN } }, @@ -409,12 +414,8 @@ Robot::Sentry::Param param = { Module::Launcher::LAUNCHER_START_FIRE }, Component::CMD::EventMapItem{ - Device::AI::AIControlData::AI_FIND_TARGET, - Module::Launcher::CHANGE_FIRE_MODE_LOADED - }, - Component::CMD::EventMapItem{ - Device::AI::AIControlData::AI_FIRE_COMMAND, - Module::Launcher::CHANGE_FIRE_MODE_LOADED + Device::AI::AIControlData::AI_AUTOPATROL, + Module::Launcher::CHANGE_FIRE_MODE_RELAX }, Component::CMD::EventMapItem{ Device::AI::AIControlData::AI_FIRE_COMMAND, @@ -551,12 +552,8 @@ Robot::Sentry::Param param = { Module::Launcher::LAUNCHER_START_FIRE }, Component::CMD::EventMapItem{ - Device::AI::AIControlData::AI_FIND_TARGET, - Module::Launcher::CHANGE_FIRE_MODE_LOADED - }, - Component::CMD::EventMapItem{ - Device::AI::AIControlData::AI_FIRE_COMMAND, - Module::Launcher::CHANGE_FIRE_MODE_LOADED + Device::AI::AIControlData::AI_AUTOPATROL, + Module::Launcher::CHANGE_FIRE_MODE_RELAX }, Component::CMD::EventMapItem{ Device::AI::AIControlData::AI_FIRE_COMMAND, diff --git a/src/robot/sentry/robot.hpp b/src/robot/sentry/robot.hpp index 29b5d8a6dfa69eed3ace2f390c179b6bacfaf687..0694c30195320030e130184c96bcd19d2132026a 100644 --- a/src/robot/sentry/robot.hpp +++ b/src/robot/sentry/robot.hpp @@ -40,7 +40,7 @@ class Sentry { Module::Launcher launcher1_; Module::Launcher launcher2_; Sentry(Param& param, float control_freq) - : cmd_(Component::CMD::CMD_AUTO_CTRL), + : cmd_(Component::CMD::CMD_OP_CTRL), bmi088_(param.bmi088_rot), cap_(param.cap), chassis_(param.chassis, control_freq),