Merge remote-tracking branch 'origin/master'
# Conflicts: # application/cmd/robot_cmd.c # engineering.jdebug.user
This commit is contained in:
zyx
commit
bde9237826
|
|
@ -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 */
|
||||
|
|
|
|||
|
|
@ -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,40 +124,60 @@ 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
|
||||
|
||||
// 图传限位
|
||||
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;
|
||||
//伸缩限位待添加
|
||||
// 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();
|
||||
}
|
||||
// 云台软件限位
|
||||
|
||||
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)) // 右侧开关状态[中],底盘平移，伸缩
|
||||
{
|
||||
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_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);
|
||||
|
||||
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);
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
}
|
||||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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,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)
|
||||
|
|
|
|||
|
|
@ -126,6 +126,8 @@ typedef enum
|
|||
M2006,
|
||||
LK9025,
|
||||
HT04,
|
||||
DM4310,
|
||||
DM6006,
|
||||
} Motor_Type_e;
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -13,7 +13,7 @@ void MotorControlTask()
|
|||
DJIMotorControl();
|
||||
|
||||
/* 如果有对应的电机则取消注释,可以加入条件编译或者register对应的idx判断是否注册了电机 */
|
||||
LKMotorControl();
|
||||
//LKMotorControl();
|
||||
|
||||
// legacy support
|
||||
// 由于ht04电机的反馈方式为接收到一帧消息后立刻回传,以此方式连续发送可能导致总线拥塞
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -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
|
||||
|
|
@ -83,6 +83,8 @@ typedef enum
|
|||
ID_robot_hurt = 0x0206, // 伤害状态数据
|
||||
ID_shoot_data = 0x0207, // 实时射击数据
|
||||
ID_student_interactive = 0x0301, // 机器人间交互数据
|
||||
ID_custom_robot = 0x302, // 自定义控制器数据（图传链路）
|
||||
ID_remote_control = 0x304, // 键鼠遥控数据（图传链路）
|
||||
} CmdID_e;
|
||||
|
||||
/* 命令码数据段长,根据官方协议来定义长度，还有自定义数据长度 */
|
||||
|
|
@ -102,6 +104,9 @@ typedef enum
|
|||
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;
|
||||
|
||||
/****************************接收数据的详细说明****************************/
|
||||
|
|
@ -157,32 +162,37 @@ typedef struct
|
|||
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 机器人位置数据 */
|
||||
|
|
@ -222,6 +232,26 @@ typedef struct
|
|||
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个，也不会超*/
|
||||
|
|
|
|||
|
|
@ -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
|
||||
{
|
||||
|
|
|
|||
Loading…
Reference in New Issue