diff --git a/MDK-ARM/.vscode/keil-assistant.log b/MDK-ARM/.vscode/keil-assistant.log
index 7382f29..80658bb 100644
--- a/MDK-ARM/.vscode/keil-assistant.log
+++ b/MDK-ARM/.vscode/keil-assistant.log
@@ -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
+
diff --git a/MDK-ARM/R2.uvoptx b/MDK-ARM/R2.uvoptx
index eb773dd..612ed0e 100644
--- a/MDK-ARM/R2.uvoptx
+++ b/MDK-ARM/R2.uvoptx
@@ -170,6 +170,36 @@
1
error_code
+
+ 3
+ 1
+ UP,0x0A
+
+
+ 4
+ 1
+ chassis,0x0A
+
+
+ 5
+ 1
+ \\R2\../User/task/chassis_task.c\chassis.sick_cali.sickparam
+
+
+ 6
+ 1
+ cmd,0x0A
+
+
+ 7
+ 1
+ nucbuf,0x0A
+
+
+ 8
+ 1
+ cmd_fromnuc
+
0
diff --git a/MDK-ARM/R2/R2.axf b/MDK-ARM/R2/R2.axf
index cf40bd0..6593a15 100644
Binary files a/MDK-ARM/R2/R2.axf and b/MDK-ARM/R2/R2.axf differ
diff --git a/User/Module/Chassis.c b/User/Module/Chassis.c
index dc23a8f..0747bdd 100644
--- a/User/Module/Chassis.c
+++ b/User/Module/Chassis.c
@@ -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;
diff --git a/User/Module/Chassis.h b/User/Module/Chassis.h
index b2e31af..90930c5 100644
--- a/User/Module/Chassis.h
+++ b/User/Module/Chassis.h
@@ -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;
diff --git a/User/Module/config.c b/User/Module/config.c
index 0ddc597..91ea5a6 100644
--- a/User/Module/config.c
+++ b/User/Module/config.c
@@ -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,
},
diff --git a/User/Module/up.c b/User/Module/up.c
index 922e03a..f514f97 100644
--- a/User/Module/up.c
+++ b/User/Module/up.c
@@ -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 ;
}