From c17f3f6cb754cadd6d5e25c4da8ca2361bfea980 Mon Sep 17 00:00:00 2001 From: zcj <2487150395@qq.com> Date: Wed, 1 May 2024 18:29:39 +0800 Subject: [PATCH] =?UTF-8?q?DM4310=E4=B8=8A=E8=BD=A6=E6=B5=8B=E8=AF=95?= =?UTF-8?q?=E6=88=90=E5=8A=9F=EF=BC=8C=E5=9B=BE=E4=BC=A0=E9=93=BE=E8=B7=AF?= =?UTF-8?q?=E6=B5=8B=E8=AF=95=E6=88=90=E5=8A=9F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Src/usart.c | 2 +- application/chassis/chassis.c | 1 + application/cmd/robot_cmd.c | 61 +++++++-- application/cmd/robot_cmd.h | 3 + application/gimbal/gimbal.c | 12 +- application/robot_def.h | 16 +-- application/shoot/shoot.c | 62 +++++++-- diary.md | 3 + modules/master_machine/master_process.c | 4 +- modules/master_machine/master_process.h | 2 +- modules/motor/DMmotor/dm4310.c | 2 +- modules/motor/DMmotor/dm4310.h | 1 - modules/referee/referee_protocol.h | 18 +++ modules/referee/referee_task.c | 16 +-- modules/referee/rm_referee.h | 7 +- modules/referee/vision_transfer.c | 162 ++++++++++++++++++++++++ modules/referee/vision_transfer.h | 46 +++++++ modules/remote/remote_control.c | 8 +- modules/remote/remote_control.h | 15 +++ 19 files changed, 390 insertions(+), 51 deletions(-) create mode 100644 diary.md create mode 100644 modules/referee/vision_transfer.c create mode 100644 modules/referee/vision_transfer.h diff --git a/Src/usart.c b/Src/usart.c index 7cc1d9c..5380395 100644 --- a/Src/usart.c +++ b/Src/usart.c @@ -46,7 +46,7 @@ void MX_USART1_UART_Init(void) /* USER CODE END USART1_Init 1 */ huart1.Instance = USART1; - huart1.Init.BaudRate = 921600; + huart1.Init.BaudRate = 115200; huart1.Init.WordLength = UART_WORDLENGTH_8B; huart1.Init.StopBits = UART_STOPBITS_1; huart1.Init.Parity = UART_PARITY_NONE; diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index a0ed56a..be86d12 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -311,6 +311,7 @@ void ChassisTask() // chassis_feedback_data.bullet_speed = referee_data->GameRobotState.shooter_id1_17mm_speed_limit; // chassis_feedback_data.rest_heat = referee_data->PowerHeatData.shooter_heat0; + chassis_feedback_data.cap_power = cap->cap_msg.power; // 推送反馈消息 #ifdef ONE_BOARD PubPushMessage(chassis_pub, (void *)&chassis_feedback_data); diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index 4f84e58..f466bbc 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -9,13 +9,15 @@ #include "message_center.h" #include "general_def.h" #include "dji_motor.h" +#include "dm4310.h" #include "auto_aim.h" +#include "vision_transfer.h" // bsp #include "bsp_dwt.h" #include "bsp_log.h" // 私有宏,自动将编码器转换成角度值 -#define YAW_ALIGN_ANGLE (YAW_CHASSIS_ALIGN_ECD * ECD_ANGLE_COEF_DJI) // 对齐时的角度,0-360 +#define YAW_ALIGN_ANGLE (YAW_CHASSIS_ALIGN_ECD * RAD_2_DEGREE) // 对齐时的角度,0-360,使用达妙电机 #define PTICH_HORIZON_ANGLE (PITCH_HORIZON_ECD * ECD_ANGLE_COEF_DJI) // pitch水平时电机的角度,0-360 /* cmd应用包含的模块实例指针和交互信息存储*/ @@ -36,6 +38,7 @@ static Chassis_Upload_Data_s chassis_fetch_data; // 从底盘应用接收的反 static RC_ctrl_t *rc_data; // 遥控器数据,初始化时返回 //static Vision_Recv_s *vision_recv_data; // 视觉接收数据指针,初始化时返回 //static Vision_Send_s vision_send_data; // 视觉发送数据 +static VT_ctrl_t *vt_data; // 图传链路下发的键鼠遥控数据 与遥控器数据格式保持一致 RecievePacket_t *vision_recv_data; // 视觉接收数据指针,初始化时返回 SendPacket_t vision_send_data; // 视觉发送数据 @@ -57,6 +60,7 @@ static Shoot_Upload_Data_s shoot_fetch_data; // 从发射获取的反馈信息 static Robot_Status_e robot_state; // 机器人整体工作状态 static referee_info_t* referee_data; // 用于获取裁判系统的数据 +static referee_info_vt_t* referee_vt_data; extern float horizontal_angle; //static int target_index = -1; static uint16_t base_HP; @@ -64,7 +68,8 @@ static uint16_t base_HP; void RobotCMDInit() { rc_data = RemoteControlInit(&huart3); // 修改为对应串口,注意如果是自研板dbus协议串口需选用添加了反相器的那个 - vision_recv_data = VisionInit(&huart1); // 视觉通信串口 + vision_recv_data = VisionInit(); // 视觉通信,用的是USB通讯 + vt_data = VTRefereeInit(&huart1); gimbal_cmd_pub = PubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s)); gimbal_feed_sub = SubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s)); @@ -142,6 +147,7 @@ static void update_ui_data() ui_data.shoot_mode = shoot_cmd_send.shoot_mode; ui_data.Chassis_Power_Data.chassis_power_mx = referee_data->GameRobotState.chassis_power_limit; ui_data.Vision_fire = aim_select.suggest_fire; + //ui_data.Chassis_Power_Data.cap_power = } static void pitch_limit() { @@ -149,8 +155,9 @@ static void pitch_limit() // gimbal_cmd_send.pitch -= 0.1f; // if(gimbal_fetch_data.pitch_motor_total_angle < horizontal_angle - 4000) // gimbal_cmd_send.pitch += 0.1f; - if(gimbal_cmd_send.pitch>18) gimbal_cmd_send.pitch = 18; - if(gimbal_cmd_send.pitch<-38) gimbal_cmd_send.pitch = -38; +//@TODO:把限位去掉了，记得加回来 +// if(gimbal_cmd_send.pitch>18) gimbal_cmd_send.pitch = 18; +// if(gimbal_cmd_send.pitch<-38) gimbal_cmd_send.pitch = -38; } static void auto_aim_mode() { @@ -225,12 +232,14 @@ static void RemoteControlSet() shoot_cmd_send.friction_mode = FRICTION_ON; chassis_cmd_send.chassis_mode = CHASSIS_ROTATE; gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; + } else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘跟随云台 { shoot_cmd_send.friction_mode = FRICTION_OFF; - chassis_cmd_send.chassis_mode = CHASSIS_NO_FOLLOW; + chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW; gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; + shoot_cmd_send.tele_mode = TELE_CLOSE; } // 云台参数,确定云台控制数据 @@ -257,8 +266,10 @@ static void RemoteControlSet() // 发射参数 if (switch_is_up(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[上],调整水平pitch参数 + { gimbal_cmd_send.gimbal_mode = ADJUST_PITCH_MODE; - // 弹舱舵机控制,待添加servo_motor模块,开启 + shoot_cmd_send.tele_mode = TELE_OPEN; + }// 弹舱舵机控制,待添加servo_motor模块,开启 else ; // 弹舱舵机控制,待添加servo_motor模块,关闭 @@ -390,6 +401,15 @@ static void MouseKeySet() shoot_cmd_send.friction_mode = FRICTION_ON; break; } + switch (rc_data[TEMP].key_count[KEY_PRESS][Key_Q] % 2) // Q键开关摩擦轮 + { + case 0: + shoot_cmd_send.tele_mode = TELE_CLOSE; + break; + default: + shoot_cmd_send.tele_mode = TELE_OPEN; + break; + } switch (rc_data[TEMP].key_count[KEY_PRESS][Key_E] % 2) // E键开关小陀螺 { case 0: @@ -415,8 +435,31 @@ static void MouseKeySet() pitch_limit(); death_check(); - } +/** + * @brief 输入为（图传链路）键鼠时模式和控制量设置 + * + */ +static void VTMouseKeySet() +{ + chassis_cmd_send.vx = vt_data[TEMP].key[KEY_PRESS].a * 1500 - vt_data[TEMP].key[KEY_PRESS].d * 1500; // 系数待测 + chassis_cmd_send.vy = vt_data[TEMP].key[KEY_PRESS].w * 800 - vt_data[TEMP].key[KEY_PRESS].s * 800; + + gimbal_cmd_send.yaw -= (float)vt_data[TEMP].mouse.x / 660 * 5; // 系数待测 + gimbal_cmd_send.pitch += (float)vt_data[TEMP].mouse.y / 660 * 10; + + + switch (vt_data[TEMP].key_count[KEY_PRESS][Key_F] % 2) // F键开关摩擦轮 + { + case 0: + shoot_cmd_send.friction_mode = FRICTION_OFF; + break; + default: + shoot_cmd_send.friction_mode = FRICTION_ON; + break; + } +} + /** * @brief 紧急停止,包括遥控器左上侧拨轮打满/重要模块离线/双板通信失效等 @@ -468,8 +511,8 @@ void RobotCMDTask() switch_is_mid(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[下],[中],遥控器控制 RemoteControlSet(); else if (switch_is_up(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[上],键盘控制 - MouseKeySet(); - + //MouseKeySet(); + VTMouseKeySet(); EmergencyHandler(); // 处理模块离线和遥控器急停等紧急情况 // 设置视觉发送数据,还需增加加速度和角速度数据 diff --git a/application/cmd/robot_cmd.h b/application/cmd/robot_cmd.h index f42e9ce..5e5ca79 100644 --- a/application/cmd/robot_cmd.h +++ b/application/cmd/robot_cmd.h @@ -2,6 +2,9 @@ #define ROBOT_CMD_H + + + /** * @brief 机器人核心控制任务初始化,会被RobotInit()调用 * diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index d315a7a..aad291c 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -24,13 +24,13 @@ void GimbalInit() // YAW Motor_Init_Config_s yaw_config = { .can_init_config = { - .can_handle = &hcan1, + .can_handle = &hcan2, .tx_id = 0x01, .rx_id = 0x43, }, .controller_param_init_config = { .angle_PID = { - .Kp = 0.5, // 8 + .Kp = 0.5f,//0.5, // 8 .Ki = 0, .Kd = 0, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, @@ -39,8 +39,8 @@ void GimbalInit() .MaxOut = 500, }, .speed_PID = { - .Kp = 0.5f, // 70 - .Ki = 0.2f, // 200 + .Kp = 3,//0.5f, + .Ki = 0,//0.2f, .Kd = 0,//10 .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, .IntegralLimit = 3000, @@ -51,8 +51,8 @@ void GimbalInit() .other_speed_feedback_ptr = &gimba_IMU_data->Gyro[2], }, .controller_setting_init_config = { - .angle_feedback_source = MOTOR_FEED, - .speed_feedback_source = MOTOR_FEED, + .angle_feedback_source = OTHER_FEED, + .speed_feedback_source = OTHER_FEED, .outer_loop_type = ANGLE_LOOP, .close_loop_type = ANGLE_LOOP |SPEED_LOOP, .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, diff --git a/application/robot_def.h b/application/robot_def.h index f971567..18c0abf 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -26,7 +26,7 @@ /* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.0或f结尾,无符号以u结尾 */ // 云台参数 -#define YAW_CHASSIS_ALIGN_ECD 295 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改 +#define YAW_CHASSIS_ALIGN_ECD 2.691132 // 云台和底盘对齐指向相同方向时的电机position值,需要加上PI，因为单圈角度计算里增加了PI若对云台有机械改动需要修改 #define YAW_ECD_GREATER_THAN_4096 1 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度 #define PITCH_HORIZON_ECD 3412 // 云台处于水平位置时编码器值,若对云台有机械改动需要修改 #define PITCH_MAX_ANGLE 0 // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度) @@ -111,9 +111,9 @@ typedef enum typedef enum { - LID_OPEN = 0, // 弹舱盖打开 - LID_CLOSE, // 弹舱盖关闭 -} lid_mode_e; + TELE_OPEN = 0, // 弹舱盖打开 + TELE_CLOSE, // 弹舱盖关闭 +} tele_mode_e; typedef enum { @@ -127,8 +127,8 @@ typedef enum // 功率限制,从裁判系统获取,是否有必要保留? typedef struct { // 功率控制 - float chassis_power_mx; - float last_power_mx; + uint16_t chassis_power_mx; + float cap_power; } Chassis_Power_Data_s; /* ----------------CMD应用发布的控制数据,应当由gimbal/chassis/shoot订阅---------------- */ @@ -167,7 +167,7 @@ typedef struct { shoot_mode_e shoot_mode; loader_mode_e load_mode; - lid_mode_e lid_mode; + tele_mode_e tele_mode; friction_mode_e friction_mode; float bullet_speed; // 发射周期 uint8_t rest_heat; @@ -189,7 +189,7 @@ typedef struct // float real_vx; // float real_vy; // float real_wz; - + float cap_power; uint8_t rest_heat; // 剩余枪口热量 Enemy_Color_e enemy_color; // 0 for blue, 1 for red diff --git a/application/shoot/shoot.c b/application/shoot/shoot.c index 0225ba2..3b096b6 100644 --- a/application/shoot/shoot.c +++ b/application/shoot/shoot.c @@ -9,13 +9,16 @@ /* 对于双发射机构的机器人,将下面的数据封装成结构体即可,生成两份shoot应用实例 */ -static DJIMotorInstance *friction_l, *friction_r,*friction_z, *loader; // 拨盘电机 +static DJIMotorInstance *friction_l, *friction_r,*friction_z, *loader,*telescope; // 拨盘电机 // static servo_instance *lid; 需要增加弹舱盖 static Publisher_t *shoot_pub; static Shoot_Ctrl_Cmd_s shoot_cmd_recv; // 来自cmd的发射控制信息 static Subscriber_t *shoot_sub; static Shoot_Upload_Data_s shoot_feedback_data; // 来自cmd的发射控制信息 +float Kp=2; +float Ki=1; +float Kd=0; static float stop_location; // dwt定时,计算冷却用 @@ -33,8 +36,8 @@ void ShootInit() }, .controller_param_init_config = { .speed_PID = { - .Kp = 2, // 20 - .Ki = 1, // 1 + .Kp = 6, // 20 + .Ki = 0, // 1 .Kd = 0, .Improve = PID_Integral_Limit, .IntegralLimit = 10000, @@ -112,6 +115,39 @@ void ShootInit() }; loader = DJIMotorInit(&loader_config); + // 倍镜电机 + Motor_Init_Config_s telescope_config = { + .can_init_config = { + .can_handle = &hcan2, + .tx_id = 7, + }, + .controller_param_init_config = { + .angle_PID = { + .Kp = 10, // 10 + .Ki = 0, + .Kd = 0, + .MaxOut = 15000, + }, + .speed_PID = { + .Kp = 3, // 10 + .Ki = 0, // 1 + .Kd = 0, + .Improve = PID_Integral_Limit, + .IntegralLimit = 5000, + .MaxOut = 30000, + }, + }, + .controller_setting_init_config = { + .angle_feedback_source = MOTOR_FEED, + .speed_feedback_source = MOTOR_FEED, + .outer_loop_type = SPEED_LOOP, // 初始化成SPEED_LOOP,让倍镜停在原地,防止拨盘上电时乱转 + .close_loop_type = SPEED_LOOP | ANGLE_LOOP, + .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, // 注意方向设置 + }, + .motor_type = M2006 // 英雄使用m3508 + }; + telescope = DJIMotorInit(&telescope_config); + shoot_pub = PubRegister("shoot_feed", sizeof(Shoot_Upload_Data_s)); shoot_sub = SubRegister("shoot_cmd", sizeof(Shoot_Ctrl_Cmd_s)); } @@ -130,7 +166,9 @@ void ShootTask() { // 从cmd获取控制数据 SubGetMessage(shoot_sub, &shoot_cmd_recv); - + friction_z->motor_controller.speed_PID.Kp=friction_r->motor_controller.speed_PID.Kp = Kp; + friction_z->motor_controller.speed_PID.Ki=friction_r->motor_controller.speed_PID.Ki = Ki; + friction_z->motor_controller.speed_PID.Kd=friction_r->motor_controller.speed_PID.Kd = Kd; // 对shoot mode等于SHOOT_STOP的情况特殊处理,直接停止所有电机(紧急停止) if (shoot_cmd_recv.shoot_mode == SHOOT_OFF) { @@ -138,6 +176,7 @@ void ShootTask() DJIMotorStop(friction_r); DJIMotorStop(friction_z); DJIMotorStop(loader); + DJIMotorStop(telescope); } else // 恢复运行 { @@ -145,6 +184,7 @@ void ShootTask() DJIMotorEnable(friction_r); DJIMotorEnable(friction_z); DJIMotorEnable(loader); + DJIMotorEnable(telescope); } // 如果上一次触发单发或3发指令的时间加上不应期仍然大于当前时间(尚未休眠完毕),直接返回即可 // 单发模式主要提供给能量机关激活使用(以及英雄的射击大部分处于单发) @@ -198,8 +238,8 @@ void ShootTask() if (shoot_cmd_recv.friction_mode == FRICTION_ON) { // DJIMotorSetRef(friction_l, 39000); - DJIMotorSetRef(friction_r, 39000); - DJIMotorSetRef(friction_z, 39000);//39000/6 = 6500 + DJIMotorSetRef(friction_r, 36000); + DJIMotorSetRef(friction_z, 36000);//39000/6 = 6500 } else // 关闭摩擦轮 { @@ -209,13 +249,15 @@ void ShootTask() } // 开关弹舱盖 - if (shoot_cmd_recv.lid_mode == LID_CLOSE) + if (shoot_cmd_recv.tele_mode == TELE_CLOSE) { - //... + DJIMotorOuterLoop(telescope, ANGLE_LOOP); + DJIMotorSetRef(telescope, 0); } - else if (shoot_cmd_recv.lid_mode == LID_OPEN) + else if (shoot_cmd_recv.tele_mode == TELE_OPEN) { - //... + DJIMotorOuterLoop(telescope, ANGLE_LOOP); + DJIMotorSetRef(telescope, 1620); } // 反馈数据,目前暂时没有要设定的反馈数据,后续可能增加应用离线监测以及卡弹反馈 diff --git a/diary.md b/diary.md new file mode 100644 index 0000000..2c74b2b --- /dev/null +++ b/diary.md @@ -0,0 +1,3 @@ +4.20： +改pitch电机，反馈负值相反 +电机反馈速度反馈值太陡，无法改到电机反馈 \ No newline at end of file diff --git a/modules/master_machine/master_process.c b/modules/master_machine/master_process.c index 02de012..0b336e4 100644 --- a/modules/master_machine/master_process.c +++ b/modules/master_machine/master_process.c @@ -141,9 +141,9 @@ static void DecodeVision(uint16_t recv_len) } /* 视觉通信初始化 */ -RecievePacket_t *VisionInit(UART_HandleTypeDef *_handle) +RecievePacket_t *VisionInit(void) { - UNUSED(_handle); // 仅为了消除警告 +// UNUSED(_handle); // 仅为了消除警告 USB_Init_Config_s conf = {.rx_cbk = DecodeVision}; vis_recv_buff = USBInit(conf); diff --git a/modules/master_machine/master_process.h b/modules/master_machine/master_process.h index 5f9a7ba..296f886 100644 --- a/modules/master_machine/master_process.h +++ b/modules/master_machine/master_process.h @@ -114,7 +114,7 @@ typedef __packed struct * @param handle 用于和视觉通信的串口handle(C板上一般为USART1,丝印为USART2,4pin) */ //Vision_Recv_s *VisionInit(UART_HandleTypeDef *_handle); -RecievePacket_t *VisionInit(UART_HandleTypeDef *_handle); +RecievePacket_t *VisionInit(void); /** * @brief 发送视觉数据 diff --git a/modules/motor/DMmotor/dm4310.c b/modules/motor/DMmotor/dm4310.c index d3849f0..28c6de1 100644 --- a/modules/motor/DMmotor/dm4310.c +++ b/modules/motor/DMmotor/dm4310.c @@ -46,7 +46,7 @@ static void DMMotorDecode(CANInstance *motor_can) tmp = (uint16_t)((rxbuff[1] << 8) | rxbuff[2]); measure->position = uint_to_float(tmp, DM_P_MIN, DM_P_MAX, 16); - measure->angle_single_round = ECD_ANGLE_COEF_DM * (measure->position+3.141593); + measure->angle_single_round = RAD_2_DEGREE * (measure->position+3.141593); tmp = (uint16_t)((rxbuff[3] << 4) | rxbuff[4] >> 4); measure->velocity = uint_to_float(tmp, DM_V_MIN, DM_V_MAX, 12); diff --git a/modules/motor/DMmotor/dm4310.h b/modules/motor/DMmotor/dm4310.h index 12d9ff0..4d74eea 100644 --- a/modules/motor/DMmotor/dm4310.h +++ b/modules/motor/DMmotor/dm4310.h @@ -14,7 +14,6 @@ #define DM_V_MAX 45.0f #define DM_T_MIN (-18.0f) #define DM_T_MAX 18.0f -#define ECD_ANGLE_COEF_DM 57.324840f // (360/2PI),将编码器值转化为角度制 typedef struct { diff --git a/modules/referee/referee_protocol.h b/modules/referee/referee_protocol.h index c37b8eb..7d76816 100644 --- a/modules/referee/referee_protocol.h +++ b/modules/referee/referee_protocol.h @@ -83,6 +83,8 @@ typedef enum ID_robot_hurt = 0x0206, // 伤害状态数据 ID_shoot_data = 0x0207, // 实时射击数据 ID_student_interactive = 0x0301, // 机器人间交互数据 + ID_custom_robot = 0x302, // 自定义控制器数据（图传链路） + ID_remote_control = 0x304, // 键鼠遥控数据（图传链路） } CmdID_e; /* 命令码数据段长,根据官方协议来定义长度，还有自定义数据长度 */ @@ -101,6 +103,8 @@ typedef enum LEN_robot_hurt = 1, // 0x0206 LEN_shoot_data = 7, // 0x0207 LEN_receive_data = 6 + Communicate_Data_LEN, // 0x0301 + LEN_self_control = 12, // 0x0304 + LEN_remote_control = 12, // 0x0304 } JudgeDataLength_e; @@ -222,6 +226,20 @@ typedef struct float bullet_speed; } ext_shoot_data_t; +/****************************图传链路数据****************************/ +/* ID: 0x0304 Byte: 7 图传链路键鼠遥控数据 */ +typedef struct +{ + int16_t mouse_x; + int16_t mouse_y; + int16_t mouse_z; + int8_t left_button_down; + int8_t right_button_down; + uint16_t keyboard_value; + uint16_t reserved; +}vision_transfer_t; +/****************************图传链路数据****************************/ + /****************************机器人交互数据****************************/ /****************************机器人交互数据****************************/ /* 发送的内容数据段最大为 113 检测是否超出大小限制?实际上图形段不会超，数据段最多30个，也不会超*/ diff --git a/modules/referee/referee_task.c b/modules/referee/referee_task.c index 9910da1..f0213b1 100644 --- a/modules/referee/referee_task.c +++ b/modules/referee/referee_task.c @@ -142,7 +142,7 @@ static void RobotModeTest(Referee_Interactive_info_t *_Interactive_data) // 测 _Interactive_data->gimbal_mode = GIMBAL_ZERO_FORCE; _Interactive_data->shoot_mode = SHOOT_ON; _Interactive_data->friction_mode = FRICTION_ON; - _Interactive_data->lid_mode = LID_OPEN; + _Interactive_data->tele_mode = TELE_OPEN; _Interactive_data->Chassis_Power_Data.chassis_power_mx += 3.5; if (_Interactive_data->Chassis_Power_Data.chassis_power_mx >= 18) _Interactive_data->Chassis_Power_Data.chassis_power_mx = 0; @@ -154,7 +154,7 @@ static void RobotModeTest(Referee_Interactive_info_t *_Interactive_data) // 测 _Interactive_data->gimbal_mode = GIMBAL_FREE_MODE; _Interactive_data->shoot_mode = SHOOT_OFF; _Interactive_data->friction_mode = FRICTION_OFF; - _Interactive_data->lid_mode = LID_CLOSE; + _Interactive_data->tele_mode = TELE_CLOSE; break; } case 2: @@ -163,7 +163,7 @@ static void RobotModeTest(Referee_Interactive_info_t *_Interactive_data) // 测 _Interactive_data->gimbal_mode = GIMBAL_GYRO_MODE; _Interactive_data->shoot_mode = SHOOT_ON; _Interactive_data->friction_mode = FRICTION_ON; - _Interactive_data->lid_mode = LID_OPEN; + _Interactive_data->tele_mode = TELE_OPEN; break; } case 3: @@ -172,7 +172,7 @@ static void RobotModeTest(Referee_Interactive_info_t *_Interactive_data) // 测 _Interactive_data->gimbal_mode = GIMBAL_ZERO_FORCE; _Interactive_data->shoot_mode = SHOOT_OFF; _Interactive_data->friction_mode = FRICTION_OFF; - _Interactive_data->lid_mode = LID_CLOSE; + _Interactive_data->tele_mode = TELE_CLOSE; break; } default: @@ -311,16 +311,16 @@ static void UIChangeCheck(Referee_Interactive_info_t *_Interactive_data) _Interactive_data->friction_last_mode = _Interactive_data->friction_mode; } - if (_Interactive_data->lid_mode != _Interactive_data->lid_last_mode) + if (_Interactive_data->tele_mode != _Interactive_data->tele_last_mode) { _Interactive_data->Referee_Interactive_Flag.lid_flag = 1; - _Interactive_data->lid_last_mode = _Interactive_data->lid_mode; + _Interactive_data->tele_last_mode = _Interactive_data->tele_mode; } - if (_Interactive_data->Chassis_Power_Data.chassis_power_mx != _Interactive_data->Chassis_last_Power_Data.last_power_mx) + if (_Interactive_data->Chassis_Power_Data.chassis_power_mx != _Interactive_data->Chassis_last_Power_Data.chassis_power_mx) { _Interactive_data->Referee_Interactive_Flag.Power_flag = 1; - _Interactive_data->Chassis_last_Power_Data.last_power_mx = _Interactive_data->Chassis_Power_Data.chassis_power_mx; + _Interactive_data->Chassis_last_Power_Data.chassis_power_mx = _Interactive_data->Chassis_Power_Data.chassis_power_mx; } if (_Interactive_data->Vision_fire != _Interactive_data->last_Vision_fire) { diff --git a/modules/referee/rm_referee.h b/modules/referee/rm_referee.h index b909609..db5190b 100644 --- a/modules/referee/rm_referee.h +++ b/modules/referee/rm_referee.h @@ -38,7 +38,8 @@ typedef struct ext_robot_hurt_t RobotHurt; // 0x0206 ext_shoot_data_t ShootData; // 0x0207 - // 自定义交互数据的接收 + vision_transfer_t VisionTransfer; + // 自定义交互数据的接收 Communicate_ReceiveData_t ReceiveData; uint8_t init_flag; @@ -66,7 +67,7 @@ typedef struct gimbal_mode_e gimbal_mode; // 云台模式 shoot_mode_e shoot_mode; // 发射模式设置 friction_mode_e friction_mode; // 摩擦轮关闭 - lid_mode_e lid_mode; // 弹舱盖打开 + tele_mode_e tele_mode; // 弹舱盖打开 Chassis_Power_Data_s Chassis_Power_Data; // 功率控制 uint8_t Vision_fire; @@ -76,7 +77,7 @@ typedef struct gimbal_mode_e gimbal_last_mode; shoot_mode_e shoot_last_mode; friction_mode_e friction_last_mode; - lid_mode_e lid_last_mode; + tele_mode_e tele_last_mode; Chassis_Power_Data_s Chassis_last_Power_Data; uint8_t last_Vision_fire; diff --git a/modules/referee/vision_transfer.c b/modules/referee/vision_transfer.c new file mode 100644 index 0000000..7241421 --- /dev/null +++ b/modules/referee/vision_transfer.c @@ -0,0 +1,162 @@ +/** + * @file rm_referee.C + * @author kidneygood (you@domain.com) + * @brief + * @version 0.1 + * @date 2022-11-18 + * + * @copyright Copyright (c) 2022 + * + */ + +#include "vision_transfer.h" +#include "string.h" +#include "crc_ref.h" +#include "bsp_usart.h" +#include "task.h" +#include "daemon.h" +#include "bsp_log.h" +#include "cmsis_os.h" +#include "remote_control.h" + +#define RE_RX_BUFFER_SIZE 128u // 裁判系统接收缓冲区大小 + +static USARTInstance *vt_usart_instance; // 裁判系统串口实例 +static DaemonInstance *vision_transfer_daemon; // 裁判系统守护进程 +static referee_info_vt_t referee_info_vt; // 裁判系统数据 +static VT_ctrl_t vt_ctrl[2]; //[0]:当前数据TEMP,[1]:上一次的数据LAST.用于按键持续按下和切换的判断 +/** + * @brief 读取裁判数据,中断中读取保证速度 + * @param buff: 读取到的裁判系统原始数据 + * @retval 是否对正误判断做处理 + * @attention 在此判断帧头和CRC校验,无误再写入数据，不重复判断帧头 + */ +static void JudgeReadVTData(uint8_t *buff) +{ + uint16_t judge_length; // 统计一帧数据长度 + if (buff == NULL) // 空数据包，则不作任何处理 + return; + + // 写入帧头数据(5-byte),用于判断是否开始存储裁判数据 + memcpy(&referee_info_vt.FrameHeader, buff, LEN_HEADER); + + // 判断帧头数据(0)是否为0xA5 + if (buff[SOF] == REFEREE_SOF) + { + // 帧头CRC8校验 + if (Verify_CRC8_Check_Sum(buff, LEN_HEADER) == TRUE) + { + // 统计一帧数据长度(byte),用于CR16校验 + judge_length = buff[DATA_LENGTH] + LEN_HEADER + LEN_CMDID + LEN_TAIL; + // 帧尾CRC16校验 + if (Verify_CRC16_Check_Sum(buff, judge_length) == TRUE) + { + // 2个8位拼成16位int + referee_info_vt.CmdID = (buff[6] << 8 | buff[5]); + // 解析数据命令码,将数据拷贝到相应结构体中(注意拷贝数据的长度) + // 第8个字节开始才是数据 data=7 + switch (referee_info_vt.CmdID) + { + case ID_custom_robot: //0x0302 + memcpy(&referee_info_vt.ReceiveData, (buff + DATA_Offset), LEN_self_control); + break; + case ID_remote_control: //0x0304 + memcpy(&referee_info_vt.VisionTransfer, (buff + DATA_Offset), LEN_remote_control); + break; + } + } + } + // 首地址加帧长度,指向CRC16下一字节,用来判断是否为0xA5,从而判断一个数据包是否有多帧数据 + if (*(buff + sizeof(xFrameHeader) + LEN_CMDID + referee_info_vt.FrameHeader.DataLength + LEN_TAIL) == 0xA5) + { // 如果一个数据包出现了多帧数据,则再次调用解析函数,直到所有数据包解析完毕 + JudgeReadVTData(buff + sizeof(xFrameHeader) + LEN_CMDID + referee_info_vt.FrameHeader.DataLength + LEN_TAIL); + } + } +} +static void vt_to_rc(void) +{ + // 鼠标解析 + vt_ctrl[TEMP].mouse.x = referee_info_vt.VisionTransfer.mouse_x; //!< Mouse X axis + vt_ctrl[TEMP].mouse.y = referee_info_vt.VisionTransfer.mouse_y; //!< Mouse Y axis + vt_ctrl[TEMP].mouse.press_l = referee_info_vt.VisionTransfer.left_button_down; //!< Mouse Left Is Press ? + vt_ctrl[TEMP].mouse.press_r = referee_info_vt.VisionTransfer.right_button_down; //!< Mouse Right Is Press ? + +// 位域的按键值解算,直接memcpy即可,注意小端低字节在前,即lsb在第一位,msb在最后 + *(uint16_t *)&vt_ctrl[TEMP].key[KEY_PRESS] = referee_info_vt.VisionTransfer.keyboard_value; + + if (vt_ctrl[TEMP].key[KEY_PRESS].ctrl) // ctrl键按下 + vt_ctrl[TEMP].key[KEY_PRESS_WITH_CTRL] = vt_ctrl[TEMP].key[KEY_PRESS]; + else + memset(&vt_ctrl[TEMP].key[KEY_PRESS_WITH_CTRL], 0, sizeof(Key_t)); + if (vt_ctrl[TEMP].key[KEY_PRESS].shift) // shift键按下 + vt_ctrl[TEMP].key[KEY_PRESS_WITH_SHIFT] = vt_ctrl[TEMP].key[KEY_PRESS]; + else + memset(&vt_ctrl[TEMP].key[KEY_PRESS_WITH_SHIFT], 0, sizeof(Key_t)); + + uint16_t key_now = vt_ctrl[TEMP].key[KEY_PRESS].keys, // 当前按键是否按下 + key_last = vt_ctrl[LAST].key[KEY_PRESS].keys, // 上一次按键是否按下 + key_with_ctrl = vt_ctrl[TEMP].key[KEY_PRESS_WITH_CTRL].keys, // 当前ctrl组合键是否按下 + key_with_shift = vt_ctrl[TEMP].key[KEY_PRESS_WITH_SHIFT].keys, // 当前shift组合键是否按下 + key_last_with_ctrl = vt_ctrl[LAST].key[KEY_PRESS_WITH_CTRL].keys, // 上一次ctrl组合键是否按下 + key_last_with_shift = vt_ctrl[LAST].key[KEY_PRESS_WITH_SHIFT].keys; // 上一次shift组合键是否按下 + + for (uint16_t i = 0, j = 0x1; i < 16; j <<= 1, i++) + { + if (i == 4 || i == 5) // 4,5位为ctrl和shift,直接跳过 + continue; +// 如果当前按键按下,上一次按键没有按下,且ctrl和shift组合键没有按下,则按键按下计数加1(检测到上升沿) + if ((key_now & j) && !(key_last & j) && !(key_with_ctrl & j) && !(key_with_shift & j)) + vt_ctrl[TEMP].key_count[KEY_PRESS][i]++; +// 当前ctrl组合键按下,上一次ctrl组合键没有按下,则ctrl组合键按下计数加1(检测到上升沿) + if ((key_with_ctrl & j) && !(key_last_with_ctrl & j)) + vt_ctrl[TEMP].key_count[KEY_PRESS_WITH_CTRL][i]++; +// 当前shift组合键按下,上一次shift组合键没有按下,则shift组合键按下计数加1(检测到上升沿) + if ((key_with_shift & j) && !(key_last_with_shift & j)) + vt_ctrl[TEMP].key_count[KEY_PRESS_WITH_SHIFT][i]++; + } + + memcpy(&vt_ctrl[LAST], &vt_ctrl[TEMP], sizeof(VT_ctrl_t)); // 保存上一次的数据,用于按键持续按下和切换的判断 +} + +/*裁判系统串口接收回调函数,解析数据 */ +static void VTRefereeRxCallback() +{ + DaemonReload(vision_transfer_daemon); + JudgeReadVTData(vt_usart_instance->recv_buff); + vt_to_rc(); +} +// 裁判系统丢失回调函数,重新初始化裁判系统串口 +static void VTRefereeLostCallback(void *arg) +{ + USARTServiceInit(vt_usart_instance); + LOGWARNING("[rm_ref] lost referee vision data "); +} + +/* 裁判系统通信初始化 */ +VT_ctrl_t *VTRefereeInit(UART_HandleTypeDef *vt_usart_handle) +{ + USART_Init_Config_s conf; + conf.module_callback = VTRefereeRxCallback; + conf.usart_handle = vt_usart_handle; + conf.recv_buff_size = RE_RX_BUFFER_SIZE; // mx 255(u8) + vt_usart_instance = USARTRegister(&conf); + + Daemon_Init_Config_s daemon_conf = { + .callback = VTRefereeLostCallback, + .owner_id = vt_usart_instance, + .reload_count = 30, // 0.3s没有收到数据,则认为丢失,重启串口接收 + }; + vision_transfer_daemon = DaemonRegister(&daemon_conf); + + return &vt_ctrl; +} + +/** + * @brief 裁判系统数据发送函数 + * @param + */ +void VTRefereeSend(uint8_t *send, uint16_t tx_len) +{ + USARTSend(vt_usart_instance, send, tx_len, USART_TRANSFER_DMA); + osDelay(115); +} diff --git a/modules/referee/vision_transfer.h b/modules/referee/vision_transfer.h new file mode 100644 index 0000000..4dd33f8 --- /dev/null +++ b/modules/referee/vision_transfer.h @@ -0,0 +1,46 @@ +#ifndef VISION_TRANSFER_H +#define VISION_TRANSFER_H + +#include "usart.h" +#include "referee_protocol.h" +#include "robot_def.h" +#include "bsp_usart.h" +#include "FreeRTOS.h" +#include "remote_control.h" + +#pragma pack(1) + +// 此结构体包含裁判系统接收数据以及UI绘制与机器人车间通信的相关信息 +typedef struct +{ + + xFrameHeader FrameHeader; // 接收到的帧头信息 + uint16_t CmdID; + + vision_transfer_t VisionTransfer; + Communicate_ReceiveData_t ReceiveData; + + uint8_t init_flag; + +} referee_info_vt_t; + +#pragma pack() + +/** + * @brief 裁判系统通信初始化,该函数会初始化裁判系统串口,开启中断 + * + * @param vt_usart_handle 串口handle,C板一般用串口6 + * @return referee_info_t* 返回裁判系统反馈的数据,包括热量/血量/状态等 + */ +VT_ctrl_t *VTRefereeInit(UART_HandleTypeDef *referee_usart_handle); + +/** + * @brief UI绘制和交互数的发送接口,由UI绘制任务和多机通信函数调用 + * @note 内部包含了一个实时系统的延时函数,这是因为裁判系统接收CMD数据至高位10Hz + * + * @param send 发送数据首地址 + * @param tx_len 发送长度 + */ +void VTRefereeSend(uint8_t *send, uint16_t tx_len); + +#endif // !REFEREE_H diff --git a/modules/remote/remote_control.c b/modules/remote/remote_control.c index 228ff68..5d637e7 100644 --- a/modules/remote/remote_control.c +++ b/modules/remote/remote_control.c @@ -5,15 +5,21 @@ #include "stdlib.h" #include "daemon.h" #include "bsp_log.h" +#include "referee_protocol.h" +#include "rm_referee.h" #define REMOTE_CONTROL_FRAME_SIZE 18u // 遥控器接收的buffer大小 // 遥控器数据 static RC_ctrl_t rc_ctrl[2]; //[0]:当前数据TEMP,[1]:上一次的数据LAST.用于按键持续按下和切换的判断 + + static uint8_t rc_init_flag = 0; // 遥控器初始化标志位 // 遥控器拥有的串口实例,因为遥控器是单例,所以这里只有一个,就不封装了 +// 第二个串口实例，有可能出现问题 static USARTInstance *rc_usart_instance; + static DaemonInstance *rc_daemon_instance; /** @@ -134,4 +140,4 @@ uint8_t RemoteControlIsOnline() if (rc_init_flag) return DaemonIsOnline(rc_daemon_instance); return 0; -} \ No newline at end of file +} diff --git a/modules/remote/remote_control.h b/modules/remote/remote_control.h index c12f810..3a100c1 100644 --- a/modules/remote/remote_control.h +++ b/modules/remote/remote_control.h @@ -113,6 +113,20 @@ typedef struct uint8_t key_count[3][16]; } RC_ctrl_t; +typedef struct +{ + struct + { + int16_t x; + int16_t y; + uint8_t press_l; + uint8_t press_r; + } mouse; + + Key_t key[3]; + uint8_t key_count[3][16]; +}VT_ctrl_t; //图传链路下发的遥控数据 + /* ------------------------- Internal Data ----------------------------------- */ /** @@ -123,6 +137,7 @@ typedef struct */ RC_ctrl_t *RemoteControlInit(UART_HandleTypeDef *rc_usart_handle); + /** * @brief 检查遥控器是否在线,若尚未初始化也视为离线 *