zyx
/
engineering
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'origin/master' 

# Conflicts:
#	application/cmd/robot_cmd.c
#	engineering.jdebug.user
This commit is contained in:
zyx 2024-05-07 22:51:51 +08:00
parent e2e32f20df 7d9955f575
commit bde9237826
16 changed files with 706 additions and 258 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Src/main.c
View File

@ -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 */

63
application/cmd/robot_cmd.c
View File

@ -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,39 +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;
} else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘不动，伸缩，图传自由
{
chassis_cmd_send.chassis_mode = CHASSIS_ZERO_FORCE;
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
to_stretch_cmd_send.tc += 0.025F * (float) rc_data[TEMP].rc.rocker_r_;//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数值方向
chassis_cmd_send.wz = -1.0f * (float) rc_data[TEMP].rc.rocker_l_; //旋转
// 图传限位
if (to_stretch_cmd_send.tc >= TUCHUAN_MAX_ANGLE) to_stretch_cmd_send.tc = TUCHUAN_MAX_ANGLE;
if (to_stretch_cmd_send.tc <= TUCHUAN_MAX_ANGLE) to_stretch_cmd_send.tc = TUCHUAN_MIN_ANGLE;
} else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘平移，伸缩
{
chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
to_stretch_cmd_send.to_stretch_mode = TO_STRETCH_MODE;
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);
}

268
application/gimbal/gimbal.cpp
View File

@ -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_Improvement_e>(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_Improvement_e>(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<Closeloop_Type_e>(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_Improvement_e>(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_Improvement_e>(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<Closeloop_Type_e>(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_Improvement_e>(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_Improvement_e>(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<Closeloop_Type_e>(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_Improvement_e>(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_Improvement_e>(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_Improvement_e>(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_Improvement_e>(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;

12
application/robot.c
View File

@ -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
}

16
application/robot_def.h
View File

@ -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;用于计算云台偏转角度
@ -133,6 +146,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;

3
application/robot_task.h
View File

@ -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)

2
bsp/can/bsp_can.c
View File

@ -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;
}

46
engineering.jdebug.user
View File

@ -1,27 +1,35 @@
OpenDocument="main.c", FilePath="D:/zhandui/cqdm/engineering/Src/main.c", Line=65
OpenDocument="chassis.c", FilePath="D:/zhandui/cqdm/engineering/application/chassis/chassis.c", Line=94
OpenDocument="to_stretch.c", FilePath="D:/zhandui/cqdm/engineering/application/to_stretch/to_stretch.c", Line=215
GraphedExpression="(lift_position_loop).Ref", Color=#e56a6f
GraphedExpression="(lift_position_loop).Measure", Color=#35792b
GraphedExpression="(lift_position_loop).Output", Color=#769dda
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="arm_mat_mult_f32.c", FilePath="C:/Users/clamar01/fork3/CMSIS_5/CMSIS/DSP/Source/MatrixFunctions/arm_mat_mult_f32.c", Line=0
OpenToolbar="Debug", Floating=0, x=0, y=0
OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=649, h=340, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
OpenWindow="Disassembly", DockArea=RIGHT, x=0, y=1, w=1206, h=264, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
OpenWindow="Memory 1", DockArea=BOTTOM, x=3, y=0, w=546, h=384, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, EditorAddress=0x30A3D70C
OpenWindow="Watched Data 1", DockArea=LEFT, x=0, y=1, w=649, h=716, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
OpenWindow="Terminal", DockArea=BOTTOM, x=0, y=0, w=948, h=384, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
OpenWindow="Data Sampling", DockArea=BOTTOM, x=2, y=0, w=1098, h=384, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0
OpenWindow="Timeline", DockArea=RIGHT, x=0, y=0, w=1206, h=792, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="2 s / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="0;539", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="716;0", CodeGraphLegendShown=0, CodeGraphLegendPosition="659;0"
OpenWindow="Console", DockArea=BOTTOM, x=1, y=0, w=285, h=384, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=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
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;950]
TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time"], ColWidths=[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;100;100;100;100;110;126;126]
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="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"], ColWidths=[238;226;104;100]
TableHeader="RegisterSelectionDialog", SortCol="None", SortOrder="ASCENDING", VisibleCols=[], ColWidths=[]
TableHeader="TargetExceptionDialog", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Address";"Description"], ColWidths=[27;27;27;27]
WatchedExpression="motor_lf", RefreshRate=2, Window=Watched Data 1
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="tuchuan", RefreshRate=2, Window=Watched Data 1
WatchedExpression="to_stretch_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

17
modules/motor/DMmotor/dmmotor.c
View File

@ -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);

6
modules/motor/DMmotor/dmmotor.h
View File

@ -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)

2
modules/motor/motor_def.h
View File

@ -126,6 +126,8 @@ typedef enum
M2006,
LK9025,
HT04,
DM4310,
DM6006,
} Motor_Type_e;
/**

2
modules/motor/motor_task.c
View File

@ -13,7 +13,7 @@ void MotorControlTask()
DJIMotorControl();
/* 如果有对应的电机则取消注释,可以加入条件编译或者register对应的idx判断是否注册了电机 */
LKMotorControl();
//LKMotorControl();
// legacy support
// 由于ht04电机的反馈方式为接收到一帧消息后立刻回传,以此方式连续发送可能导致总线拥塞

112
modules/referee/referee_VT.c Normal file
View File

@ -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;
}

16
modules/referee/referee_VT.h Normal file
View File

@ -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

382
modules/referee/referee_protocol.h
View File

@ -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;

15
modules/referee/rm_referee.h
View File

@ -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
{