LYL/R2_NEW
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

sick改pid，远距离抖

Browse Source
This commit is contained in:
ZHAISHUI04 2025-07-02 20:42:48 +08:00
parent 67960d2805
commit 40ca2de038
7 changed files with 171 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
MDK-ARM/.vscode/keil-assistant.log vendored
View File

@ -481,3 +481,7 @@
[info] Log at : 2025/6/28|01:22:24|GMT+0800
[info] Log at : 2025/6/30|18:03:06|GMT+0800
[info] Log at : 2025/7/1|14:01:22|GMT+0800

30
MDK-ARM/R2.uvoptx
View File

@ -170,6 +170,36 @@
<WinNumber>1</WinNumber>
<ItemText>error_code</ItemText>
</Ww>
<Ww>
<count>3</count>
<WinNumber>1</WinNumber>
<ItemText>UP,0x0A</ItemText>
</Ww>
<Ww>
<count>4</count>
<WinNumber>1</WinNumber>
<ItemText>chassis,0x0A</ItemText>
</Ww>
<Ww>
<count>5</count>
<WinNumber>1</WinNumber>
<ItemText>\\R2\../User/task/chassis_task.c\chassis.sick_cali.sickparam</ItemText>
</Ww>
<Ww>
<count>6</count>
<WinNumber>1</WinNumber>
<ItemText>cmd,0x0A</ItemText>
</Ww>
<Ww>
<count>7</count>
<WinNumber>1</WinNumber>
<ItemText>nucbuf,0x0A</ItemText>
</Ww>
<Ww>
<count>8</count>
<WinNumber>1</WinNumber>
<ItemText>cmd_fromnuc</ItemText>
</Ww>
</WatchWindow1>
<Tracepoint>
<THDelay>0</THDelay>

BIN
MDK-ARM/R2/R2.axf
View File

Binary file not shown.

91
User/Module/Chassis.c
View File

