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/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> <WinNumber>1</WinNumber>
<ItemText>error_code</ItemText> <ItemText>error_code</ItemText>
</Ww> </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> </WatchWindow1>
<Tracepoint> <Tracepoint>
<THDelay>0</THDelay> <THDelay>0</THDelay>

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

Binary file not shown.

85
User/Module/Chassis.c
View File

@ -3,7 +3,6 @@
#include "user_math.h" #include "user_math.h"
#include "bsp_buzzer.h" #include "bsp_buzzer.h"
#include "bsp_delay.h" #include "bsp_delay.h"
/* /*
机器人坐标系，向前x，右y，上yaw 机器人坐标系，向前x，右y，上yaw
不同于nuc (前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[i]=can->sickfed.raw_dis[i] / 10; //没使用原值
// } // }
c->sick_cali.sick_dis[0]=(fp32)(can->sickfed.raw_dis[0]) ; 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->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[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[6] = c->sick_cali.sick_dis[0];
// c->vofa_send[5] =c->sick_cali.sick_dis[1]; c->vofa_send[5] =c->sick_cali.sick_dis[1];
// c->vofa_send[4] =c->sick_cali.sick_dis[2]; c->vofa_send[4] =c->sick_cali.sick_dis[2];
return CHASSIS_OK; 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)); 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[0]), target_freq, 80.0f); // 角加速度滤波
LowPassFilter2p_Init(&(c->filled[1]), target_freq, 80.0f); // w滤波 LowPassFilter2p_Init(&(c->filled[1]), target_freq, 50.0f);
LowPassFilter2p_Init(&(c->filled[2]), target_freq, 80.0f); // y滤波 LowPassFilter2p_Init(&(c->filled[2]), target_freq, 80.0f);
LowPassFilter2p_Init(&(c->filled[3]), 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); KalmanCreate(&c->extKalman[0],10,1);
LowPassFilter2p_Init(&(c->filled[6]), target_freq, 80.0f); KalmanCreate(&c->extKalman[1],10,1);
LowPassFilter2p_Init(&(c->filled[7]), target_freq, 80.0f); KalmanCreate(&c->extKalman[2],10,1);
c->sick_cali .sickparam=c->param ->sickparam ; c->sick_cali .sickparam=c->param ->sickparam ;
@ -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->NUC_send.send[0] = 0;
c->sick_cali.is_reach = 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) { switch (c->chassis_ctrl.ctrl) {
case RCcontrol: case RCcontrol:
switch (c->chassis_ctrl.mode) { 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); sick_calibration(c, ctrl, out);
c->NUC_send.send[0] = (c->sick_cali.is_reach == 1) ? 1 : 0; c->NUC_send.send[0] = (c->sick_cali.is_reach == 1) ? 1 : 0;
break; break;
default:
c->move_vec.Vw = 0;
c->move_vec.Vx = 0;
c->move_vec.Vy = 0;
break;
} }
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) switch (c->chassis_ctrl.mode)
{ {
case PB_UP: case PB_UP:
case PB_MID:
case PB_DOWN:
c->move_vec.Vw = ctrl->Vw * 6000; c->move_vec.Vw = ctrl->Vw * 6000;
c->move_vec.Vx = ctrl->Vy * 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.Vx, 5000.0f);
abs_limit_fp(&c->move_vec.Vy, 5000.0f); abs_limit_fp(&c->move_vec.Vy, 5000.0f);
abs_limit_fp(&c->move_vec.Vw, 5000.0f); abs_limit_fp(&c->move_vec.Vw, 5000.0f);
break ; break ;
} }
break;
case RELAXED:
c->move_vec.Vw = 0;
c->move_vec.Vx = 0;
c->move_vec.Vy = 0;
break ;
@ -189,6 +202,7 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
break; break;
} }
// 电机速度限幅 // 电机速度限幅
abs_limit_fp(&c->move_vec.Vx, 6000.0f); abs_limit_fp(&c->move_vec.Vx, 6000.0f);
abs_limit_fp(&c->move_vec.Vy, 6000.0f); abs_limit_fp(&c->move_vec.Vy, 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; if (ctrl == NULL) return CHASSIS_ERR_NULL;
fp32 delta_w,delta_x,delta_y; fp32 delta_w,delta_x,delta_y;
fp32 sick0 =LowPassFilter2p_Apply( &(c->filled[5]),c->sick_cali.sick_dis[0]) ; fp32 sick0 = c->sick_cali.sick_dis[0];
fp32 sick1 =LowPassFilter2p_Apply( &(c->filled[6]), c->sick_cali.sick_dis[1]) ; fp32 sick1 = c->sick_cali.sick_dis[1];
fp32 sick2 = LowPassFilter2p_Apply( &(c->filled[7]),c->sick_cali.sick_dis[2] ); fp32 sick2 = c->sick_cali.sick_dis[2];
const sickparam_t *param = &c->sick_cali.sickparam; const sickparam_t *param = &c->sick_cali.sickparam;
diff_yaw = -(fp32)(sick1 - sick2);
diff_y = -(fp32)(sick1 - c->sick_cali.sickparam.y_set); diff_yaw =LowPassFilter2p_Apply(&(c->filled[1]), -(fp32)(sick1 +10 - sick2));
diff_x = (fp32)(sick0 - c->sick_cali.sickparam.x_set); 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_w=PID_calc(&c->pid.SickCaliWzOutPID, diff_yaw, 0);
delta_x=PID_calc(&c->pid.SickCaliVxOutPID, diff_x, 0); delta_x=PID_calc(&c->pid.SickCaliVxOutPID, diff_x, 0);
delta_y=PID_calc(&c->pid.SickCaliVyOutPID, diff_y,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);
}
c->move_vec.Vw=PID_calc(&c->pid.SickCaliWzInPID,-delta_w,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; static uint16_t reach_cnt = 0;

7
User/Module/Chassis.h
View File

@ -31,6 +31,7 @@
#include "can_use.h" #include "can_use.h"
#include "cmd.h" #include "cmd.h"
#include "filter.h" #include "filter.h"
#include "kalman.h"
#define CHASSIS_OK (0) #define CHASSIS_OK (0)
#define CHASSIS_ERR (-1) #define CHASSIS_ERR (-1)
@ -172,11 +173,13 @@ typedef struct{
}pid; }pid;
fp32 vofa_send[8]; fp32 vofa_send[8];
/*卡尔曼滤波*/
extKalman_t extKalman[3];
LowPassFilter2p_t filled[8]; /* 输出滤波器滤波器数组 */ LowPassFilter2p_t filled[8]; /* 输出滤波器滤波器数组 */
struct { struct {
int32_t sick_dis[4]; //获取到的sick激光值 fp32 sick_dis[4]; //获取到的sick激光值
sickparam_t sickparam; sickparam_t sickparam;
int is_reach; int is_reach;
}sick_cali; }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 = { .SickCali_WInparam = {
.p = 3.0f, .p = 3.0f,
.i = 0.005f, .i = 0.000f,
.d = 0.0f, .d = 0.0f,
.i_limit = 500.0f, .i_limit = 500.0f,
.out_limit = 2000.0f, .out_limit = 2000.0f,
}, },
.SickCali_WOutparam = { .SickCali_WOutparam = {
.p = 18.0f, .p = 2.6f,
.i = 0.0f, .i = 0.0f,
.d = 0.0f, .d = 0.0f,
.i_limit = 0.0f, .i_limit = 0.0f,
.out_limit = 1000.0f, .out_limit = 1000.0f,
}, },
.SickCali_YInparam = { .SickCali_YInparam = {
.p = 0.0f, .p = 1.0f,
.i = 0.0f, .i = 0.0f,
.d = 0.0f, .d = 0.0f,
.i_limit = 0.0f, .i_limit = 0.0f,
.out_limit = 0.0f, .out_limit = 5000.0f,
}, },
.SickCali_YOutparam = { .SickCali_YOutparam = {
.p = 2.9f, .p = 4.5f,
.i = 0.005f, .i = 0.0f,
.d = 0.0f, .d = 0.0f,
.i_limit = 500.0f, .i_limit = 00.0f,
.out_limit = 1000.0f, .out_limit = 1000.0f,
}, },
.SickCali_XInparam = { .SickCali_XInparam = {
.p = 0.0f, .p = 1.0f,
.i = 0.0f, .i = 0.0f,
.d = 0.0f, .d = 0.0f,
.i_limit = 0.0f, .i_limit = 0.0f,
.out_limit = 0.0f, .out_limit = 5000.0f,
}, },
.SickCali_XOutparam = { .SickCali_XOutparam = {
.p = 2.9f, .p = 4.5f,
.i = 0.0065f, .i = 0.00f,
.d = 0.0f, .d = 0.0f,
.i_limit = 500.0f, .i_limit = 500.0f,
.out_limit = 3000.0f, .out_limit = 2000.0f,
}, },
.Chassis_AngleAdjust_param={ .Chassis_AngleAdjust_param={
.p = 10.0f, .p = 10.0f,
.i = 0.0f, .i = 0.0f,
@ -184,10 +235,10 @@ static const ConfigParam_t param ={
}, },
.sickparam={ .sickparam={
.w_error=5, .w_error=2,
.xy_error=5, .xy_error=3,
.x_set=600, .x_set=10000,
.y_set=100, .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->go_shoot= pitch_cfg .
target->Pitch_angle = 58; target->Pitch_angle = 58;
target->go_shoot = -190.0f; target->go_shoot = -190.0f;
//失控下，最好切手动，手动用pitch
pitch_cfg ->go_release_pos=-190;
// target->Pitch_angle = 58; // 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 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_pull_speed=LaunchContext->LaunchCfg.go_down_speed;
u->motor_target.go_shoot = EndPos ; u->motor_target.go_shoot = EndPos ;
if(is_reached(u->motorfeedback.go_data.Pos,EndPos,1.0f)) // if(is_reached(u->motorfeedback.go_data.Pos,EndPos,1.0f))
LaunchContext->LaunchState = Launch_SHOOT_WAIT; // LaunchContext->LaunchState = Launch_SHOOT_WAIT;
} break; } break;
case Launch_SHOOT_WAIT: 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); 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);
} PassCfg ->go_release_pos =
CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos)+0.15,3,4,&u->PassContext.Curve);
switch (*state) { //遥控器按键进行状态切换 switch (*state) { //遥控器按键进行状态切换
case PASS_STOP: case PASS_STOP:
@ -373,7 +373,7 @@ int8_t Pass_Process(UP_t *u,CAN_Output_t *out,CMD_t *c)
break; break;
case PASS_START: case PASS_START:
if(LaunchContext->LaunchState == Launch_SHOOT_WAIT){ if(LaunchContext->LaunchState == Launch_TRIGGER){
target->go_pull_speed=PassCfg->go_down_speed; target->go_pull_speed=PassCfg->go_down_speed;
target->go_shoot = PassCfg->go_release_pos ; target->go_shoot = PassCfg->go_release_pos ;
} }