diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index 22ebe57..20b8468 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -44,7 +44,7 @@ static Chassis_Ctrl_Cmd_s chassis_cmd_recv; // 底盘接收到的控制 static Chassis_Upload_Data_s chassis_feedback_data; // 底盘回传的反馈数据 static referee_info_t* referee_data; // 用于获取裁判系统的数据 -static Referee_Interactive_info_t ui_data; // UI数据,将底盘中的数据传入此结构体的对应变量中,UI会自动检测是否变化,对应显示UI + static SuperCapInstance *cap; // 超级电容 static DJIMotorInstance *motor_lf, *motor_rf, *motor_lb, *motor_rb; // left right forward back @@ -110,7 +110,7 @@ void ChassisInit() chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL; motor_rb = DJIMotorInit(&chassis_motor_config); - referee_data = UITaskInit(&huart6,&ui_data); // 裁判系统初始化,会同时初始化UI + referee_data = UITaskInit(&huart6, &ui_data); // 裁判系统初始化,会同时初始化UI SuperCap_Init_Config_s cap_conf = { .can_config = { @@ -290,6 +290,7 @@ void ChassisTask() // // 获取裁判系统数据 建议将裁判系统与底盘分离,所以此处数据应使用消息中心发送 // // 我方颜色id小于7是红色,大于7是蓝色,注意这里发送的是对方的颜色, 0:blue , 1:red chassis_feedback_data.enemy_color = !referee_data->referee_id.Robot_Color; + // // 当前只做了17mm热量的数据获取,后续根据robot_def中的宏切换双枪管和英雄42mm的情况 // chassis_feedback_data.bullet_speed = referee_data->GameRobotState.shooter_id1_17mm_speed_limit; // chassis_feedback_data.rest_heat = referee_data->PowerHeatData.shooter_heat0; diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index 49ab7e0..66f2be0 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -3,6 +3,7 @@ #include "robot_cmd.h" // module #include "remote_control.h" +#include "referee_task.h" #include "ins_task.h" #include "master_process.h" #include "message_center.h" @@ -54,6 +55,7 @@ static Shoot_Upload_Data_s shoot_fetch_data; // 从发射获取的反馈信息 static Robot_Status_e robot_state; // 机器人整体工作状态 extern float horizontal_angle; +int target_index = -1; void RobotCMDInit() { @@ -118,7 +120,13 @@ static void CalcOffsetAngle() // else // chassis_cmd_send.offset_angle = chassis_cmd_send.offset_angle; } - +static void update_ui_data() +{ + ui_data.chassis_mode = chassis_cmd_send.chassis_mode; + ui_data.gimbal_mode = gimbal_cmd_send.gimbal_mode; + ui_data.friction_mode = shoot_cmd_send.friction_mode; + ui_data.shoot_mode = shoot_cmd_send.shoot_mode; +} static void pitch_limit() { if(gimbal_fetch_data.pitch_motor_total_angle > horizontal_angle + 1500) @@ -132,6 +140,7 @@ static void pitch_limit() */ static void RemoteControlSet() { + aim_select.suggest_fire = 0; // 控制底盘和云台运行模式,云台待添加,云台是否始终使用IMU数据? if (switch_is_down(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[下],小陀螺 { @@ -158,7 +167,7 @@ static void RemoteControlSet() } // 左侧开关状态为[下],视觉模式 if (switch_is_down(rc_data[TEMP].rc.switch_left)) { - trajectory_cal.v0 = 30; //弹速30 + trajectory_cal.v0 = 12.5; //弹速30 if (vision_recv_data->x == 0 && vision_recv_data->y == 0 && vision_recv_data->z == 0 && vision_recv_data->vx == 0 && vision_recv_data->vy == 0 && vision_recv_data->vz == 0) { aim_select.suggest_fire = 0; @@ -176,7 +185,8 @@ static void RemoteControlSet() no_find_cnt = 0; auto_aim_flag = 1; - int target_index = auto_aim(&aim_select, &trajectory_cal, vision_recv_data); + target_index = auto_aim(&aim_select, &trajectory_cal, vision_recv_data); + VisionSetAim(aim_select.aim_point[0],aim_select.aim_point[1],aim_select.aim_point[2]); gimbal_cmd_send.yaw = -trajectory_cal.cmd_yaw * 180 / PI; @@ -221,7 +231,6 @@ static void RemoteControlSet() else shoot_cmd_send.load_mode = LOAD_STOP; // 射频控制,固定每秒1发,后续可以根据左侧拨轮的值大小切换射频, - shoot_cmd_send.shoot_rate = 8; } /** @@ -230,37 +239,31 @@ static void RemoteControlSet() */ static void MouseKeySet() { - chassis_cmd_send.vx = rc_data[TEMP].key[KEY_PRESS].a * 7000 - rc_data[TEMP].key[KEY_PRESS].d * 7000; // 系数待测 - chassis_cmd_send.vy = rc_data[TEMP].key[KEY_PRESS].w * 7000 - rc_data[TEMP].key[KEY_PRESS].s * 7000; + chassis_cmd_send.vx = rc_data[TEMP].key[KEY_PRESS].a * 6000 - rc_data[TEMP].key[KEY_PRESS].d * 6000; // 系数待测 + chassis_cmd_send.vy = rc_data[TEMP].key[KEY_PRESS].w * 6000 - rc_data[TEMP].key[KEY_PRESS].s * 6000; gimbal_cmd_send.yaw += (float)rc_data[TEMP].mouse.x / 660 * 7; // 系数待测 gimbal_cmd_send.pitch += (float)rc_data[TEMP].mouse.y / 660 * 7; pitch_limit(); - switch (rc_data[TEMP].key_count[KEY_PRESS][Key_Z] % 3) // Z键设置弹速 +// switch (rc_data[TEMP].key_count[KEY_PRESS][Key_Z] % 3) // Z键设置弹速// +// { +// case 0: +// shoot_cmd_send.bullet_speed = 15; +// break; +// case 1: +// shoot_cmd_send.bullet_speed = 18; +// break; +// default: +// shoot_cmd_send.bullet_speed = 30; +// break; +// } + switch (rc_data[TEMP].mouse.press_l) // 左键发射模式 { - case 0: - shoot_cmd_send.bullet_speed = 15; - break; - case 1: - shoot_cmd_send.bullet_speed = 18; - break; - default: - shoot_cmd_send.bullet_speed = 30; - break; - } - switch (rc_data[TEMP].key_count[KEY_PRESS][Key_E] % 4) // E键设置发射模式 - { - case 0: - shoot_cmd_send.load_mode = LOAD_STOP; - break; case 1: shoot_cmd_send.load_mode = LOAD_1_BULLET; break; - case 2: - shoot_cmd_send.load_mode = LOAD_3_BULLET; - break; default: - shoot_cmd_send.load_mode = LOAD_BURSTFIRE; + shoot_cmd_send.load_mode = LOAD_STOP; break; } switch (rc_data[TEMP].key_count[KEY_PRESS][Key_R] % 2) // R键开关弹舱 @@ -349,7 +352,8 @@ void RobotCMDTask() #endif // GIMBAL_BOARD SubGetMessage(shoot_feed_sub, &shoot_fetch_data); SubGetMessage(gimbal_feed_sub, &gimbal_fetch_data); - + //更新UI + update_ui_data(); // 根据gimbal的反馈值计算云台和底盘正方向的夹角,不需要传参,通过static私有变量完成 CalcOffsetAngle(); // 根据遥控器左侧开关,确定当前使用的控制模式为遥控器调试还是键鼠 diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index 9d04e7e..db5a125 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -168,5 +168,6 @@ void GimbalTask() gimbal_feedback_data.yaw_motor_single_round_angle = yaw_motor->measure.angle_single_round; gimbal_feedback_data.pitch_motor_total_angle = pitch_motor->measure.total_angle; // 推送消息 + PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data); } \ No newline at end of file diff --git a/application/shoot/shoot.c b/application/shoot/shoot.c index 3787e92..e23f7d5 100644 --- a/application/shoot/shoot.c +++ b/application/shoot/shoot.c @@ -4,6 +4,7 @@ #include "dji_motor.h" #include "message_center.h" #include "bsp_dwt.h" +#include "referee_task.h" #include "general_def.h" @@ -191,30 +192,9 @@ void ShootTask() // 确定是否开启摩擦轮,后续可能修改为键鼠模式下始终开启摩擦轮(上场时建议一直开启) if (shoot_cmd_recv.friction_mode == FRICTION_ON) { - // 根据收到的弹速设置设定摩擦轮电机参考值,需实测后填入 - switch (shoot_cmd_recv.bullet_speed) - { - case BIG_AMU_10: - DJIMotorSetRef(friction_l, 10); - DJIMotorSetRef(friction_r, 10); - DJIMotorSetRef(friction_z, 10); - break; - case BIG_AMU_16: - DJIMotorSetRef(friction_l, 16); - DJIMotorSetRef(friction_r, 16); - DJIMotorSetRef(friction_z, 16); - break; - case BULLET_SPEED_NONE: DJIMotorSetRef(friction_l, 39000); DJIMotorSetRef(friction_r, 39000); DJIMotorSetRef(friction_z, 39000); - break; - default: // 当前为了调试设定的默认值4000,因为还没有加入裁判系统无法读取弹速. - DJIMotorSetRef(friction_l, 0); - DJIMotorSetRef(friction_r, 0); - DJIMotorSetRef(friction_z, 0); - break; - } } else // 关闭摩擦轮 { @@ -236,6 +216,7 @@ void ShootTask() // 反馈数据,目前暂时没有要设定的反馈数据,后续可能增加应用离线监测以及卡弹反馈 //推送消息 + //ui_data.friction_mode = shoot_cmd_recv.friction_mode; PubPushMessage(shoot_pub, (void *)&shoot_feedback_data); } \ No newline at end of file diff --git a/modules/auto_aim/auto_aim.c b/modules/auto_aim/auto_aim.c index 45d8bad..a9e2bec 100644 --- a/modules/auto_aim/auto_aim.c +++ b/modules/auto_aim/auto_aim.c @@ -10,7 +10,7 @@ * @param[in] trajectory_cal:弹道解算结构体 * @retval 返回空 */ -void aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal) { +int aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal) { aim_sel->delay_time = trajectory_cal->fly_time + trajectory_cal->extra_delay_time; aim_sel->target_state.yaw += aim_sel->target_state.v_yaw * aim_sel->delay_time; @@ -190,9 +190,12 @@ void aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_ } float r = use_1 ? aim_sel->target_state.r2 : aim_sel->target_state.r1; - aim_sel->aim_point[0] = aim_sel->armor_pose[idx].x * ((center_length - r)/center_length); - aim_sel->aim_point[1] = aim_sel->armor_pose[idx].y + ((center_length - r)/center_length); - aim_sel->aim_point[2] = aim_sel->armor_pose[idx].z + ((center_length - r)/center_length); + + aim_sel->aim_point[0] = aim_sel->target_state.x * ((center_length - r) / center_length); + aim_sel->aim_point[1] = aim_sel->target_state.y * ((center_length - r) / center_length); + aim_sel->aim_point[2] = aim_sel->target_state.z * ((center_length - r) / center_length); + + return idx; } /** @@ -260,8 +263,8 @@ void get_cmd_angle(Trajectory_Type_t *trajectory_cal) { trajectory_cal->cmd_pitch = trajectory_cal->theta_k; } -void auto_aim(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal, RecievePacket_t *receive_packet) { - trajectory_cal->extra_delay_time = 0.035;//0.025 +int auto_aim(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal, RecievePacket_t *receive_packet) { + trajectory_cal->extra_delay_time = 0.025f;//0.025 aim_sel->target_state.armor_type = receive_packet->id; aim_sel->target_state.armor_num = receive_packet->armors_num; @@ -277,7 +280,7 @@ void auto_aim(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal, Rec aim_sel->target_state.r2 = receive_packet->r2; aim_sel->target_state.dz = receive_packet->dz; - aim_armor_select(aim_sel, trajectory_cal); + int idx = aim_armor_select(aim_sel, trajectory_cal); trajectory_cal->position_xy[0] = aim_sel->aim_point[0]; trajectory_cal->position_xy[1] = aim_sel->aim_point[1]; @@ -287,5 +290,5 @@ void auto_aim(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal, Rec trajectory_cal->velocity[2] = aim_sel->target_state.vz; get_cmd_angle(trajectory_cal); - + return idx; } diff --git a/modules/auto_aim/auto_aim.h b/modules/auto_aim/auto_aim.h index 1bcf134..bb0c47e 100644 --- a/modules/auto_aim/auto_aim.h +++ b/modules/auto_aim/auto_aim.h @@ -69,9 +69,9 @@ typedef struct }Aim_Select_Type_t; -void aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal); +int aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal); float get_fly_time(float x, float vx, float v_x0); void get_cmd_angle(Trajectory_Type_t *trajectory_cal); -void auto_aim(Aim_Select_Type_t *aim_sel,Trajectory_Type_t *trajectory_cal,RecievePacket_t *receive_packet); +int auto_aim(Aim_Select_Type_t *aim_sel,Trajectory_Type_t *trajectory_cal,RecievePacket_t *receive_packet); #endif //BASIC_FRAMEWORK_AUTO_AIM_H \ No newline at end of file diff --git a/modules/master_machine/master_process.c b/modules/master_machine/master_process.c index 871aab1..02de012 100644 --- a/modules/master_machine/master_process.c +++ b/modules/master_machine/master_process.c @@ -172,7 +172,7 @@ void VisionSend() send_data.header = 0x5A; // VisionSetFlag(1); - VisionSetAim(recv_data.x,recv_data.y,recv_data.z); + //VisionSetAim(recv_data.x,recv_data.y,recv_data.z); send_data.checksum = crc_16(&send_data.header,sizeof(send_data)-2); memcpy(send_buffer,&send_data,sizeof(send_data)); diff --git a/modules/referee/referee_protocol.h b/modules/referee/referee_protocol.h index 0070412..22416b3 100644 --- a/modules/referee/referee_protocol.h +++ b/modules/referee/referee_protocol.h @@ -16,8 +16,8 @@ /****************************宏定义部分****************************/ #define REFEREE_SOF 0xA5 // 起始字节,协议固定为0xA5 -#define Robot_Red 1 -#define Robot_Blue 0 +#define Robot_Red 0 +#define Robot_Blue 1 #define Communicate_Data_LEN 5 // 自定义交互数据长度,该长度决定了我方发送和他方接收,自定义交互数据协议更改时只需要更改此宏定义即可 #pragma pack(1) diff --git a/modules/referee/referee_task.c b/modules/referee/referee_task.c index 4827507..846d060 100644 --- a/modules/referee/referee_task.c +++ b/modules/referee/referee_task.c @@ -14,7 +14,7 @@ #include "referee_UI.h" #include "string.h" #include "cmsis_os.h" - +Referee_Interactive_info_t ui_data; static Referee_Interactive_info_t *Interactive_data; // UI绘制需要的机器人状态数据 static referee_info_t *referee_recv_info; // 接收到的裁判系统数据 uint8_t UI_Seq; // 包序号,供整个referee文件使用 @@ -49,8 +49,9 @@ referee_info_t *UITaskInit(UART_HandleTypeDef *referee_usart_handle, Referee_Int void UITask() { - RobotModeTest(Interactive_data); // 测试用函数,实现模式自动变化,用于检查该任务和裁判系统是否连接正常 +// RobotModeTest(Interactive_data); // 测试用函数,实现模式自动变化,用于检查该任务和裁判系统是否连接正常 MyUIRefresh(referee_recv_info, Interactive_data); + } static Graph_Data_t UI_shoot_line[10]; // 射击准线 diff --git a/modules/referee/referee_task.h b/modules/referee/referee_task.h index 1eec04b..99568d5 100644 --- a/modules/referee/referee_task.h +++ b/modules/referee/referee_task.h @@ -8,6 +8,7 @@ * @brief 初始化裁判系统交互任务(UI和多机通信) * */ +extern Referee_Interactive_info_t ui_data; // UI数据,将底盘中的数据传入此结构体的对应变量中,UI会自动检测是否变化,对应显示UI referee_info_t *UITaskInit(UART_HandleTypeDef *referee_usart_handle, Referee_Interactive_info_t *UI_data); /**