@ -3,7 +3,6 @@
#include "user_math.h"
#include "bsp_buzzer.h"
#include "bsp_delay.h"
/*
机器人坐标系，向前x，右y，上yaw
不同于nuc (前x，左y，上yaw)
@ -33,13 +32,13 @@ int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can) {
// c->sick_cali.sick_dis[i]=can->sickfed.raw_dis[i] / 10; //没使用原值
// }
c->sick_cali.sick_dis[0]=(fp32)(can->sickfed.raw_dis[0]) ;
c->sick_cali.sick_dis[1]=(fp32)(can->sickfed.raw_dis[1]+26) ; //有点误差，手动补偿
c->sick_cali.sick_dis[1]=(fp32)(can->sickfed.raw_dis[1]+30) ; //有点误差，手动补偿
c->sick_cali.sick_dis[2]=(fp32)(can->sickfed.raw_dis[2] );
// c->vofa_send[7] =c->sick_cali.sick_dis[0]-c->sick_cali.sick_dis[1];
// c->vofa_send[6] = c->sick_cali.sick_dis[0];
// c->vofa_send[5] =c->sick_cali.sick_dis[1];
// c->vofa_send[4] =c->sick_cali.sick_dis[2];
c->vofa_send[7] =c->sick_cali.sick_dis[1]-c->sick_cali.sick_dis[2];
c->vofa_send[6] = c->sick_cali.sick_dis[0];
c->vofa_send[5] =c->sick_cali.sick_dis[1];
c->vofa_send[4] =c->sick_cali.sick_dis[2];
return CHASSIS_OK;
}
@ -63,13 +62,14 @@ int8_t Chassis_init(Chassis_t *c, const Chassis_Param_t *param, float target_fre
PID_init(&(c->pid.Chassis_AngleAdjust), PID_POSITION, &(c->param->Chassis_AngleAdjust_param));
LowPassFilter2p_Init(&(c->filled[0]), target_freq, 80.0f); // 角加速度滤波
LowPassFilter2p_Init(&(c->filled[1]), target_freq, 80.0f); // w滤波
LowPassFilter2p_Init(&(c->filled[2]), target_freq, 80.0f); // y滤波
LowPassFilter2p_Init(&(c->filled[1]), target_freq, 50.0f);
LowPassFilter2p_Init(&(c->filled[2]), target_freq, 80.0f);
LowPassFilter2p_Init(&(c->filled[3]), target_freq, 80.0f);
LowPassFilter2p_Init(&(c->filled[4]), target_freq, 80.0f);
LowPassFilter2p_Init(&(c->filled[5]), target_freq, 80.0f);
LowPassFilter2p_Init(&(c->filled[6]), target_freq, 80.0f);
LowPassFilter2p_Init(&(c->filled[7]), target_freq, 80.0f);
KalmanCreate(&c->extKalman[0],10,1);
KalmanCreate(&c->extKalman[1],10,1);
KalmanCreate(&c->extKalman[2],10,1);
c->sick_cali .sickparam=c->param ->sickparam ;
@ -80,7 +80,7 @@ fp32 pian_yaw;
void Chassis_speed_calculate(Chassis_t *c, fp32 Vx, fp32 Vy, fp32 Vw) {
fp64 Nor_Vx, Nor_Vy;
normalize_vector(Vx, Vy, &Nor_Vx, &Nor_Vy);
c->hopemotorout.OmniSpeedOut[0] = -Nor_Vx + Nor_Vy + Vw+pian_yaw; // 右前
c->hopemotorout.OmniSpeedOut[1] = -Nor_Vx - Nor_Vy + Vw+pian_yaw; // 右后
c->hopemotorout.OmniSpeedOut[2] = Nor_Vx - Nor_Vy + Vw +pian_yaw; // 左后
@ -102,6 +102,13 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
c->NUC_send.send[0] = 0;
c->sick_cali.is_reach = 0;
c->vofa_send[0]= KalmanFilter(&c->extKalman[0] , c->sick_cali.sick_dis[0]);
c->vofa_send[1]= KalmanFilter(&c->extKalman[1] , c->sick_cali.sick_dis[1]);
c->vofa_send[2]= KalmanFilter(&c->extKalman[2] , c->sick_cali.sick_dis[2]);
switch (c->chassis_ctrl.ctrl) {
case RCcontrol:
switch (c->chassis_ctrl.mode) {
@ -124,11 +131,7 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
sick_calibration(c, ctrl, out);
c->NUC_send.send[0] = (c->sick_cali.is_reach == 1) ? 1 : 0;
break;
default:
c->move_vec.Vw = 0;
c->move_vec.Vx = 0;
c->move_vec.Vy = 0;
break;
}
break;
@ -166,17 +169,27 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
switch (c->chassis_ctrl.mode)
{
case PB_UP:
case PB_MID:
case PB_DOWN:
c->move_vec.Vw = ctrl->Vw * 6000;
c->move_vec.Vx = ctrl->Vy * 6000;
c->move_vec.Vy = ctrl->Vx * 6000;
c->move_vec.Vy = ctrl->Vx * 6000;
break ;
case PB_MID:
case PB_DOWN:
c->move_vec.Vw = ctrl->cmd_MID360.posw * 1000;
c->move_vec.Vy = -ctrl->cmd_MID360.posy * 1000;
c->move_vec.Vx = -ctrl->cmd_MID360.posx * 1000;
abs_limit_fp(&c->move_vec.Vx, 5000.0f);
abs_limit_fp(&c->move_vec.Vy, 5000.0f);
abs_limit_fp(&c->move_vec.Vw, 5000.0f);
break ;
}
}
break;
case RELAXED:
c->move_vec.Vw = 0;
c->move_vec.Vx = 0;
c->move_vec.Vy = 0;
break ;
@ -188,6 +201,7 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
break;
}
// 电机速度限幅
abs_limit_fp(&c->move_vec.Vx, 6000.0f);
@ -227,26 +241,35 @@ int8_t sick_calibration(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out)
if (ctrl == NULL) return CHASSIS_ERR_NULL;
fp32 delta_w,delta_x,delta_y;
fp32 sick0 =LowPassFilter2p_Apply( &(c->filled[5]),c->sick_cali.sick_dis[0]) ;
fp32 sick1 =LowPassFilter2p_Apply( &(c->filled[6]), c->sick_cali.sick_dis[1]) ;
fp32 sick2 = LowPassFilter2p_Apply( &(c->filled[7]),c->sick_cali.sick_dis[2] );
fp32 sick0 = c->sick_cali.sick_dis[0];
fp32 sick1 = c->sick_cali.sick_dis[1];
fp32 sick2 = c->sick_cali.sick_dis[2];
const sickparam_t *param = &c->sick_cali.sickparam;
diff_yaw = -(fp32)(sick1 - sick2);
diff_y = -(fp32)(sick1 - c->sick_cali.sickparam.y_set);
diff_x = (fp32)(sick0 - c->sick_cali.sickparam.x_set);
diff_yaw =LowPassFilter2p_Apply(&(c->filled[1]), -(fp32)(sick1 +10 - sick2));
diff_y = LowPassFilter2p_Apply(&(c->filled[2]),-(fp32)(sick1 - c->sick_cali.sickparam.y_set));
diff_x = LowPassFilter2p_Apply(&(c->filled[3]),(fp32)(sick0 - c->sick_cali.sickparam.x_set));
delta_w=PID_calc(&c->pid.SickCaliWzOutPID, diff_yaw, 0);
delta_x=PID_calc(&c->pid.SickCaliVxOutPID, diff_x, 0);
delta_y=PID_calc(&c->pid.SickCaliVyOutPID, diff_y,0);
if(fabs(diff_yaw)>5){
if(fabs(diff_yaw)>param->w_error){
c->move_vec.Vw=PID_calc(&c->pid.SickCaliWzInPID,-delta_w,0);
// c->move_vec.Vw=delta_w;
// c->move_vec.Vx=PID_calc(&c->pid.SickCaliVxInPID,delta_x,0);
// c->move_vec.Vx=PID_calc(&c->pid.SickCaliVyInPID,delta_y,0);
}
// if(fabs(diff_y)>param->xy_error){
//
// c->move_vec.Vy=PID_calc(&c->pid.SickCaliVyInPID,delta_y,0);
// }
// if(fabs(diff_x)>param->xy_error){
//
// c->move_vec.Vx=PID_calc(&c->pid.SickCaliVxInPID,-delta_x,0);
// }
static uint16_t reach_cnt = 0;

7
User/Module/Chassis.h
View File

@ -31,6 +31,7 @@
#include "can_use.h"
#include "cmd.h"
#include "filter.h"
#include "kalman.h"
#define CHASSIS_OK (0)
#define CHASSIS_ERR (-1)
@ -172,11 +173,13 @@ typedef struct{
}pid;
fp32 vofa_send[8];
/*卡尔曼滤波*/
extKalman_t extKalman[3];
LowPassFilter2p_t filled[8]; /* 输出滤波器滤波器数组 */
struct {
int32_t sick_dis[4]; //获取到的sick激光值
fp32 sick_dis[4]; //获取到的sick激光值
sickparam_t sickparam;
int is_reach;
}sick_cali;

