diff --git a/src/component/comp_cmd.hpp b/src/component/comp_cmd.hpp index c43fda26a2da7852c06ee63c4952333c96b71edc..3db2763d72efd880e98e2f3e8280c8e95f0ad436 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; + Eulr eulr; GimbalCommandMode mode; } GimbalCMD; @@ -100,6 +102,7 @@ class CMD { static void SetCtrlSource(ControlSource source) { self_->ctrl_source_ = source; } + static uint8_t GetCtrlMode() { return self_->mode_; } private: bool online_ = false; diff --git a/src/component/comp_mixer.cpp b/src/component/comp_mixer.cpp index 35d18a506fc8517b46a56ff4ba1da0f80fd60154..3f9ec3b4851b4afd1fcaa1fc8a9b854e98f35c96 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 dfa9e091b4aa8cbb37da82188676148eef183dda..86ad8b1a63411751e055ac0ed7de2642a8430e9d 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; } @@ -133,34 +136,75 @@ bool AI::PackRef() { } bool AI::PackCMD() { - 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)); + + /* 控制权在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->ai_data_status_ = this->IS_INVALID_AMING_DATA; + } else { + this->ai_data_status_ = this->IS_USEFUL_AMING_DATA; + } + } else { + this->ai_data_status_ = this->AI_OFFLINE; + } + + /*AI云台数据无效,巡逻模式 */ + if (this->ai_data_status_ == this->IS_INVALID_AMING_DATA) { + this->event_.Active(AI_AUTOPATROL); + } + + /* AI转向 */ + /* 自瞄优先级高于转向 */ + 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开火发弹指令 */ + } + } + } + + memcpy(&(this->last_eulr_), &(this->cmd_.gimbal.eulr), + sizeof(this->cmd_.gimbal.eulr)); this->cmd_.ctrl_source = Component::CMD::CTRL_SOURCE_AI; this->cmd_tp_.Publish(this->cmd_); - return true; } 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; @@ -171,9 +215,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 97b75136faa0b27f66cd88cebea713814e02e2ef..426dcbb96088a103dc3e981896df61fc42ea6b2a 100644 --- a/src/device/ai/dev_ai.hpp +++ b/src/device/ai/dev_ai.hpp @@ -31,6 +31,18 @@ class AI { uint32_t hp; } RefForAI; + typedef enum { + IS_INVALID_AMING_DATA, + IS_USEFUL_AMING_DATA, + AI_OFFLINE, + } AIDataStatus; + typedef enum { + AI_FIND_TARGET = 128, + AI_AUTOPATROL, + AI_TURN, + AI_FIRE_COMMAND, + } AIControlData; + AI(); bool StartRecv(); @@ -51,9 +63,14 @@ class AI { private: bool ref_updated_ = false; + uint32_t last_online_time_ = 0; - Protocol_DownPackage_t form_host_{}; + Protocol_DownPackage_t from_host_{}; + + uint8_t ai_data_status_; + + uint8_t fire_command_; struct { RefereePckage ref{}; @@ -66,10 +83,14 @@ class AI { System::Semaphore data_ready_; + 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/device/custom_controller/dev_custom_controller.hpp b/src/device/custom_controller/dev_custom_controller.hpp index e1a7e9425ba8c6fd48c6ba7d0ac52d5857288eac..e12eb453addd07d54d7d05781bc59b8b2badbd30 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/device/referee/dev_referee.cpp b/src/device/referee/dev_referee.cpp index e7d2125fe3e80fce8c6c0ee77b99b235a994f692..9abf72587c54cf8fe39093c03735544858d6a665 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); @@ -250,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); @@ -258,6 +266,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,8 +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 41e751805aa739e707407ffaa2338f749c8d9a74..a7a76e1227b1ea0881dd7e209d479db92f80c55e 100644 --- a/src/device/referee/dev_referee.hpp +++ b/src/device/referee/dev_referee.hpp @@ -1,5 +1,6 @@ /* 裁判系统抽象。 + 2024_03_21 已修改。 */ #pragma once @@ -11,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) @@ -50,11 +54,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 +74,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, + 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, /* 选手端小地图接受机器人消息 */ } CommandID; typedef enum { @@ -98,11 +105,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 +117,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 +125,60 @@ 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)) { + 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 +188,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 +267,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 +275,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 +389,8 @@ class Referee { REF_STDNT_CMD_ID_UI_DRAW7 = 0x0104, REF_STDNT_CMD_ID_UI_STR = 0x0110, REF_STDNT_CMD_ID_CUSTOM = 0x0200, + REF_STDNT_CMD_ID_SENTRY_COMMAND = 0X0120, + REF_STDNT_CMD_ID_RADAR_COMMAND = 0X0121, } CMDID; typedef struct __attribute__((packed)) { @@ -373,32 +401,35 @@ 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 */ + 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 */ + MapRobotData map_robot_data; /* 0x0305 */ + CustomKeyMouseData custom_key_mouse_data; /* 0x0306 */ } Data; typedef struct __attribute__((packed)) { diff --git a/src/module/gimbal/mod_gimbal.cpp b/src/module/gimbal/mod_gimbal.cpp index 0d1613b6a309daacacf79fe190ab0bddbbe20232..fdf7dfc2d665d3f5d7cd67d4173b8fe0c5435988 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,19 @@ 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; + case SET_AI_TURN: + gimbal->SetMode(static_cast(TURN)); + break; } gimbal->ctrl_lock_.Post(); }; @@ -54,16 +57,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 +82,21 @@ Gimbal::Gimbal(Param& param, float control_freq) } void Gimbal::UpdateFeedback() { - this->pit_motor_.Update(); + this->pit_motor_.Update(); /* 电机*/ this->yaw_motor_.Update(); + 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()); - this->yaw_ = this->yaw_motor_.GetAngle() - this->param_.mech_zero.yaw; + break; + } } void Gimbal::Control() { @@ -121,23 +135,43 @@ void Gimbal::Control() { this->setpoint_.eulr_.pit += gimbal_pit_cmd; /* 控制相关逻辑 */ + float yaw_out = 0; + float pit_out = 0; + float autopatrol_yaw = 0; switch (this->mode_) { case RELAX: this->yaw_motor_.Relax(); this->pit_motor_.Relax(); break; case ABSOLUTE: + case TURN: /* 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->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_); + + this->yaw_motor_.Control(yaw_out); + this->pit_motor_.Control(pit_out); + break; } } diff --git a/src/module/gimbal/mod_gimbal.hpp b/src/module/gimbal/mod_gimbal.hpp index 0c65e0d941be8dd517ba376b861f7be6c5b44265..15c28a6921a1827189d91922af60ead89efbe61f 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,8 @@ class Gimbal { typedef enum { RELAX, /* 放松模式,电机不输出。一般情况云台初始化之后的模式 */ ABSOLUTE, /* 绝对坐标系控制,控制在空间内的绝对姿态 */ + AUTOPATROL, + TURN, } Mode; enum { @@ -39,7 +37,9 @@ class Gimbal { SET_MODE_RELAX, SET_MODE_ABSOLUTE, START_AUTO_AIM, - STOP_AUTO_AIM + STOP_AUTO_AIM, + SET_AUTOPATROL, + SET_AI_TURN, } GimbalEvent; typedef struct { @@ -54,6 +54,9 @@ class Gimbal { Component::Type::Eulr mech_zero; + float patrol_range; + float patrol_omega; + struct { Component::Type::CycleValue pitch_max; Component::Type::CycleValue pitch_min; @@ -84,7 +87,7 @@ class Gimbal { Param param_; - Gimbal::Mode mode_ = RELAX; /* 云台模式 */ + Gimbal::Mode mode_ = ABSOLUTE; /* 云台模式 */ struct { Component::Type::Eulr eulr_; /* 表示云台姿态的欧拉角 */ diff --git a/src/module/launcher/mod_launcher.cpp b/src/module/launcher/mod_launcher.cpp index 6d10de4ee72f961a7388dd6e58a6431cab73ed5e..a0b14fb1e6d68a41f81fa51cf275f35ec0f77222 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 = BULLET_SPEED_LIMIT_42MM; 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 = BULLET_SPEED_LIMIT_17MM; 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 9f8a12309ec00c8038a51f39c988f2c530062288..b7e0c773f1ddfef0d451dd526d142b413b11fb76 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, /* 总共的控制器数量 */ }; diff --git a/src/robot/sentry/robot.cpp b/src/robot/sentry/robot.cpp index 45e586b77588e4e37d9aba4a8a7ff5b3e431081f..4a9e64cf83d9dbbd7912674d92a725328c7b2741 100644 --- a/src/robot/sentry/robot.cpp +++ b/src/robot/sentry/robot.cpp @@ -7,12 +7,12 @@ #include "system.hpp" /* clang-format off */ -Robot::Infantry::Param param = { +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, @@ -29,14 +29,26 @@ 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 + }, + Component::CMD::EventMapItem{ + Device::AI::AI_TURN, Module::RMChassis::SET_MODE_ROTOR } }, @@ -44,7 +56,7 @@ Robot::Infantry::Param param = { .actuator_param = { Component::SpeedActuator::Param{ .speed = { - .k = 0.00015f, + .k = 0.00020f, .p = 1.0f, .i = 0.0f, .d = 0.0f, @@ -77,7 +89,7 @@ Robot::Infantry::Param param = { }, Component::SpeedActuator::Param{ .speed = { - .k = 0.00015f, + .k = 0.00020f, .p = 1.0f, .i = 0.0f, .d = 0.0f, @@ -93,7 +105,7 @@ Robot::Infantry::Param param = { }, Component::SpeedActuator::Param{ .speed = { - .k = 0.00015f, + .k = 0.00022f, .p = 1.0f, .i = 0.0f, .d = 0.0f, @@ -159,7 +171,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 +183,10 @@ Robot::Infantry::Param param = { .position = { /* GIMBAL_CTRL_YAW_ANGLE_IDX */ - .k = 20.0f, + .k = 5.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, @@ -188,7 +200,7 @@ Robot::Infantry::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, @@ -200,7 +212,7 @@ Robot::Infantry::Param param = { .position = { /* GIMBAL_CTRL_PIT_ANGLE_IDX */ - .k = 20.0f, + .k = 16.0f, .p = 1.0f, .i = 0.0f, .d = 0.0f, @@ -216,28 +228,31 @@ 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, }, .mech_zero = { - .yaw = 1.3f, + .yaw = 3.12f, .pit = 4.0f, .rol = 0.0f, }, + .patrol_range = 0.25, + .patrol_omega = 0.005, + .limit = { - .pitch_max = 3.8f, - .pitch_min = 3.0f, + .pitch_max = 0.57f, + .pitch_min = 0.23f, }, .EVENT_MAP = { @@ -250,25 +265,37 @@ 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 + }, + Component::CMD::EventMapItem{ + Device::AI::AI_TURN, + Module::Gimbal::SET_AI_TURN } }, }, - .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 +329,7 @@ Robot::Infantry::Param param = { }, .fric_actr = { - Component::SpeedActuator::Param{ + Component::SpeedActuator::Param{ .speed = { .k = 0.00015f, .p = 1.0f, @@ -338,7 +365,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,17 +374,17 @@ 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 = { @@ -370,24 +397,178 @@ 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, + 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 + }, + Component::CMD::EventMapItem{ + Device::AI::AIControlData::AI_FIRE_COMMAND, Module::Launcher::LAUNCHER_START_FIRE + }, + Component::CMD::EventMapItem{ + Device::AI::AIControlData::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, + }, }, - }, /* launcher */ + .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 = true, + }, + }, + + .fric_motor = { + Device::RMMotor::Param{ + .id_feedback = 0x202, + .id_control = M3508_M2006_CTRL_ID_BASE, + .model = Device::RMMotor::MOTOR_M3508, + .can = BSP_CAN_2, + .reverse = 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::CMD_EVENT_LOST_CTRL, + Module::Launcher::CHANGE_FIRE_MODE_RELAX + }, + Component::CMD::EventMapItem{ + Device::DR16::DR16_SW_R_POS_TOP, + Module::Launcher::CHANGE_FIRE_MODE_SAFE + }, + Component::CMD::EventMapItem{ + Device::DR16::DR16_SW_R_POS_MID,/* 模拟找到目标模式,云台绝对 */ + Module::Launcher::CHANGE_FIRE_MODE_LOADED + }, + Component::CMD::EventMapItem{ + Device::DR16::DR16_SW_R_POS_BOT,/* 模拟未找到目标,巡逻模式 */ + Module::Launcher::CHANGE_FIRE_MODE_LOADED + }, + Component::CMD::EventMapItem{ + Device::DR16::DR16_SW_R_POS_BOT,/* 模拟未找到目标,巡逻模式 */ + Module::Launcher::LAUNCHER_START_FIRE + }, + Component::CMD::EventMapItem{ + Device::AI::AIControlData::AI_FIND_TARGET, + Module::Launcher::CHANGE_FIRE_MODE_LOADED + }, + Component::CMD::EventMapItem{ + 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::AIControlData::AI_FIRE_COMMAND, + Module::Launcher::CHANGE_TRIG_MODE_SINGLE + } + + }, + }, /* launcher */ .bmi088_rot = { .rot_mat = { { +1, +0, +0}, @@ -405,5 +586,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 2c8444cef6922acefee7745d2f5aeed5ce9c8ee3..29b5d8a6dfa69eed3ace2f390c179b6bacfaf687 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