From 7d9955f5754e524ce3c004481bf168e44569970d Mon Sep 17 00:00:00 2001 From: HshineO <1491134420@qq.com> Date: Tue, 7 May 2024 14:06:47 +0800 Subject: [PATCH] =?UTF-8?q?=E6=9C=BA=E6=A2=B0=E8=87=82=E5=90=84=E5=85=B3?= =?UTF-8?q?=E8=8A=82=E5=9F=BA=E7=A1=80=E8=BF=90=E5=8A=A8=E8=B0=83=E8=AF=95?= =?UTF-8?q?=E5=AE=8C=E6=88=90?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Src/main.c | 2 + application/cmd/robot_cmd.c | 55 +++-- application/gimbal/gimbal.cpp | 268 ++++++++++++++++++-- application/robot.c | 12 +- application/robot_def.h | 16 ++ application/robot_task.h | 3 + bsp/can/bsp_can.c | 2 +- engineering.jdebug.user | 66 +++-- modules/motor/DMmotor/dmmotor.c | 17 +- modules/motor/DMmotor/dmmotor.h | 6 +- modules/motor/motor_def.h | 2 + modules/motor/motor_task.c | 2 +- modules/referee/referee_VT.c | 112 +++++++++ modules/referee/referee_VT.h | 16 ++ modules/referee/referee_protocol.h | 382 ++++++++++++++++------------- modules/referee/rm_referee.h | 15 ++ 16 files changed, 718 insertions(+), 258 deletions(-) create mode 100644 modules/referee/referee_VT.c create mode 100644 modules/referee/referee_VT.h diff --git a/Src/main.c b/Src/main.c index 5367de8..b36416f 100644 --- a/Src/main.c +++ b/Src/main.c @@ -120,6 +120,8 @@ int main(void) MX_CRC_Init(); MX_DAC_Init(); /* USER CODE BEGIN 2 */ + + HAL_Delay(1500); //等待达妙电机上电初始化 RobotInit(); // 唯一的初始化函数 LOGINFO("[main] SystemInit() and RobotInit() done"); /* USER CODE END 2 */ diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index e96c098..b583564 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -10,6 +10,7 @@ #include "dji_motor.h" #include "auto_aim.h" #include "super_cap.h" +#include "user_lib.h" // bsp #include "bsp_dwt.h" #include "bsp_log.h" @@ -123,35 +124,59 @@ static void update_ui_data() { } + + // 出招表 + static void RemoteControlSet() { - if (switch_is_down(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[下],底盘运动,伸缩保持不动 + if (switch_is_down(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[上],底盘运动,伸缩保持不动 { chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW; to_stretch_cmd_send.to_stretch_mode = TO_STRETCH_KEEP; + gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; + // 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整，遥控器输入灵敏度 + chassis_cmd_send.vx = 15.0f * (float) rc_data[TEMP].rc.rocker_r_; // _水平方向 + chassis_cmd_send.vy = 15.0f * (float) rc_data[TEMP].rc.rocker_r1; // 1数值方向 + chassis_cmd_send.wz = -1.0f * (float) rc_data[TEMP].rc.rocker_l_; //旋转 - } else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘不动，伸缩 + } else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘平移，伸缩 { - chassis_cmd_send.chassis_mode = CHASSIS_ZERO_FORCE; + chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW; to_stretch_cmd_send.to_stretch_mode = TO_STRETCH_MODE; - } - if (switch_is_mid(rc_data[TEMP].rc.switch_left) && (switch_is_mid(rc_data[TEMP].rc.switch_right))) - { - to_stretch_cmd_send.ud += 0.0025F * (float) rc_data[TEMP].rc.rocker_l1;//0.0025f - to_stretch_cmd_send.fb += 0.0025F * (float) rc_data[TEMP].rc.rocker_r1;//0.0025f + gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; + + chassis_cmd_send.vx = 15.0f * (float) rc_data[TEMP].rc.rocker_r_; // _水平方向 + chassis_cmd_send.vy = 15.0f * (float) rc_data[TEMP].rc.rocker_r1; // 1数值方向 + + to_stretch_cmd_send.ud += 0.0025F * (float) rc_data[TEMP].rc.rocker_l_;//抬升 + to_stretch_cmd_send.fb += 0.0025F * (float) rc_data[TEMP].rc.rocker_l1;//前伸 //伸缩限位待添加 // if (gimbal_cmd_send.pitch >= PITCH_MAX_ANGLE) gimbal_cmd_send.pitch = PITCH_MAX_ANGLE; // if (gimbal_cmd_send.pitch <= PITCH_MIN_ANGLE) gimbal_cmd_send.pitch = PITCH_MIN_ANGLE; } - // 左侧开关状态为[下],视觉模式 - if (switch_is_down(rc_data[TEMP].rc.switch_left)) { - //auto_aim_mode(); + + // 右侧开关状态为[下],机械臂 + if (switch_is_down(rc_data[TEMP].rc.switch_right)) { + // 左侧开关状态为[下],前三轴 + if(switch_is_down(rc_data[TEMP].rc.switch_left)) + { + gimbal_cmd_send.pitch += 0.001f * (float) rc_data[TEMP].rc.rocker_r1; + gimbal_cmd_send.yaw += 0.001f * (float) rc_data[TEMP].rc.rocker_r_; + gimbal_cmd_send.roll += 0.001f * (float) rc_data[TEMP].rc.rocker_l_; + } + // 左侧开关状态为[中],后两轴 + if(switch_is_mid(rc_data[TEMP].rc.switch_left)) + { + gimbal_cmd_send.diff_pitch += 0.001f * (float) rc_data[TEMP].rc.rocker_r1; + gimbal_cmd_send.diff_roll += 0.001f * (float) rc_data[TEMP].rc.rocker_r_; + } } // 云台软件限位 + gimbal_cmd_send.pitch = float_constrain(gimbal_cmd_send.pitch,PITCH_MIN,PITCH_MAX); + gimbal_cmd_send.yaw = float_constrain(gimbal_cmd_send.yaw,-YAW,YAW); + //gimbal_cmd_send.roll = float_constrain(gimbal_cmd_send.roll,-ROLL,ROLL); - // 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整，遥控器输入灵敏度 - chassis_cmd_send.vx = 15.0f * (float) rc_data[TEMP].rc.rocker_r_; // _水平方向 - chassis_cmd_send.vy = 15.0f * (float) rc_data[TEMP].rc.rocker_r1; // 1数值方向 - chassis_cmd_send.wz = -1.0f * (float) rc_data[TEMP].rc.rocker_l_; //旋转 + gimbal_cmd_send.diff_pitch = float_constrain(gimbal_cmd_send.diff_pitch,-DIFF_PITCH,DIFF_PITCH); + gimbal_cmd_send.diff_roll = float_constrain(gimbal_cmd_send.diff_roll,-DIFF_ROLL,DIFF_ROLL); } diff --git a/application/gimbal/gimbal.cpp b/application/gimbal/gimbal.cpp index 9ad3535..eaa7f37 100644 --- a/application/gimbal/gimbal.cpp +++ b/application/gimbal/gimbal.cpp @@ -4,10 +4,12 @@ extern "C" { #include "gimbal.h" #include "robot_def.h" #include "dji_motor.h" +#include "dmmotor.h" #include "ins_task.h" #include "message_center.h" #include "general_def.h" #include "bmi088.h" +#include "user_lib.h" #ifdef __cplusplus } #endif @@ -15,19 +17,212 @@ extern "C" { #include "robotics.h" static attitude_t *gimba_IMU_data; // 云台IMU数据 -static DJIMotorInstance *yaw_motor, *pitch_motor; +static DMMotorInstance *yaw_motor, *pitch_motor, *roll_motor; +static DMMotorInstance *diff_r_motor,*diff_l_motor; + +static PIDInstance diff_pitch_loop,diff_roll_loop; +static PIDInstance diff_pitch_spd_loop,diff_roll_spd_loop; + +float arm_gravity_feedforward = 0; +float GRAVITY_COMP = 5.5; static Publisher_t *gimbal_pub; // 云台应用消息发布者(云台反馈给cmd) static Subscriber_t *gimbal_sub; // cmd控制消息订阅者 static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给cmd的云台状态信息 static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv; // 来自cmd的控制信息 +first_order_filter_type_t pitch_spd_filter,yaw_spd_filter,roll_spd_filter; +static float pitch_spd,yaw_spd,roll_spd; + +first_order_filter_type_t diff_r_spd_filter,diff_l_spd_filter; +static float diff_r_spd,diff_l_spd; + void GimbalInit() { + Motor_Init_Config_s yaw_motor_config = { + .controller_param_init_config = { + .other_speed_feedback_ptr = &yaw_spd, + .speed_PID = { + .Kp = 3,//5, + .Ki = 3.0f, + .Kd = 0.02f, + .MaxOut = 10, + .Improve = static_cast(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement), + .IntegralLimit = 10.0F, + }, + .angle_PID = { + .Kp = 10.0f,//13.0f, + .Ki = 0.0f, + .Kd = 0.0f, + .MaxOut = 10, + .Improve = static_cast(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement), + .IntegralLimit = 10.0F, + }, + }, + .controller_setting_init_config = { + .outer_loop_type = ANGLE_LOOP, + .close_loop_type = static_cast(ANGLE_LOOP | SPEED_LOOP), + .angle_feedback_source = MOTOR_FEED, + .speed_feedback_source = OTHER_FEED, + .feedforward_flag = CURRENT_FEEDFORWARD, + }, + .motor_type = DM6006, + }; + + yaw_motor_config.can_init_config.can_handle = &hcan2; + yaw_motor_config.can_init_config.tx_id = 1; + yaw_motor_config.can_init_config.rx_id = 2; + yaw_motor = DMMotorInit(&yaw_motor_config); + + Motor_Init_Config_s pitch_motor_config = { + .controller_param_init_config = { + .other_speed_feedback_ptr = &pitch_spd, + .current_feedforward_ptr = &arm_gravity_feedforward, + .speed_PID = { + .Kp = 2.5f, + .Ki = 0.8f, + .Kd = 0.0f, + .MaxOut = 10, + .Improve = static_cast(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement), + .IntegralLimit = 10.0F, + }, + .angle_PID = { + .Kp = 8.0f, + .Ki = 0.0f, + .Kd = 0.0f, + .MaxOut = 10, + .Improve = static_cast(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement), + .IntegralLimit = 10.0F, + }, + }, + .controller_setting_init_config = { + .outer_loop_type = ANGLE_LOOP, + .close_loop_type = static_cast(ANGLE_LOOP | SPEED_LOOP), + .angle_feedback_source = MOTOR_FEED, + .speed_feedback_source = OTHER_FEED, + .feedforward_flag = CURRENT_FEEDFORWARD, + }, + .motor_type = DM6006, + }; + + pitch_motor_config.can_init_config.can_handle = &hcan2; + pitch_motor_config.can_init_config.tx_id = 3; + pitch_motor_config.can_init_config.rx_id = 4; + pitch_motor = DMMotorInit(&pitch_motor_config); + + //@todo：roll轴机械固定不牢 待细调 + Motor_Init_Config_s roll_motor_config = { + .controller_param_init_config = { + .other_speed_feedback_ptr = &roll_spd, + .speed_PID = { + .Kp = 0.8f, + .Ki = 0.3f, + .Kd = 0.02f, + .MaxOut = 10, + .Improve = static_cast(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement), + .IntegralLimit = 10.0F, + }, + .angle_PID = { + .Kp = 10.0f, + .Ki = 0.0f, + .Kd = 0.0f, + .MaxOut = 10, + .Improve = static_cast(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement), + .IntegralLimit = 10.0F, + }, + }, + .controller_setting_init_config = { + .outer_loop_type = ANGLE_LOOP, + .close_loop_type = static_cast(ANGLE_LOOP | SPEED_LOOP), + .angle_feedback_source = MOTOR_FEED, + .speed_feedback_source = OTHER_FEED, + //.feedforward_flag = CURRENT_FEEDFORWARD, + }, + .motor_type = DM4310, + }; + + roll_motor_config.can_init_config.can_handle = &hcan2; + roll_motor_config.can_init_config.tx_id = 5; + roll_motor_config.can_init_config.rx_id = 6; + roll_motor = DMMotorInit(&roll_motor_config); + + Motor_Init_Config_s diff_motor_config = { + .controller_setting_init_config = { + .outer_loop_type = OPEN_LOOP, + .close_loop_type = OPEN_LOOP, + .angle_feedback_source = MOTOR_FEED, + .speed_feedback_source = OTHER_FEED, + //.feedforward_flag = CURRENT_FEEDFORWARD, + }, + .motor_type = DM4310, + }; + + diff_motor_config.can_init_config.can_handle = &hcan2; + diff_motor_config.can_init_config.tx_id = 7; + diff_motor_config.can_init_config.rx_id = 8; + diff_motor_config.controller_param_init_config.other_speed_feedback_ptr = &diff_r_spd; + diff_r_motor = DMMotorInit(&diff_motor_config); + + diff_motor_config.can_init_config.can_handle = &hcan2; + diff_motor_config.can_init_config.tx_id = 9; + diff_motor_config.can_init_config.rx_id = 10; + diff_motor_config.controller_param_init_config.other_speed_feedback_ptr = &diff_l_spd; + diff_l_motor = DMMotorInit(&diff_motor_config); + + const float spd_filter_num = 0.05f; + first_order_filter_init(&pitch_spd_filter,5e-3,&spd_filter_num); + const float spd_filter_num_yaw = 0.1f; + first_order_filter_init(&yaw_spd_filter,5e-3,&spd_filter_num_yaw); + const float spd_filter_num_roll = 0.05f; + first_order_filter_init(&roll_spd_filter,5e-3,&spd_filter_num_roll); + + const float spd_filter_num_diff = 0.05f; + first_order_filter_init(&diff_r_spd_filter,5e-3,&spd_filter_num_diff); + first_order_filter_init(&diff_l_spd_filter,5e-3,&spd_filter_num_diff); + + PID_Init_Config_s diff_pitch_config= { + .Kp = 15.0f, + .Ki = 0, + .Kd = 0, + .MaxOut = 10, + .Improve = static_cast(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement), + .IntegralLimit = 100, + }; + PIDInit(&diff_pitch_loop,&diff_pitch_config); + + PID_Init_Config_s diff_pitch_spd_config= { + .Kp = 0.6f, + .Ki = 2.0f, + .Kd = 0.01f, + .MaxOut = 10, + .Improve = static_cast(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement), + .IntegralLimit = 100, + }; + PIDInit(&diff_pitch_spd_loop,&diff_pitch_spd_config); + + PID_Init_Config_s diff_roll_config= { + .Kp = 14.0f, + .Ki = 0, + .Kd = 0, + .MaxOut = 10, + .Improve = static_cast(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement), + .IntegralLimit = 100, + }; + PIDInit(&diff_roll_loop,&diff_roll_config); + + PID_Init_Config_s diff_roll_spd_config= { + .Kp = 1.0f, + .Ki = 1.0f, + .Kd = 0.02f, + .MaxOut = 10, + .Improve = static_cast(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement), + .IntegralLimit = 100, + }; + PIDInit(&diff_roll_spd_loop,&diff_roll_spd_config); + + gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源 // YAW - - gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s)); gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s)); } @@ -45,6 +240,35 @@ void GimbalTask() // 获取云台控制数据 // 后续增加未收到数据的处理 SubGetMessage(gimbal_sub, &gimbal_cmd_recv); + //大臂重力补偿力矩 + arm_gravity_feedforward = - GRAVITY_COMP * arm_cos_f32(- pitch_motor->measure.position); + //电机速度滤波 + first_order_filter_cali(&pitch_spd_filter,pitch_motor->measure.velocity); + pitch_spd = pitch_spd_filter.out; + first_order_filter_cali(&yaw_spd_filter,yaw_motor->measure.velocity); + yaw_spd = yaw_spd_filter.out; + first_order_filter_cali(&roll_spd_filter,roll_motor->measure.velocity); + roll_spd = roll_spd_filter.out; + + //手腕关节 + first_order_filter_cali(&diff_r_spd_filter,diff_r_motor->measure.velocity); + diff_r_spd = diff_r_spd_filter.out; + first_order_filter_cali(&diff_l_spd_filter,diff_l_motor->measure.velocity); + diff_l_spd = diff_l_spd_filter.out; + + float diff_pitch_angle = (diff_r_motor->measure.position + (-diff_l_motor->measure.position))/2; + float diff_roll_angle = (diff_r_motor->measure.position - (-diff_l_motor->measure.position))/2 * 18/52; + + float pitch_angle_out = PIDCalculate(&diff_pitch_loop,diff_pitch_angle,gimbal_cmd_recv.diff_pitch * DEGREE_2_RAD); + float roll_angle_out = PIDCalculate(&diff_roll_loop,diff_roll_angle,gimbal_cmd_recv.diff_roll * DEGREE_2_RAD); + + float diff_pitch_spd = (diff_r_spd + (-diff_l_spd)) / 2; + float diff_roll_spd = (diff_r_spd - (-diff_l_spd)) /2 * 18/52; + float pitch_out = PIDCalculate(&diff_pitch_spd_loop,diff_pitch_spd,pitch_angle_out); + float roll_out = PIDCalculate(&diff_roll_spd_loop,diff_roll_spd,roll_angle_out); + + float r_speed_set = pitch_out + roll_out; + float l_speed_set = pitch_out - roll_out; // @todo:现在已不再需要电机反馈,实际上可以始终使用IMU的姿态数据来作为云台的反馈,yaw电机的offset只是用来跟随底盘 // 根据控制模式进行电机反馈切换和过渡,视觉模式在robot_cmd模块就已经设置好,gimbal只看yaw_ref和pitch_ref @@ -52,30 +276,26 @@ void GimbalTask() { // 停止 case GIMBAL_ZERO_FORCE: - DJIMotorStop(yaw_motor); - DJIMotorStop(pitch_motor); + DMMotorStop(yaw_motor); + DMMotorStop(pitch_motor); + DMMotorStop(roll_motor); + DMMotorStop(diff_r_motor); + DMMotorStop(diff_l_motor); + break; // 使用陀螺仪的反馈,底盘根据yaw电机的offset跟随云台或视觉模式采用 case GIMBAL_GYRO_MODE: // 后续只保留此模式 - DJIMotorEnable(yaw_motor); - DJIMotorEnable(pitch_motor); - DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED); - DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, OTHER_FEED); - DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, OTHER_FEED); - DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED); - DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈 - DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch); - break; - // 云台自由模式,使用编码器反馈,底盘和云台分离,仅云台旋转,一般用于调整云台姿态(英雄吊射等)/能量机关 - case GIMBAL_FREE_MODE: // 后续删除,或加入云台追地盘的跟随模式(响应速度更快) - DJIMotorEnable(yaw_motor); - DJIMotorEnable(pitch_motor); - DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED); - DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, OTHER_FEED); - DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, OTHER_FEED); - DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED); - DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈 - DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch); + DMMotorEnable(yaw_motor); + DMMotorEnable(pitch_motor); + DMMotorEnable(roll_motor); + DMMotorSetRef(pitch_motor,gimbal_cmd_recv.pitch * DEGREE_2_RAD); + DMMotorSetRef(yaw_motor,gimbal_cmd_recv.yaw * DEGREE_2_RAD); + DMMotorSetRef(roll_motor,gimbal_cmd_recv.roll * DEGREE_2_RAD); + + DMMotorEnable(diff_r_motor); + DMMotorEnable(diff_l_motor); + DMMotorSetRef(diff_r_motor,r_speed_set); + DMMotorSetRef(diff_l_motor,-l_speed_set); break; default: break; diff --git a/application/robot.c b/application/robot.c index f9c72cd..4619b61 100644 --- a/application/robot.c +++ b/application/robot.c @@ -31,12 +31,12 @@ void RobotInit() #if defined(ONE_BOARD) || defined(GIMBAL_BOARD) RobotCMDInit(); -// GimbalInit(); - To_stretchInit(); + GimbalInit(); + //To_stretchInit(); #endif #if defined(ONE_BOARD) || defined(CHASSIS_BOARD) - ChassisInit(); + //ChassisInit(); #endif OSTaskInit(); // 创建基础任务 @@ -49,12 +49,12 @@ void RobotTask() { #if defined(ONE_BOARD) || defined(GIMBAL_BOARD) RobotCMDTask(); -// GimbalTask(); - To_stretchTask(); + GimbalTask(); + //To_stretchTask(); #endif #if defined(ONE_BOARD) || defined(CHASSIS_BOARD) - ChassisTask(); + //ChassisTask(); #endif } \ No newline at end of file diff --git a/application/robot_def.h b/application/robot_def.h index 586aee6..97bc82b 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -25,6 +25,19 @@ // #define VISION_USE_UART // 使用串口发送视觉数据 /* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.0或f结尾,无符号以u结尾 */ +// 关节限位参数 +#define DIFF_PITCH 90 //差动手腕关节 +#define DIFF_ROLL 180 + +#define ROLL 90 +#define PITCH_MIN (-160.0f) +#define PITCH_MAX 20.0F + +#define YAW 60 + + + + // 云台参数 #define YAW_CHASSIS_ALIGN_ECD 0 // 云台和底盘对齐指向相同方向时的电机position值,若对云台有机械改动需要修改 #define YAW_ECD_GREATER_THAN_4096 1 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度 @@ -131,6 +144,9 @@ typedef struct { // 云台角度控制 float yaw; float pitch; + float roll; + float diff_pitch; //差速器pitch + float diff_roll; //差速器roll float chassis_rotate_wz; gimbal_mode_e gimbal_mode; diff --git a/application/robot_task.h b/application/robot_task.h index 252e77b..6e9ccb3 100644 --- a/application/robot_task.h +++ b/application/robot_task.h @@ -13,6 +13,7 @@ #include "master_process.h" #include "daemon.h" #include "HT04.h" +#include "dmmotor.h" #include "buzzer.h" #include "bsp_log.h" @@ -52,6 +53,8 @@ void OSTaskInit() uiTaskHandle = osThreadCreate(osThread(uitask), NULL); HTMotorControlInit(); // 没有注册HT电机则不会执行 + + DMMotorControlInit(); // 没有注册DM电机则不会执行 } __attribute__((noreturn)) void StartINSTASK(void const *argument) diff --git a/bsp/can/bsp_can.c b/bsp/can/bsp_can.c index 9e32a13..15bfdda 100644 --- a/bsp/can/bsp_can.c +++ b/bsp/can/bsp_can.c @@ -112,7 +112,7 @@ uint8_t CANTransmit(CANInstance *_instance, float timeout) { if (DWT_GetTimeline_ms() - dwt_start > timeout) // 超时 { - LOGWARNING("[bsp_can] CAN MAILbox full! failed to add msg to mailbox. Cnt [%d]", busy_count); + LOGWARNING("[bsp_can] CAN MAILbox full! failed to add msg to mailbox."); busy_count++; return 0; } diff --git a/engineering.jdebug.user b/engineering.jdebug.user index eedf644..e5aa726 100644 --- a/engineering.jdebug.user +++ b/engineering.jdebug.user @@ -1,35 +1,51 @@ -GraphedExpression="(lift_position_loop).Ref", Color=#e56a6f -GraphedExpression="(lift_position_loop).Measure", Color=#35792b -GraphedExpression="(lift_position_loop).Output", Color=#769dda + +Breakpoint=D:/CLion/Project/engineering/application/gimbal/gimbal.cpp:259:5, State=BP_STATE_DISABLED +GraphedExpression="(((yaw_motor)->motor_controller).speed_PID).Ref", Color=#e56a6f +GraphedExpression="(((yaw_motor)->motor_controller).speed_PID).Measure", Color=#35792b +GraphedExpression="(((yaw_motor)->motor_controller).angle_PID).Ref", Color=#769dda +GraphedExpression="(((yaw_motor)->motor_controller).angle_PID).Measure", Color=#b14f0d OpenDocument="main.c", FilePath="D:/CLion/Project/engineering/Src/main.c", Line=69 -OpenDocument="to_stretch.c", FilePath="D:/CLion/Project/engineering/application/to_stretch/to_stretch.c", Line=0 -OpenDocument="tasks.c", FilePath="D:/CLion/Project/engineering/Middlewares/Third_Party/FreeRTOS/Source/tasks.c", Line=3399 -OpenDocument="startup_stm32f407ighx.s", FilePath="D:/CLion/Project/engineering/Startup/startup_stm32f407ighx.s", Line=51 +OpenDocument="tasks.c", FilePath="D:/CLion/Project/engineering/Middlewares/Third_Party/FreeRTOS/Source/tasks.c", Line=3422 OpenDocument="arm_mat_mult_f32.c", FilePath="C:/Users/clamar01/fork3/CMSIS_5/CMSIS/DSP/Source/MatrixFunctions/arm_mat_mult_f32.c", Line=0 +OpenDocument="bsp_dwt.c", FilePath="D:/CLion/Project/engineering/bsp/dwt/bsp_dwt.c", Line=0 +OpenDocument="stm32f4xx_it.c", FilePath="D:/CLion/Project/engineering/Src/stm32f4xx_it.c", Line=107 +OpenDocument="startup_stm32f407ighx.s", FilePath="D:/CLion/Project/engineering/Startup/startup_stm32f407ighx.s", Line=51 +OpenDocument="stm32f4xx_hal.c", FilePath="D:/CLion/Project/engineering/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal.c", Line=315 +OpenDocument="gimbal.cpp", FilePath="D:/CLion/Project/engineering/application/gimbal/gimbal.cpp", Line=153 +OpenDocument="stm32f4xx_hal_spi.c", FilePath="D:/CLion/Project/engineering/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_spi.c", Line=1255 +OpenDocument="to_stretch.c", FilePath="D:/CLion/Project/engineering/application/to_stretch/to_stretch.c", Line=0 +OpenDocument="stm32f4xx_hal_can.c", FilePath="D:/CLion/Project/engineering/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_can.c", Line=1412 +OpenDocument="chassis.c", FilePath="D:/CLion/Project/engineering/application/chassis/chassis.c", Line=165 OpenToolbar="Debug", Floating=0, x=0, y=0 -OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=603, h=208, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 -OpenWindow="Memory 1", DockArea=BOTTOM, x=3, y=0, w=398, h=245, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, EditorAddress=0x30A3D70C -OpenWindow="Watched Data 1", DockArea=LEFT, x=0, y=1, w=603, h=470, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 -OpenWindow="Terminal", DockArea=BOTTOM, x=0, y=0, w=1046, h=245, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 -OpenWindow="Data Sampling", DockArea=BOTTOM, x=2, y=0, w=264, h=245, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0 -OpenWindow="Timeline", DockArea=RIGHT, x=0, y=0, w=1011, h=679, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="2 s", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="0;472", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="717;0", CodeGraphLegendShown=0, CodeGraphLegendPosition="238;0" -OpenWindow="Console", DockArea=BOTTOM, x=1, y=0, w=209, h=245, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 +OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=602, h=160, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 +OpenWindow="Memory 1", DockArea=BOTTOM, x=3, y=0, w=401, h=282, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, EditorAddress=0x30A3D70C +OpenWindow="Watched Data 1", DockArea=LEFT, x=0, y=1, w=602, h=481, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 +OpenWindow="Terminal", DockArea=BOTTOM, x=0, y=0, w=1041, h=282, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 +OpenWindow="Data Sampling", DockArea=BOTTOM, x=2, y=0, w=265, h=282, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0 +OpenWindow="Timeline", DockArea=RIGHT, x=0, y=0, w=672, h=642, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="5 s", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="0;66", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="313;0", CodeGraphLegendShown=0, CodeGraphLegendPosition="326;0" +OpenWindow="Console", DockArea=BOTTOM, x=1, y=0, w=210, h=282, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 SmartViewPlugin="", Page="", Toolbar="Hidden", Window="SmartView 1" -TableHeader="Source Files", SortCol="File", SortOrder="ASCENDING", VisibleCols=["File";"Status";"Size";"#Insts";"Path"], ColWidths=[263;100;100;100;770] -TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";" (lift_position_loop).Ref";" (lift_position_loop).Measure";" (lift_position_loop).Output"], ColWidths=[100;100;100;100;100] -TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[438;100;118;118;110;110;100;118;110] +TableHeader="Source Files", SortCol="File", SortOrder="ASCENDING", VisibleCols=["File";"Status";"Size";"#Insts";"Path"], ColWidths=[263;100;100;100;889] +TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";" (((yaw_motor)->motor_controller).speed_PID).Ref";" (((yaw_motor)->motor_controller).speed_PID).Measure";" (((yaw_motor)->motor_controller).angle_PID).Ref";" (((yaw_motor)->motor_controller).angle_PID).Measure"], ColWidths=[100;100;100;100;100;100] +TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[438;100;126;110;102;126;100;118;110] TableHeader="Power Sampling", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";"Ch 0"], ColWidths=[100;100;100] TableHeader="Task List", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Name";"Run Count";"Priority";"Status";"Timeout";"Stack Info";"ID";"Mutex Count";"Notified Value";"Notify State"], ColWidths=[110;110;110;110;110;110;110;110;110;110] +TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh";"Access"], ColWidths=[257;124;104;117;100] TableHeader="RegisterSelectionDialog", SortCol="None", SortOrder="ASCENDING", VisibleCols=[], ColWidths=[] TableHeader="TargetExceptionDialog", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Address";"Description"], ColWidths=[200;100;100;351] -TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh"], ColWidths=[257;124;104;100] -WatchedExpression="chassis_cmd_recv", RefreshRate=2, Window=Watched Data 1 -WatchedExpression="motor_ld", RefreshRate=2, Window=Watched Data 1 -WatchedExpression="motor_rd", RefreshRate=2, Window=Watched Data 1 -WatchedExpression="gravity_feedforward", RefreshRate=2, Window=Watched Data 1 -WatchedExpression="motor_lu", Window=Watched Data 1 -WatchedExpression="motor_ru", Window=Watched Data 1 -WatchedExpression="protract_position_loop", RefreshRate=2, Window=Watched Data 1 -WatchedExpression="lift_position_loop", RefreshRate=2, Window=Watched Data 1 \ No newline at end of file +WatchedExpression="diff_r_motor", RefreshRate=2, Window=Watched Data 1 +WatchedExpression="diff_l_motor", RefreshRate=2, Window=Watched Data 1 +WatchedExpression="diff_pitch_loop", RefreshRate=2, Window=Watched Data 1 +WatchedExpression="diff_pitch_angle", RefreshRate=2, Window=Watched Data 1 +WatchedExpression="diff_pitch_angle", Window=Watched Data 1 +WatchedExpression="diff_roll_loop", Window=Watched Data 1 +WatchedExpression="diff_pitch_spd_loop", RefreshRate=2, Window=Watched Data 1 +WatchedExpression="diff_roll_spd_loop", RefreshRate=2, Window=Watched Data 1 +WatchedExpression="(diff_roll_spd_loop).Measure", Window=Watched Data 1 +WatchedExpression="gimbal_cmd_recv", RefreshRate=2, Window=Watched Data 1 +WatchedExpression="yaw_motor", RefreshRate=2, Window=Watched Data 1 +WatchedExpression="roll_motor", RefreshRate=2, Window=Watched Data 1 +WatchedExpression="(((roll_motor)->motor_controller).speed_PID).Ref", Window=Watched Data 1 +WatchedExpression="roll_spd_filter", RefreshRate=2, Window=Watched Data 1 \ No newline at end of file diff --git a/modules/motor/DMmotor/dmmotor.c b/modules/motor/DMmotor/dmmotor.c index 6420394..4256e17 100644 --- a/modules/motor/DMmotor/dmmotor.c +++ b/modules/motor/DMmotor/dmmotor.c @@ -58,12 +58,12 @@ static void DMMotorDecode(CANInstance *motor_can) measure->T_Rotor = (float)rxbuff[7]; // 多圈角度计算,前提是假设两次采样间电机转过的角度小于180°,自己画个图就清楚计算过程了 - if (measure->position - measure->last_position > 3.1425926) + if (measure->position - measure->last_position > PI) //3.1425926 measure->total_round--; - else if (measure->position - measure->last_position < -3.1415926) + else if (measure->position - measure->last_position < -PI) measure->total_round++; - measure->total_angle = measure->total_round * 360 + measure->angle_single_round; + measure->total_angle = measure->total_round * (DM_P_MAX * DEGREE_2_RAD) + measure->angle_single_round; } static void DMMotorLostCallback(void *motor_ptr) @@ -91,6 +91,9 @@ DMMotorInstance *DMMotorInit(Motor_Init_Config_s *config) motor->motor_controller.other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr; motor->motor_controller.other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr; + motor->motor_controller.current_feedforward_ptr = config->controller_param_init_config.current_feedforward_ptr; + motor->motor_controller.speed_feedforward_ptr = config->controller_param_init_config.speed_feedforward_ptr; + config->can_init_config.can_module_callback = DMMotorDecode; config->can_init_config.id = motor; motor->motor_can_instance = CANRegister(&config->can_init_config); @@ -157,7 +160,7 @@ void DMMotorTask(void const *argument) if (setting->angle_feedback_source == OTHER_FEED) pid_measure = *motor_controller->other_angle_feedback_ptr; else - pid_measure = measure->total_angle; // MOTOR_FEED,对total angle闭环,防止在边界处出现突跃 + pid_measure = measure->position; // MOTOR_FEED,对total angle闭环,防止在边界处出现突跃 // 更新pid_ref进入下一个环 pid_ref = PIDCalculate(&motor_controller->angle_PID, pid_measure, pid_ref); } @@ -176,6 +179,9 @@ void DMMotorTask(void const *argument) // 更新pid_ref进入下一个环 pid_ref = PIDCalculate(&motor_controller->speed_PID, pid_measure, pid_ref); } + // 电流环前馈 + if (setting->feedforward_flag & CURRENT_FEEDFORWARD) + pid_ref += *motor_controller->current_feedforward_ptr; if (setting->feedback_reverse_flag == FEEDBACK_DIRECTION_REVERSE) pid_ref *= -1; @@ -193,7 +199,6 @@ void DMMotorTask(void const *argument) motor_send_mailbox.Kd = 0;//只使用力矩控制无需kpkd参数 - motor->motor_can_instance->tx_buff[0] = (uint8_t)(motor_send_mailbox.position_des >> 8); motor->motor_can_instance->tx_buff[1] = (uint8_t)(motor_send_mailbox.position_des); motor->motor_can_instance->tx_buff[2] = (uint8_t)(motor_send_mailbox.velocity_des >> 4); @@ -204,8 +209,6 @@ void DMMotorTask(void const *argument) motor->motor_can_instance->tx_buff[7] = (uint8_t)(motor_send_mailbox.torque_des); - - CANTransmit(motor->motor_can_instance, 1); osDelay(2); diff --git a/modules/motor/DMmotor/dmmotor.h b/modules/motor/DMmotor/dmmotor.h index 4d74eea..68c8fe0 100644 --- a/modules/motor/DMmotor/dmmotor.h +++ b/modules/motor/DMmotor/dmmotor.h @@ -6,10 +6,10 @@ #include "motor_def.h" #include "daemon.h" -#define DM_MOTOR_CNT 1 +#define DM_MOTOR_CNT 6 -#define DM_P_MIN (-3.1415926f) -#define DM_P_MAX 3.1415926f +#define DM_P_MIN (-12.5f)//(-3.1415926f) +#define DM_P_MAX 12.5f//3.1415926f #define DM_V_MIN (-45.0f) #define DM_V_MAX 45.0f #define DM_T_MIN (-18.0f) diff --git a/modules/motor/motor_def.h b/modules/motor/motor_def.h index f97b6b9..13ec41a 100644 --- a/modules/motor/motor_def.h +++ b/modules/motor/motor_def.h @@ -126,6 +126,8 @@ typedef enum M2006, LK9025, HT04, + DM4310, + DM6006, } Motor_Type_e; /** diff --git a/modules/motor/motor_task.c b/modules/motor/motor_task.c index 47ff232..17c281e 100644 --- a/modules/motor/motor_task.c +++ b/modules/motor/motor_task.c @@ -13,7 +13,7 @@ void MotorControlTask() DJIMotorControl(); /* 如果有对应的电机则取消注释,可以加入条件编译或者register对应的idx判断是否注册了电机 */ - LKMotorControl(); + //LKMotorControl(); // legacy support // 由于ht04电机的反馈方式为接收到一帧消息后立刻回传,以此方式连续发送可能导致总线拥塞 diff --git a/modules/referee/referee_VT.c b/modules/referee/referee_VT.c new file mode 100644 index 0000000..6eaaf5d --- /dev/null +++ b/modules/referee/referee_VT.c @@ -0,0 +1,112 @@ +// +// Created by SJQ on 2024/4/28. +// + +#include "referee_VT.h" + +/** + * @file rm_referee.C + * @author kidneygood (you@domain.com) + * @brief + * @version 0.1 + * @date 2022-11-18 + * + * @copyright Copyright (c) 2022 + * + */ + +#include "rm_referee.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" + +#define RE_RX_BUFFER_SIZE 255u // 裁判系统接收缓冲区大小 + +static USARTInstance *VT_usart_instance; // 裁判系统串口实例 +static DaemonInstance *VT_daemon; // 裁判系统守护进程 +static VT_info_t VT_info; // 裁判系统数据 + +/** + * @brief 读取图传链路数据,中断中读取保证速度 + * @param buff: 读取到的裁判系统原始数据 + * @retval 是否对正误判断做处理 + * @attention 在此判断帧头和CRC校验,无误再写入数据，不重复判断帧头 + */ +static void VTJudgeReadData(uint8_t *buff) +{ + uint16_t judge_length; // 统计一帧数据长度 + if (buff == NULL) // 空数据包，则不作任何处理 + return; + + // 写入帧头数据(5-byte),用于判断是否开始存储裁判数据 + memcpy(&VT_info.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 + VT_info.CmdID = (buff[6] << 8 | buff[5]); + // 解析数据命令码,将数据拷贝到相应结构体中(注意拷贝数据的长度) + // 第8个字节开始才是数据 data=7 + switch (VT_info.CmdID) + { + case ID_remote_control: // 0x0304 + memcpy(&VT_info.RemoteControl, (buff + DATA_Offset), LEN_remote_control); + break; + case ID_custom_robot: // 0x0302 + memcpy(&VT_info.CustomControl, (buff + DATA_Offset), LEN_custom_control); + break; + } + } + } + // 首地址加帧长度,指向CRC16下一字节,用来判断是否为0xA5,从而判断一个数据包是否有多帧数据 + if (*(buff + sizeof(xFrameHeader) + LEN_CMDID + VT_info.FrameHeader.DataLength + LEN_TAIL) == 0xA5) + { // 如果一个数据包出现了多帧数据,则再次调用解析函数,直到所有数据包解析完毕 + VTJudgeReadData(buff + sizeof(xFrameHeader) + LEN_CMDID + VT_info.FrameHeader.DataLength + LEN_TAIL); + } + } +} + +/*裁判系统串口接收回调函数,解析数据 */ +static void VTRxCallback() +{ + DaemonReload(VT_daemon); + VTJudgeReadData(VT_usart_instance->recv_buff); +} +// 裁判系统丢失回调函数,重新初始化裁判系统串口 +static void VTLostCallback(void *arg) +{ + USARTServiceInit(VT_usart_instance); + LOGWARNING("[rm_vt] lost vt data"); +} + +/* 裁判系统通信初始化 */ +VT_info_t *VTInit(UART_HandleTypeDef *VT_usart_handle) +{ + USART_Init_Config_s conf; + conf.module_callback = VTRxCallback; + 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 = VTLostCallback, + .owner_id = VT_usart_instance, + .reload_count = 30, // 0.3s没有收到数据,则认为丢失,重启串口接收 + }; + VT_daemon = DaemonRegister(&daemon_conf); + + return &VT_info; +} \ No newline at end of file diff --git a/modules/referee/referee_VT.h b/modules/referee/referee_VT.h new file mode 100644 index 0000000..f72040e --- /dev/null +++ b/modules/referee/referee_VT.h @@ -0,0 +1,16 @@ +// +// Created by SJQ on 2024/4/28. +// + +#ifndef BASIC_FRAMEWORK_REFEREE_VT_H +#define BASIC_FRAMEWORK_REFEREE_VT_H +#include "rm_referee.h" +/** + * @brief 图传链路通信初始化,该函数会初始化裁判系统串口,开启中断 + * + * @param referee_usart_handle 串口handle,C板一般用串口1（丝印为uart2） + * @return VT_info_t* 返回图传链路发来的数据,包括键鼠、自定义控制器 + */ +VT_info_t *VTInit(UART_HandleTypeDef *VT_usart_handle); + +#endif //BASIC_FRAMEWORK_REFEREE_VT_H diff --git a/modules/referee/referee_protocol.h b/modules/referee/referee_protocol.h index 669a712..947227f 100644 --- a/modules/referee/referee_protocol.h +++ b/modules/referee/referee_protocol.h @@ -27,19 +27,19 @@ /* 通信协议格式偏移，枚举类型,代替#define声明 */ typedef enum { - FRAME_HEADER_Offset = 0, - CMD_ID_Offset = 5, - DATA_Offset = 7, + FRAME_HEADER_Offset = 0, + CMD_ID_Offset = 5, + DATA_Offset = 7, } JudgeFrameOffset_e; /* 通信协议长度 */ typedef enum { - LEN_HEADER = 5, // 帧头长 - LEN_CMDID = 2, // 命令码长度 - LEN_TAIL = 2, // 帧尾CRC16 + LEN_HEADER = 5, // 帧头长 + LEN_CMDID = 2, // 命令码长度 + LEN_TAIL = 2, // 帧尾CRC16 - LEN_CRC8 = 4, // 帧头CRC8校验长度=帧头+数据长+包序号 + LEN_CRC8 = 4, // 帧头CRC8校验长度=帧头+数据长+包序号 } JudgeFrameLength_e; /****************************帧头****************************/ @@ -48,19 +48,19 @@ typedef enum /* 帧头偏移 */ typedef enum { - SOF = 0, // 起始位 - DATA_LENGTH = 1, // 帧内数据长度,根据这个来获取数据长度 - SEQ = 3, // 包序号 - CRC8 = 4 // CRC8 + SOF = 0, // 起始位 + DATA_LENGTH = 1, // 帧内数据长度,根据这个来获取数据长度 + SEQ = 3, // 包序号 + CRC8 = 4 // CRC8 } FrameHeaderOffset_e; /* 帧头定义 */ typedef struct { - uint8_t SOF; - uint16_t DataLength; - uint8_t Seq; - uint8_t CRC8; + uint8_t SOF; + uint16_t DataLength; + uint8_t Seq; + uint8_t CRC8; } xFrameHeader; /****************************cmd_id命令码说明****************************/ @@ -69,38 +69,43 @@ typedef struct /* 命令码ID,用来判断接收的是什么数据 */ typedef enum { - ID_game_state = 0x0001, // 比赛状态数据 - ID_game_result = 0x0002, // 比赛结果数据 - ID_game_robot_survivors = 0x0003, // 比赛机器人血量数据 - ID_event_data = 0x0101, // 场地事件数据 - ID_supply_projectile_action = 0x0102, // 场地补给站动作标识数据 - ID_supply_projectile_booking = 0x0103, // 场地补给站预约子弹数据 - ID_game_robot_state = 0x0201, // 机器人状态数据 - ID_power_heat_data = 0x0202, // 实时功率热量数据 - ID_game_robot_pos = 0x0203, // 机器人位置数据 - ID_buff_musk = 0x0204, // 机器人增益数据 - ID_aerial_robot_energy = 0x0205, // 空中机器人能量状态数据 - ID_robot_hurt = 0x0206, // 伤害状态数据 - ID_shoot_data = 0x0207, // 实时射击数据 - ID_student_interactive = 0x0301, // 机器人间交互数据 + ID_game_state = 0x0001, // 比赛状态数据 + ID_game_result = 0x0002, // 比赛结果数据 + ID_game_robot_survivors = 0x0003, // 比赛机器人血量数据 + ID_event_data = 0x0101, // 场地事件数据 + ID_supply_projectile_action = 0x0102, // 场地补给站动作标识数据 + ID_supply_projectile_booking = 0x0103, // 场地补给站预约子弹数据 + ID_game_robot_state = 0x0201, // 机器人状态数据 + ID_power_heat_data = 0x0202, // 实时功率热量数据 + ID_game_robot_pos = 0x0203, // 机器人位置数据 + ID_buff_musk = 0x0204, // 机器人增益数据 + ID_aerial_robot_energy = 0x0205, // 空中机器人能量状态数据 + ID_robot_hurt = 0x0206, // 伤害状态数据 + ID_shoot_data = 0x0207, // 实时射击数据 + ID_student_interactive = 0x0301, // 机器人间交互数据 + ID_custom_robot = 0x302, // 自定义控制器数据（图传链路） + ID_remote_control = 0x304, // 键鼠遥控数据（图传链路） } CmdID_e; /* 命令码数据段长,根据官方协议来定义长度，还有自定义数据长度 */ typedef enum { - LEN_game_state = 3, // 0x0001 - LEN_game_result = 1, // 0x0002 - LEN_game_robot_HP = 2, // 0x0003 - LEN_event_data = 4, // 0x0101 - LEN_supply_projectile_action = 4, // 0x0102 - LEN_game_robot_state = 27, // 0x0201 - LEN_power_heat_data = 14, // 0x0202 - LEN_game_robot_pos = 16, // 0x0203 - LEN_buff_musk = 1, // 0x0204 - LEN_aerial_robot_energy = 1, // 0x0205 - LEN_robot_hurt = 1, // 0x0206 - LEN_shoot_data = 7, // 0x0207 - LEN_receive_data = 6 + Communicate_Data_LEN, // 0x0301 + LEN_game_state = 3, // 0x0001 + LEN_game_result = 1, // 0x0002 + LEN_game_robot_HP = 2, // 0x0003 + LEN_event_data = 4, // 0x0101 + LEN_supply_projectile_action = 4, // 0x0102 + LEN_game_robot_state = 27, // 0x0201 + LEN_power_heat_data = 14, // 0x0202 + LEN_game_robot_pos = 16, // 0x0203 + LEN_buff_musk = 1, // 0x0204 + LEN_aerial_robot_energy = 1, // 0x0205 + LEN_robot_hurt = 1, // 0x0206 + LEN_shoot_data = 7, // 0x0207 + LEN_receive_data = 6 + Communicate_Data_LEN, // 0x0301 + + LEN_custom_control = 30, // 0x0302 + LEN_remote_control = 12, // 0x0304 } JudgeDataLength_e; @@ -110,176 +115,201 @@ typedef enum /* ID: 0x0001 Byte: 3 比赛状态数据 */ typedef struct { - uint8_t game_type : 4; - uint8_t game_progress : 4; - uint16_t stage_remain_time; + uint8_t game_type : 4; + uint8_t game_progress : 4; + uint16_t stage_remain_time; } ext_game_state_t; /* ID: 0x0002 Byte: 1 比赛结果数据 */ typedef struct { - uint8_t winner; + uint8_t winner; } ext_game_result_t; /* ID: 0x0003 Byte: 32 比赛机器人血量数据 */ typedef struct { - uint16_t red_1_robot_HP; - uint16_t red_2_robot_HP; - uint16_t red_3_robot_HP; - uint16_t red_4_robot_HP; - uint16_t red_5_robot_HP; - uint16_t red_7_robot_HP; - uint16_t red_outpost_HP; - uint16_t red_base_HP; - uint16_t blue_1_robot_HP; - uint16_t blue_2_robot_HP; - uint16_t blue_3_robot_HP; - uint16_t blue_4_robot_HP; - uint16_t blue_5_robot_HP; - uint16_t blue_7_robot_HP; - uint16_t blue_outpost_HP; - uint16_t blue_base_HP; + uint16_t red_1_robot_HP; + uint16_t red_2_robot_HP; + uint16_t red_3_robot_HP; + uint16_t red_4_robot_HP; + uint16_t red_5_robot_HP; + uint16_t red_7_robot_HP; + uint16_t red_outpost_HP; + uint16_t red_base_HP; + uint16_t blue_1_robot_HP; + uint16_t blue_2_robot_HP; + uint16_t blue_3_robot_HP; + uint16_t blue_4_robot_HP; + uint16_t blue_5_robot_HP; + uint16_t blue_7_robot_HP; + uint16_t blue_outpost_HP; + uint16_t blue_base_HP; } ext_game_robot_HP_t; /* ID: 0x0101 Byte: 4 场地事件数据 */ typedef struct { - uint32_t event_type; + uint32_t event_type; } ext_event_data_t; /* ID: 0x0102 Byte: 3 场地补给站动作标识数据 */ typedef struct { - uint8_t supply_projectile_id; - uint8_t supply_robot_id; - uint8_t supply_projectile_step; - uint8_t supply_projectile_num; + uint8_t supply_projectile_id; + uint8_t supply_robot_id; + uint8_t supply_projectile_step; + uint8_t supply_projectile_num; } ext_supply_projectile_action_t; -/* ID: 0X0201 Byte: 13 V1.6.1 机器人性能体系数据 */ +/* ID: 0X0201 Byte: 27 机器人状态数据 */ typedef struct { uint8_t robot_id; uint8_t robot_level; - uint16_t current_HP; - uint16_t maximum_HP; - uint16_t shooter_barrel_cooling_value; - uint16_t shooter_barrel_heat_limit; + 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 chassis_power_limit; - - uint8_t power_management_gimbal_output : 1; - uint8_t power_management_chassis_output : 1; - uint8_t power_management_shooter_output : 1; + uint8_t mains_power_gimbal_output : 1; + uint8_t mains_power_chassis_output : 1; + uint8_t mains_power_shooter_output : 1; } ext_game_robot_state_t; -/* ID: 0X0202 Byte: 14 v1.6.1 实时功率热量数据 */ +/* ID: 0X0202 Byte: 14 实时功率热量数据 */ typedef struct { - uint16_t chassis_voltage; + uint16_t chassis_volt; uint16_t chassis_current; - float chassis_power; - uint16_t buffer_energy; - uint16_t shooter_17mm_1_barrel_heat; - uint16_t shooter_17mm_2_barrel_heat; - uint16_t shooter_42mm_barrel_heat; + float chassis_power; // 瞬时功率 + uint16_t chassis_power_buffer; // 60焦耳缓冲能量 + uint16_t shooter_heat0; // 17mm + uint16_t shooter_heat1; } ext_power_heat_data_t; /* ID: 0x0203 Byte: 16 机器人位置数据 */ typedef struct { - float x; - float y; - float z; - float yaw; + float x; + float y; + float z; + float yaw; } ext_game_robot_pos_t; /* ID: 0x0204 Byte: 1 机器人增益数据 */ typedef struct { - uint8_t power_rune_buff; + uint8_t power_rune_buff; } ext_buff_musk_t; /* ID: 0x0205 Byte: 1 空中机器人能量状态数据 */ typedef struct { - uint8_t attack_time; + uint8_t attack_time; } aerial_robot_energy_t; /* ID: 0x0206 Byte: 1 伤害状态数据 */ typedef struct { - uint8_t armor_id : 4; - uint8_t hurt_type : 4; + uint8_t armor_id : 4; + uint8_t hurt_type : 4; } ext_robot_hurt_t; /* ID: 0x0207 Byte: 7 实时射击数据 */ typedef struct { - uint8_t bullet_type; - uint8_t shooter_id; - uint8_t bullet_freq; - float bullet_speed; + uint8_t bullet_type; + uint8_t shooter_id; + uint8_t bullet_freq; + float bullet_speed; } ext_shoot_data_t; +/****************************图传链路数据****************************/ +/* ID: 0x0302 Byte: 30 自定义控制器数据 */ +typedef struct +{ + uint8_t data[30]; +}custom_robot_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; +}remote_control_t; +/****************************图传链路数据****************************/ + /****************************机器人交互数据****************************/ /****************************机器人交互数据****************************/ /* 发送的内容数据段最大为 113 检测是否超出大小限制?实际上图形段不会超，数据段最多30个，也不会超*/ /* 交互数据头结构 */ typedef struct { - uint16_t data_cmd_id; // 由于存在多个内容 ID，但整个cmd_id 上行频率最大为 10Hz，请合理安排带宽。注意交互部分的上行频率 - uint16_t sender_ID; - uint16_t receiver_ID; + uint16_t data_cmd_id; // 由于存在多个内容 ID，但整个cmd_id 上行频率最大为 10Hz，请合理安排带宽。注意交互部分的上行频率 + uint16_t sender_ID; + uint16_t receiver_ID; } ext_student_interactive_header_data_t; /* 机器人id */ typedef enum { - // 红方机器人ID - RobotID_RHero = 1, - RobotID_REngineer = 2, - RobotID_RStandard1 = 3, - RobotID_RStandard2 = 4, - RobotID_RStandard3 = 5, - RobotID_RAerial = 6, - RobotID_RSentry = 7, - RobotID_RRadar = 9, - // 蓝方机器人ID - RobotID_BHero = 101, - RobotID_BEngineer = 102, - RobotID_BStandard1 = 103, - RobotID_BStandard2 = 104, - RobotID_BStandard3 = 105, - RobotID_BAerial = 106, - RobotID_BSentry = 107, - RobotID_BRadar = 109, + // 红方机器人ID + RobotID_RHero = 1, + RobotID_REngineer = 2, + RobotID_RStandard1 = 3, + RobotID_RStandard2 = 4, + RobotID_RStandard3 = 5, + RobotID_RAerial = 6, + RobotID_RSentry = 7, + RobotID_RRadar = 9, + // 蓝方机器人ID + RobotID_BHero = 101, + RobotID_BEngineer = 102, + RobotID_BStandard1 = 103, + RobotID_BStandard2 = 104, + RobotID_BStandard3 = 105, + RobotID_BAerial = 106, + RobotID_BSentry = 107, + RobotID_BRadar = 109, } Robot_ID_e; /* 交互数据ID */ typedef enum { - UI_Data_ID_Del = 0x100, - UI_Data_ID_Draw1 = 0x101, - UI_Data_ID_Draw2 = 0x102, - UI_Data_ID_Draw5 = 0x103, - UI_Data_ID_Draw7 = 0x104, - UI_Data_ID_DrawChar = 0x110, + UI_Data_ID_Del = 0x100, + UI_Data_ID_Draw1 = 0x101, + UI_Data_ID_Draw2 = 0x102, + UI_Data_ID_Draw5 = 0x103, + UI_Data_ID_Draw7 = 0x104, + UI_Data_ID_DrawChar = 0x110, - /* 自定义交互数据部分 */ - Communicate_Data_ID = 0x0200, + /* 自定义交互数据部分 */ + Communicate_Data_ID = 0x0200, } Interactive_Data_ID_e; /* 交互数据长度 */ typedef enum { - Interactive_Data_LEN_Head = 6, - UI_Operate_LEN_Del = 2, - UI_Operate_LEN_PerDraw = 15, - UI_Operate_LEN_DrawChar = 15 + 30, + Interactive_Data_LEN_Head = 6, + UI_Operate_LEN_Del = 2, + UI_Operate_LEN_PerDraw = 15, + UI_Operate_LEN_DrawChar = 15 + 30, - /* 自定义交互数据部分 */ - // Communicate_Data_LEN = 5, + /* 自定义交互数据部分 */ + // Communicate_Data_LEN = 5, } Interactive_Data_Length_e; @@ -292,23 +322,23 @@ typedef enum // 自定义交互数据协议，可更改，更改后需要修改最上方宏定义数据长度的值 typedef struct { - uint8_t data[Communicate_Data_LEN]; // 数据段,n需要小于113 + uint8_t data[Communicate_Data_LEN]; // 数据段,n需要小于113 } robot_interactive_data_t; // 机器人交互信息_发送 typedef struct { - xFrameHeader FrameHeader; - uint16_t CmdID; - ext_student_interactive_header_data_t datahead; - robot_interactive_data_t Data; // 数据段 - uint16_t frametail; + xFrameHeader FrameHeader; + uint16_t CmdID; + ext_student_interactive_header_data_t datahead; + robot_interactive_data_t Data; // 数据段 + uint16_t frametail; } Communicate_SendData_t; // 机器人交互信息_接收 typedef struct { - ext_student_interactive_header_data_t datahead; - robot_interactive_data_t Data; // 数据段 + ext_student_interactive_header_data_t datahead; + robot_interactive_data_t Data; // 数据段 } Communicate_ReceiveData_t; /****************************UI交互数据****************************/ @@ -316,69 +346,69 @@ typedef struct /* 图形数据 */ typedef struct { - uint8_t graphic_name[3]; - uint32_t operate_tpye : 3; - uint32_t graphic_tpye : 3; - uint32_t layer : 4; - uint32_t color : 4; - uint32_t start_angle : 9; - uint32_t end_angle : 9; - uint32_t width : 10; - uint32_t start_x : 11; - uint32_t start_y : 11; - uint32_t radius : 10; - uint32_t end_x : 11; - uint32_t end_y : 11; + uint8_t graphic_name[3]; + uint32_t operate_tpye : 3; + uint32_t graphic_tpye : 3; + uint32_t layer : 4; + uint32_t color : 4; + uint32_t start_angle : 9; + uint32_t end_angle : 9; + uint32_t width : 10; + uint32_t start_x : 11; + uint32_t start_y : 11; + uint32_t radius : 10; + uint32_t end_x : 11; + uint32_t end_y : 11; } Graph_Data_t; typedef struct { - Graph_Data_t Graph_Control; - uint8_t show_Data[30]; + Graph_Data_t Graph_Control; + uint8_t show_Data[30]; } String_Data_t; // 打印字符串数据 /* 删除操作 */ typedef enum { - UI_Data_Del_NoOperate = 0, - UI_Data_Del_Layer = 1, - UI_Data_Del_ALL = 2, // 删除全部图层，后面的参数已经不重要了。 + UI_Data_Del_NoOperate = 0, + UI_Data_Del_Layer = 1, + UI_Data_Del_ALL = 2, // 删除全部图层，后面的参数已经不重要了。 } UI_Delete_Operate_e; /* 图形配置参数__图形操作 */ typedef enum { - UI_Graph_ADD = 1, - UI_Graph_Change = 2, - UI_Graph_Del = 3, + UI_Graph_ADD = 1, + UI_Graph_Change = 2, + UI_Graph_Del = 3, } UI_Graph_Operate_e; /* 图形配置参数__图形类型 */ typedef enum { - UI_Graph_Line = 0, // 直线 - UI_Graph_Rectangle = 1, // 矩形 - UI_Graph_Circle = 2, // 整圆 - UI_Graph_Ellipse = 3, // 椭圆 - UI_Graph_Arc = 4, // 圆弧 - UI_Graph_Float = 5, // 浮点型 - UI_Graph_Int = 6, // 整形 - UI_Graph_Char = 7, // 字符型 + UI_Graph_Line = 0, // 直线 + UI_Graph_Rectangle = 1, // 矩形 + UI_Graph_Circle = 2, // 整圆 + UI_Graph_Ellipse = 3, // 椭圆 + UI_Graph_Arc = 4, // 圆弧 + UI_Graph_Float = 5, // 浮点型 + UI_Graph_Int = 6, // 整形 + UI_Graph_Char = 7, // 字符型 } UI_Graph_Type_e; /* 图形配置参数__图形颜色 */ typedef enum { - UI_Color_Main = 0, // 红蓝主色 - UI_Color_Yellow = 1, - UI_Color_Green = 2, - UI_Color_Orange = 3, - UI_Color_Purplish_red = 4, // 紫红色 - UI_Color_Pink = 5, - UI_Color_Cyan = 6, // 青色 - UI_Color_Black = 7, - UI_Color_White = 8, + UI_Color_Main = 0, // 红蓝主色 + UI_Color_Yellow = 1, + UI_Color_Green = 2, + UI_Color_Orange = 3, + UI_Color_Purplish_red = 4, // 紫红色 + UI_Color_Pink = 5, + UI_Color_Cyan = 6, // 青色 + UI_Color_Black = 7, + UI_Color_White = 8, } UI_Graph_Color_e; diff --git a/modules/referee/rm_referee.h b/modules/referee/rm_referee.h index adbf848..47ccdea 100644 --- a/modules/referee/rm_referee.h +++ b/modules/referee/rm_referee.h @@ -47,6 +47,21 @@ typedef struct } referee_info_t; +// 此结构体为图传链路传输的数据 +typedef struct +{ + referee_id_t referee_id; + xFrameHeader FrameHeader; // 接收到的帧头信息 + uint16_t CmdID; + + remote_control_t RemoteControl; // 0x304 + custom_robot_data_t CustomControl; // 0x302 + + uint8_t init_flag; + +} VT_info_t; + + // 模式是否切换标志位，0为未切换，1为切换，static定义默认为0 typedef struct {