83
User/Module/config.c
View File

@ -133,48 +133,99 @@ static const ConfigParam_t param ={
},
// .SickCali_WInparam = {
// .p = 3.0f,
// .i = 0.000f,
// .d = 0.0f,
// .i_limit = 500.0f,
// .out_limit = 2000.0f,
// },
// .SickCali_WOutparam = {
// .p = 2.6f,
// .i = 0.0f,
// .d = 0.0f,
// .i_limit = 0.0f,
// .out_limit = 1000.0f,
// },
// .SickCali_YInparam = {
// .p = 1.0f,
// .i = 0.0f,
// .d = 0.0f,
// .i_limit = 0.0f,
// .out_limit = 5000.0f,
// },
// .SickCali_YOutparam = {
// .p = 4.5f,
// .i = 0.0f,
// .d = 0.0f,
// .i_limit = 00.0f,
// .out_limit = 1000.0f,
// },
// .SickCali_XInparam = {
// .p = 1.0f,
// .i = 0.0f,
// .d = 0.0f,
// .i_limit = 0.0f,
// .out_limit = 5000.0f,
// },
// .SickCali_XOutparam = {
// .p = 4.5f,
// .i = 0.00f,
// .d = 0.0f,
// .i_limit = 500.0f,
// .out_limit = 2000.0f,
// },
.SickCali_WInparam = {
.p = 3.0f,
.i = 0.005f,
.i = 0.000f,
.d = 0.0f,
.i_limit = 500.0f,
.out_limit = 2000.0f,
},
.SickCali_WOutparam = {
.p = 18.0f,
.p = 2.6f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 0.0f,
.out_limit = 1000.0f,
},
.SickCali_YInparam = {
.p = 0.0f,
.p = 1.0f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 0.0f,
.out_limit = 0.0f,
.out_limit = 5000.0f,
},
.SickCali_YOutparam = {
.p = 2.9f,
.i = 0.005f,
.p = 4.5f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 500.0f,
.i_limit = 00.0f,
.out_limit = 1000.0f,
},
.SickCali_XInparam = {
.p = 0.0f,
.p = 1.0f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 0.0f,
.out_limit = 0.0f,
.out_limit = 5000.0f,
},
.SickCali_XOutparam = {
.p = 2.9f,
.i = 0.0065f,
.p = 4.5f,
.i = 0.00f,
.d = 0.0f,
.i_limit = 500.0f,
.out_limit = 3000.0f,
.out_limit = 2000.0f,
},
.Chassis_AngleAdjust_param={
.p = 10.0f,
.i = 0.0f,
@ -184,10 +235,10 @@ static const ConfigParam_t param ={
},
.sickparam={
.w_error=5,
.xy_error=5,
.x_set=600,
.y_set=100,
.w_error=2,
.xy_error=3,
.x_set=10000,
.y_set=2000,
},

16
User/Module/up.c
View File

@ -246,6 +246,8 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
// target->go_shoot= pitch_cfg .
target->Pitch_angle = 58;
target->go_shoot = -190.0f;
//失控下，最好切手动，手动用pitch
pitch_cfg ->go_release_pos=-190;
// target->Pitch_angle = 58;
@ -287,8 +289,8 @@ int8_t Pitch_Launch_Sequence(UP_t *u, LaunchContext_t *LaunchContext,fp32 StartP
if( u->motorfeedback .DJmotor_feedback [4].total_angle>-1){ //当2006的总角度小于1，可以认为已经勾上,误差为1
u->motor_target.go_pull_speed=LaunchContext->LaunchCfg.go_down_speed;
u->motor_target.go_shoot = EndPos ;
if(is_reached(u->motorfeedback.go_data.Pos,EndPos,1.0f))
LaunchContext->LaunchState = Launch_SHOOT_WAIT;
// if(is_reached(u->motorfeedback.go_data.Pos,EndPos,1.0f))
// LaunchContext->LaunchState = Launch_SHOOT_WAIT;
} break;
case Launch_SHOOT_WAIT:
@ -352,12 +354,10 @@ int8_t Pass_Process(UP_t *u,CAN_Output_t *out,CMD_t *c)
Pass_Sequence_Check(u,c);
if(c->pos) //
{
PassCfg ->go_release_pos =
CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos),3,4,&u->PassContext.Curve);
CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos)+0.15,3,4,&u->PassContext.Curve);
}
switch (*state) { //遥控器按键进行状态切换
case PASS_STOP:
@ -367,13 +367,13 @@ int8_t Pass_Process(UP_t *u,CAN_Output_t *out,CMD_t *c)
break;
case PASS_PREPARE:
target->go_pull_speed=PassCfg->go_up_speed;
Pitch_Launch_Sequence(u,LaunchContext,u->motorfeedback.go_data.Pos,PassCfg->go_wait,out);
break;
case PASS_START:
if(LaunchContext->LaunchState == Launch_SHOOT_WAIT){
if(LaunchContext->LaunchState == Launch_TRIGGER){
target->go_pull_speed=PassCfg->go_down_speed;
target->go_shoot = PassCfg->go_release_pos ;
}