xiaocheng/Er_sentry
1
1
Fork 0
Code Issues 1 Pull Requests Actions Packages Projects Releases Wiki Activity

加裁判系统前保留内容，导航待测试版本

Browse Source
This commit is contained in:
Xiaocheng 2026-03-13 01:51:37 +08:00
parent d290e6c3a2
commit ba7ef4e260
9 changed files with 85 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
User/device/remote_control.c
View File

@ -10,7 +10,7 @@
#include "bsp/calc_lib.h"
#define R12DS
//#define DR16
// #define DR16
/*
两个遥控器都是
x
@ -85,25 +85,25 @@ static bool DR16_DataCorrupted(const DR16_t *dr16) {
if ((dr16->data.ch_r_x < REMOTE_CH_VALUE_MIN) ||
(dr16->data.ch_r_x > REMOTE_CH_VALUE_MAX))
return true;
return false;
if ((dr16->data.ch_r_y < REMOTE_CH_VALUE_MIN) ||
(dr16->data.ch_r_y > REMOTE_CH_VALUE_MAX))
return true;
return false;
if ((dr16->data.ch_l_x < REMOTE_CH_VALUE_MIN) ||
(dr16->data.ch_l_x > REMOTE_CH_VALUE_MAX))
return true;
return false;
if ((dr16->data.ch_l_y < REMOTE_CH_VALUE_MIN) ||
(dr16->data.ch_l_y > REMOTE_CH_VALUE_MAX))
return true;
if (dr16->data.sw_l == 0) return true;
if (dr16->data.sw_r == 0) return true;
return false;
if (dr16->data.sw_l == 0) return false;
if (dr16->data.sw_r == 0) return false;
return true;
}
int8_t DR16_ParseRaw(DR16_t *dr16){

8
User/module/chassis.c
View File

@ -83,7 +83,7 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_mode_t mode ,uint32_t now)
PID_Reset(&c->pid.chassis_6020anglePid[i]);
PID_Reset(&c->pid.chassis_3508DAOHANG_pid[i]);
}
PID_Reset(&c->pid.daohang_chassis_follow_gimbal_pid);
c->mode = mode;
return CHASSIS_OK;
}
@ -148,6 +148,7 @@ int8_t chassis_init(Chassis_t *c, Chassis_Param_t *param, float target_freq)
PID_Init(&c->pid.chassis_3508DAOHANG_pid[i], KPID_MODE_CALC_D, target_freq, &c->param->M3508DAOHANG_param);
}
PID_Init(&c->pid.chassis_follow_gimbal_pid, KPID_MODE_CALC_D, target_freq, &c->param->chassis_follow_gimbalPID);
PID_Init(&c->pid.daohang_chassis_follow_gimbal_pid, KPID_MODE_CALC_D, target_freq, &c->param->daohang_chassis_follow_gimbalPID);
LowPassFilter2p_Init(&c->filled[0], target_freq, 20.0f); // vx
LowPassFilter2p_Init(&c->filled[1], target_freq, 20.0f); // vy
@ -411,7 +412,7 @@ case CHASSIS_MODE_DAOHANG:
// 导航模式
c->move_vec.Vx = -c_cmd->Vx/1;
c->move_vec.Vy = c_cmd->Vy/1;
c->move_vec.Vw = PID_Calc(&c->pid.chassis_follow_gimbal_pid, 1.44215584f ,c->motorfeedback.gimbal_yaw_encoder.rotor_abs_angle, 0.0f, c->dt);;
c->move_vec.Vw =PID_Calc(&c->pid.daohang_chassis_follow_gimbal_pid, 1.44215584f ,c->motorfeedback.gimbal_yaw_encoder.rotor_abs_angle, 0.0f, c->dt);;
break;
default:
@ -450,6 +451,7 @@ case CHASSIS_MODE_DAOHANG:
c->motorfeedback.rotor_rpm3508[i], 0.0f, c->dt);
c->out.rotor3508_out[i] = LowPassFilter2p_Apply(&c->filled[3+i], c->final_out.final_3508out[i]);
}
break;
case CHASSIS_MODE_DAOHANG:
for(int i=0;i<4;i++)
@ -469,6 +471,8 @@ for(int i=0;i<4;i++)
c->motorfeedback.rotor_truespeed3508[i], 0.0f, c->dt);
c->out.rotor3508_out[i] = LowPassFilter2p_Apply(&c->filled[3+i], c->final_out.final_3508out[i]);
}
break;
}
return CHASSIS_OK;
}

4
User/module/chassis.h
View File

@ -59,7 +59,7 @@ extern "C"
KPID_Params_t M3508DAOHANG_param;
KPID_Params_t chassis_follow_gimbalPID;
KPID_Params_t daohang_chassis_follow_gimbalPID;
MOTOR_RM_Param_t motor_3508_param[4]; // 四个3508电机
MOTOR_RM_Param_t motor_6020_param[4]; // 四个6020电机
@ -132,7 +132,7 @@ extern "C"
KPID_t chassis_3508VPID[4];
KPID_t chassis_3508DAOHANG_pid[4];
KPID_t daohang_chassis_follow_gimbal_pid;
KPID_t chassis_follow_gimbal_pid;
} pid;

2
User/module/cmd.c
View File

@ -155,7 +155,7 @@ int8_t CMD_CtrlSet(Chassis_CMD_t *c_cmd, const CMD_RC_t *rc, Gimbal_CMD_t *g_cmd
// c_cmd->mode = CHASSIS_MODE_FOLLOW_GIMBAL;
break;
case CMD_SW_DOWN:
g_cmd->ctrl_mode = GIMBAL_MODE_AI;
g_cmd->ctrl_mode = GIMBAL_MODE_DAOHANG;
s_cmd->control_mode = SHOOT_AI;
c_cmd->mode = CHASSIS_MODE_DAOHANG;
break;

2
User/module/cmd.h
View File

@ -106,12 +106,14 @@ typedef struct
GIMBAL_MODE_RELAX, /* 放松模式，电机不输出。一般情况云台初始化之后的模式 */
GIMBAL_MODE_ABSOLUTE, /* 绝对坐标系控制，控制在空间内的绝对姿态 */
GIMBAL_MODE_RELATIVE, /* 相对坐标系控制，控制相对于底盘的姿态 */
GIMBAL_MODE_DAOHANG, /* 导航模式，云台根据导航数据进行移动 */
} Gimbal_Mode_t;
typedef enum
{
GIMBAL_MODE_REMOTE,
GIMBAL_MODE_AI,
}GIMBAL_Ctrl_mode_t;
typedef struct {
Gimbal_Mode_t mode;

47
User/module/config.c
View File

@ -57,7 +57,7 @@ static const Config_Param_t config = {
// .d_cutoff_freq= -1.0f,
// .range=-1.0f
.k = 1.0f,
.p = 4.0f,
.p = 3.0f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 1.0f,
@ -68,7 +68,7 @@ static const Config_Param_t config = {
.M3508DAOHANG_param={
.k = 1.0f,
.p = 2.0f,
.p = 1.0f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 1.0f,
@ -76,14 +76,24 @@ static const Config_Param_t config = {
.d_cutoff_freq = -1.0f,
.range = -1.0f
},
.chassis_follow_gimbalPID = {
.k = 0.7f,
.p = 0.5f,
.i = 0.1f,
.daohang_chassis_follow_gimbalPID={
.k = 1.0f,
.p = 2.5f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 1.0f,
.out_limit = 0.5f,
.out_limit = 1.0f,
.d_cutoff_freq = -1.0f,
.range = M_2PI
},
.chassis_follow_gimbalPID = {
.k = 0.5f,
.p = 0.5f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 1.0f,
.out_limit = 1.0f,
.d_cutoff_freq = -1.0f,
.range = M_2PI
},
@ -235,6 +245,27 @@ static const Config_Param_t config = {
.range = -1.0f
},
.pid.daohang_6020_motor_angle={
.k = 1.0f,
.p = 2.0f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 1.0f,
.out_limit = 1.0f,
.d_cutoff_freq = 20.0f,
.range = M_2PI
},
.pid.daohang_4310_motor_angle={
.k = 2.0f,
.p = 5.0f,
.i = 0.0f,
.d = 1.0f,
.i_limit = 0.0f,
.out_limit = 2.0f,
.d_cutoff_freq = 20.0f,
.range = M_2PI
},
.mech_zero = {
.yaw_6020 = 1.31f,/*1.31*/
.yaw_4310 = 2.06f, /*大yaw零点*/

15
User/module/gimbal.c
View File

@ -74,6 +74,8 @@ static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) {
PID_Reset(&g->pid.pitch_4310_omega);
PID_Reset(&g->pid.yaw_6020_angle);
PID_Reset(&g->pid.yaw_6020_omega);
PID_Reset(&g->pid.daohang_6020_angle);
PID_Reset(&g->pid.daohang_4310_angle);
LowPassFilter2p_Reset(&g->filter_out.yaw_4310,0.0f);
LowPassFilter2p_Reset(&g->filter_out.pitch_4310,0.0f);
LowPassFilter2p_Reset(&g->filter_out.yaw_6020,0.0f);
@ -119,6 +121,9 @@ int8_t Gimbal_Init(Gimbal_t *g, Gimbal_Param_t *param,float target_freq)
/*4310 pitch pid初始化 单环*/
PID_Init(&g->pid.pitch_4310_angle,KPID_MODE_NO_D, target_freq,&g->param->pid.pitch_4310_motor_angle );
PID_Init(&g->pid.pitch_4310_omega,KPID_MODE_CALC_D, target_freq,&g->param->pid.pitch_4310_motor_omega );
/*导航PID*/
PID_Init(&g->pid.daohang_6020_angle,KPID_MODE_CALC_D, target_freq,&g->param->pid.daohang_6020_motor_angle );
PID_Init(&g->pid.daohang_4310_angle,KPID_MODE_CALC_D, target_freq,&g->param->pid.daohang_4310_motor_angle );
/*输出滤波器初始化*/
LowPassFilter2p_Init(&g->filter_out.yaw_6020, target_freq, g->param->low_pass_cutoff_freq.out);
LowPassFilter2p_Init(&g->filter_out.gyro_yaw, target_freq, g->param->low_pass_cutoff_freq.gyro);
@ -364,6 +369,16 @@ g->setpoint.pit_accl = g_cmd->pit_accl;
g->out.yaw_6020 = 0.0f;
break;
case GIMBAL_MODE_DAOHANG:
/*导航模式，NUC直接给出目标角度，云台控制环跟随即可*/
g->setpoint.eulr.yaw = g->param->mech_zero.yaw_6020+g->param->travel.yaw_6020*0.5f; // 这里可以直接设置为NUC传来的目标角度
g->setpoint.yaw_4310 = 1.44215584f;
g->out.yaw_6020 = PID_Calc(&g->pid.daohang_6020_angle,g->setpoint.eulr.yaw,
g->feedback.motor.yaw_6020_motor_feedback.rotor_abs_angle,0.0f,g->dt);
g->out.yaw_4310 = PID_Calc(&g->pid.daohang_4310_angle,g->setpoint.yaw_4310,
g->feedback.motor.yaw_4310_motor_feedback.rotor_abs_angle,g->feedback.motor.yaw_4310_motor_feedback.rotor_speed,g->dt);
/* 6020输出滤波 4310个人觉得不是很需要滤波，可以自己加*/
// g->out.yaw_6020 = LowPassFilter2p_Apply(&g->filter_out.yaw_6020, g->out.yaw_6020);
g->out.yaw_6020 = LowPassFilter2p_Apply(&g->filter_out.yaw_6020, g->out.yaw_6020);

6
User/module/gimbal.h
View File

@ -42,6 +42,9 @@ extern "C"
KPID_Params_t yaw_4310_motor_angle; /* yaw轴4310角位置环PID参数 */
KPID_Params_t pitch_4310_motor_omega; /* pitch轴角速度环PID参数 */
KPID_Params_t pitch_4310_motor_angle; /* pitch轴角位置环PID参数 */
KPID_Params_t daohang_6020_motor_angle; /* 导航模式小yaw角位置环PID参数 */
KPID_Params_t daohang_4310_motor_angle; /* 导航模式大yaw角位置环PID参数 */
} pid;
/* 低通滤波器截止频率 */
@ -134,6 +137,9 @@ extern "C"
KPID_t yaw_4310_omega; /* yaw轴4310角速度环PID */
KPID_t pitch_4310_angle; /* pitch轴4310角位置环PID */
KPID_t pitch_4310_omega; /* pitch轴4310角速度环PID */
KPID_t daohang_6020_angle; /* 导航模式小yaw角位置环PID */
KPID_t daohang_4310_angle; /* 导航模式大yaw角位置环PID */
} pid;
struct

5
User/task/gimbal.c
View File

@ -67,6 +67,7 @@ osMessageQueueGet(task_runtime.msgq.gimbal.cmd, &cmd_gimbal, NULL, 0);
if(ai_gimbal_result_cmd.mode==0){
cmd_gimbal.set_pitch =gimbal_imu.eulr.pit;
cmd_gimbal.set_yaw=gimbal_imu.eulr.yaw;
}
if(ai_gimbal_result_cmd.mode==1)
{
@ -76,6 +77,7 @@ if(ai_gimbal_result_cmd.mode==0){
cmd_gimbal.yaw_accl=ai_gimbal_result_cmd.gimbal_t.accl.yaw;
cmd_gimbal.pit_vel=ai_gimbal_result_cmd.gimbal_t.vel.pit;
cmd_gimbal.pit_accl=ai_gimbal_result_cmd.gimbal_t.accl.pit;
}
if(ai_gimbal_result_cmd.mode==2){
cmd_gimbal.set_pitch=ai_gimbal_result_cmd.gimbal_t.setpoint.pit;
@ -84,7 +86,10 @@ if(ai_gimbal_result_cmd.mode==0){
cmd_gimbal.yaw_accl=ai_gimbal_result_cmd.gimbal_t.accl.yaw;
cmd_gimbal.pit_vel=ai_gimbal_result_cmd.gimbal_t.vel.pit;
cmd_gimbal.pit_accl=ai_gimbal_result_cmd.gimbal_t.accl.pit;
}
Gimbal_UpdateFeedback(&gimbal);
osMessageQueueReset(task_runtime.msgq.gimbal.yaw6020);