Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

update referee system protocol to 1.6.4 #76

Merged
merged 2 commits into from
Aug 1, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions examples/referee/main.cc
Original file line number Diff line number Diff line change
Expand Up @@ -89,6 +89,7 @@ void RM_RTOS_Default_Task(const void* argument) {
print("Chassis Volt: %.3f\r\n", referee->power_heat_data.chassis_volt / 1000.0);
print("Chassis Curr: %.3f\r\n", referee->power_heat_data.chassis_current / 1000.0);
print("Chassis Power: %.3f\r\n", referee->power_heat_data.chassis_power);
print("Chassis Power Limit: %.3f\r\n", (float)(referee->game_robot_status.chassis_power_limit));
print("\r\n");
print("Shooter Cooling Heat: %hu\r\n", referee->power_heat_data.shooter_id1_17mm_cooling_heat);
print("Bullet Frequency: %hhu\r\n", referee->shoot_data.bullet_freq);
Expand Down
13 changes: 13 additions & 0 deletions shared/libraries/protocol.cc
Original file line number Diff line number Diff line change
Expand Up @@ -134,6 +134,19 @@ bool Referee::ProcessDataRx(int cmd_id, const uint8_t* data, int length) {
case DART_CLIENT_CMD:
memcpy(&dart_client_cmd, data, length);
break;
case GROUND_ROBOT_POSITION:
memcpy(&ground_robot_position, data, length);
break;
case RADAR_MARK_PROGRESS:
memcpy(&radar_mark_progress, data, length);
break;
case SENTRY_INFO:
memcpy(&sentry_info, data, length);
break;
case RADAR_INFO:
memcpy(&radar_info, data, length);
break;

default:
return false;
}
Expand Down
74 changes: 60 additions & 14 deletions shared/libraries/protocol.h
Original file line number Diff line number Diff line change
Expand Up @@ -160,7 +160,12 @@ typedef enum {
BULLET_REMAINING = 0x0208,
RFID_STATUS = 0x0209,
DART_CLIENT_CMD = 0x020A,
GROUND_ROBOT_POSITION = 0x020B,
RADAR_MARK_PROGRESS = 0x020C,
SENTRY_INFO = 0x020D,
RADAR_INFO = 0x020E,
STUDENT_INTERACTIVE = 0x0301,

} referee_cmd;

/* ===== GAME_STATUS 0x0001 1Hz ===== */
Expand Down Expand Up @@ -218,11 +223,13 @@ typedef struct {
typedef struct {
uint8_t level;
uint8_t foul_robot_id;
uint8_t count;
} __packed referee_warning_t;

/* ===== DART_REMAINING_TIME 0x0105 1Hz ===== */
typedef struct {
uint8_t dart_remaining_time;
uint16_t dart_info;
} __packed dart_remaining_time_t;

/* ===== GAME_ROBOT_STATUS 0x0201 10Hz ===== */
Expand All @@ -232,17 +239,8 @@ typedef struct {
uint16_t remain_HP;
uint16_t max_HP;

uint16_t shooter_id1_17mm_cooling_rate;
uint16_t shooter_id1_17mm_cooling_limit;
uint16_t shooter_id1_17mm_speed_limit;

uint16_t shooter_id2_17mm_cooling_rate;
uint16_t shooter_id2_17mm_cooling_limit;
uint16_t shooter_id2_17mm_speed_limit;

uint16_t shooter_id1_42mm_cooling_rate;
uint16_t shooter_id1_42mm_cooling_limit;
uint16_t shooter_id1_42mm_speed_limit;
uint16_t shooter_barrel_cooling_value;
uint16_t shooter_barrel_heat_limit;

uint16_t chassis_power_limit;
uint8_t mains_power_gimbal_output : 1;
Expand All @@ -265,18 +263,22 @@ typedef struct {
typedef struct {
float x;
float y;
float z;
float yaw;
} __packed game_robot_pos_t;

/* ===== BUFF 0x0204 1Hz ===== */
typedef struct {
uint8_t power_rune_buff;
uint8_t recovery_buff;
uint8_t cooling_buff;
uint8_t defence_buff;
uint8_t vulnerability_buff;
uint16_t attack_buff;
} __packed buff_t;

/* ===== AERIAL_ROBOT_ENERGY 0x0205 10Hz ===== */
typedef struct {
uint8_t attack_time;
uint8_t airforce_status;
uint8_t time_remain;
} __packed aerial_robot_energy_t;

/* ===== ROBOT_HURT 0x0206 ===== */
Expand Down Expand Up @@ -313,6 +315,46 @@ typedef struct {
uint16_t operate_launch_cmd_time;
} __packed dart_client_cmd_t;


/* ====== GROUND_ROBOT_POSITION 0x020B 1Hz ====== */
typedef struct {
float hero_x;
float hero_y;
float engiener_x;
float engineer_y;
float standard_3_x;
float standard_3_y;
float standard_4_x;
float standard_4_y;
float standard_5_x;
float standard_5_y;

} __packed ground_robot_position_t;


/* ====== RADAR_MARK_PROGRESS 0x020C 1Hz ======= */
typedef struct
{
uint8_t mark_hero_progress;
uint8_t mark_engineer_progress;
uint8_t mark_standard_3_progress;
uint8_t mark_standard_4_progress;
uint8_t mark_standard_5_progress;
uint8_t mark_sentry_progress;
} __packed radar_mark_data_t;

/* ====== SENTRY_INFO 0x020D 1Hz ======= */
typedef struct
{
uint32_t sentry_info;
uint16_t sentry_info_2;
} __packed sentry_info_t;

/* ====== RADAR_INFO 0x020E 1Hz ======= */
typedef struct {
uint8_t radar_info;
} __packed radar_info_t;

typedef struct {
uint16_t data_cmd_id;
uint16_t sender_ID;
Expand Down Expand Up @@ -401,6 +443,10 @@ class Referee : public Protocol {
bullet_remaining_t bullet_remaining{};
rfid_status_t rfid_status{};
dart_client_cmd_t dart_client_cmd{};
ground_robot_position_t ground_robot_position{};
radar_mark_data_t radar_mark_progress{};
sentry_info_t sentry_info{};
radar_info_t radar_info{};

graphic_delete_t graphic_delete{};
graphic_single_t graphic_single{};
Expand Down
12 changes: 6 additions & 6 deletions vehicles/Fortress/main.cc
Original file line number Diff line number Diff line change
Expand Up @@ -440,17 +440,17 @@ void shooterTask(void* arg) {

if (referee->game_robot_status.mains_power_shooter_output &&
referee->power_heat_data.shooter_id1_17mm_cooling_heat <
referee->game_robot_status.shooter_id1_17mm_cooling_limit - 20 &&
referee->game_robot_status.shooter_barrel_cooling_value - 20 &&
(dbus->mouse.l || dbus->swr == remote::UP))
shooter->LoadNext();
if (!referee->game_robot_status.mains_power_shooter_output || dbus->keyboard.bit.Q ||
dbus->swr == remote::DOWN) {
flywheelFlag = false;
shooter->SetFlywheelSpeed(0);
} else if (referee->game_robot_status.shooter_id1_17mm_speed_limit == 15) {
} else if (referee->game_robot_status.shooter_barrel_heat_limit == 15) {
flywheelFlag = true;
shooter->SetFlywheelSpeed(440); // 445 MAX
} else if (referee->game_robot_status.shooter_id1_17mm_speed_limit >= 18) {
} else if (referee->game_robot_status.shooter_barrel_heat_limit >= 18) {
flywheelFlag = true;
shooter->SetFlywheelSpeed(485); // 490 MAX
} else {
Expand Down Expand Up @@ -1219,13 +1219,13 @@ void RM_RTOS_Default_Task(const void* arg) {
print("\r\n");

print("Shooter Heat: %hu / %d\r\n", referee->power_heat_data.shooter_id1_17mm_cooling_heat,
referee->game_robot_status.shooter_id1_17mm_cooling_limit);
referee->game_robot_status.shooter_barrel_cooling_value);
print("Bullet Speed: %.3f / %d\r\n", referee->shoot_data.bullet_speed,
referee->game_robot_status.shooter_id1_17mm_speed_limit);
referee->game_robot_status.shooter_barrel_heat_limit);
print("Bullet Frequency: %hhu\r\n", referee->shoot_data.bullet_freq);

if (referee->shoot_data.bullet_speed >
referee->game_robot_status.shooter_id1_17mm_speed_limit)
referee->game_robot_status.shooter_barrel_heat_limit)
pass = false;
print("\r\nSpeed Limit Test: %s\r\n", pass ? "PASS" : "FAIL");
}
Expand Down
11 changes: 2 additions & 9 deletions vehicles/Steering/chassis.cc
Original file line number Diff line number Diff line change
Expand Up @@ -341,19 +341,12 @@ void chassisTask(void* arg) {
receive->TransmitOutput();

receive->cmd.id = bsp::COOLING_LIMIT1;
receive->cmd.data_float = (float)referee->game_robot_status.shooter_id1_17mm_cooling_limit;
receive->cmd.data_float = (float)referee->game_robot_status.shooter_barrel_cooling_value;
receive->TransmitOutput();

receive->cmd.id = bsp::COOLING_LIMIT2;
receive->cmd.data_float = (float)referee->game_robot_status.shooter_id2_17mm_cooling_limit;
receive->TransmitOutput();

receive->cmd.id = bsp::SPEED_LIMIT1;
receive->cmd.data_float = (float)referee->game_robot_status.shooter_id1_17mm_speed_limit;
receive->TransmitOutput();

receive->cmd.id = bsp::SPEED_LIMIT2;
receive->cmd.data_float = (float)referee->game_robot_status.shooter_id2_17mm_speed_limit;
receive->cmd.data_float = (float)referee->game_robot_status.shooter_barrel_heat_limit;
receive->TransmitOutput();

osDelay(CHASSIS_TASK_DELAY);
Expand Down
Loading