diff --git a/Core/Src/main.c b/Core/Src/main.c
index 2372718..4290161 100644
--- a/Core/Src/main.c
+++ b/Core/Src/main.c
@@ -81,6 +81,7 @@ int main(void)
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
+
HAL_Init();
/* USER CODE BEGIN Init */
diff --git a/MDK-ARM/R2.uvoptx b/MDK-ARM/R2.uvoptx
index c08fbe8..0a52c45 100644
--- a/MDK-ARM/R2.uvoptx
+++ b/MDK-ARM/R2.uvoptx
@@ -168,47 +168,37 @@
3
1
- UP,0x0A
+ posss
4
1
- chassis,0x0A
+ UP,0x0A
5
1
- cmd,0x0A
+ chassis,0x0A
6
1
- nucbuf,0x0A
+ nuc_raw
7
1
- cmd_fromnuc
+ cmd_fromnuc,0x0A
8
1
- NUC_send,0x0A
+ drop_message,0x0A
9
1
- \\R2\../User/task/up_task.c\UP.MID360Context.IsLaunch
-
-
- 10
- 1
- ang_58,0x0A
-
-
- 11
- 1
- ang_66,0x0A
+ \\R2\../User/task/chassis_task.c\chassis.param->M3508_param.d
diff --git a/MDK-ARM/R2/R2.axf b/MDK-ARM/R2/R2.axf
index c50144b..f2dc0ab 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 b0a0331..c220a4a 100644
--- a/User/Module/Chassis.c
+++ b/User/Module/Chassis.c
@@ -163,10 +163,6 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
switch (c->chassis_ctrl.mode)
{
case PB_UP:
- c->move_vec.Vw = ctrl->Vw * 6000;
- c->move_vec.Vx = ctrl->Vy * 6000;
- c->move_vec.Vy = ctrl->Vx * 6000;
- break ;
case PB_MID:
case PB_DOWN:
c->move_vec.Vw = -ctrl->cmd_MID360.angle * 1000;
diff --git a/User/Module/config.c b/User/Module/config.c
index 34b8bbe..b95e58c 100644
--- a/User/Module/config.c
+++ b/User/Module/config.c
@@ -111,11 +111,18 @@ static const ConfigParam_t param ={
.M3508_param = {
- .p = 15.1f,
- .i = 0.02f,
+// .p = 15.1f,
+// .i = 0.02f,
+// .d = 3.2f,
+// .i_limit = 200.0f,
+// .out_limit =6000.0f,
+ .p = 20.5f,
+ .i = 0.00011f,
.d = 3.2f,
.i_limit = 200.0f,
.out_limit =6000.0f,
+
+
},
/*视觉*/
.chassis_PICKWzPID_HIGN_param ={
diff --git a/User/Module/up.c b/User/Module/up.c
index cabda56..a15f93d 100644
--- a/User/Module/up.c
+++ b/User/Module/up.c
@@ -23,6 +23,7 @@
// 定义继电器控制
#define RELAY1_TOGGLE(state) HAL_GPIO_WritePin(GPIOE, RELAY_Pin, (state) ? GPIO_PIN_SET : GPIO_PIN_RESET)
+
int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
{
u->param = param; /*初始化参数 */
@@ -40,6 +41,9 @@ int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
PID_init (&u->pid .Pitch_M2006_speed ,PID_POSITION ,&(u->param->Pitch_M2006_speed_param ));
u->go_cmd =u->param ->go_cmd ;
+
+
+
LowPassFilter2p_Init(&(u->filled[0]),target_freq,100.0f);
@@ -126,6 +130,12 @@ int8_t VESC_M5065_Control(UP_t *u,fp32 speed)
int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
{
+ /*目标值限位*/
+ abs_limit_min_max_fp(&u->motor_target.go_shoot ,-215.0f,0.0f);
+ abs_limit_min_max_fp(&u->motor_target.Pitch_angle ,48.0f,67.0f);
+
+
+
//电机控制 ，传进can
DJ_Angle_Control(u,0x205,&u->motorfeedback .DJmotor_feedback[4] ,
&u->pid .Shoot_M2006angle ,
@@ -175,13 +185,9 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
LaunchCfg_t *LaunchCfg =&u->LaunchContext.LaunchCfg;
up_motor_target_t *target=&u->motor_target ;
- /*config值限位*/
- abs_limit_min_max_fp(&u->PitchContext.PitchConfig.go_release_pos ,-215.0f,0.0f);
- abs_limit_min_max_fp(&u->PitchContext.PitchConfig.Pitch_angle ,48.0f,67.0f);
/*部分数据更新*/
static int is_pitch=1;
- posss=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->cmd_MID360.dis),3.4,4.2,&u->MID360Context.Curve);
switch (c->CMD_CtrlType )
{
@@ -254,7 +260,7 @@ return 0;
//复用发射，
-int8_t Pitch_Launch_Sequence(UP_t *u, LaunchContext_t *LaunchContext,fp32 StartPos,fp32 EndPos,CAN_Output_t *out){
+int8_t Pitch_Launch_Sequence(UP_t *u,CMD_t *c, LaunchContext_t *LaunchContext,fp32 StartPos,fp32 EndPos,CAN_Output_t *out){
/*电机位置到达判断*/
bool is_GoStartReach=is_reached(u->motorfeedback.go_data.Pos,StartPos,1.0f); //go开始位置
@@ -291,8 +297,6 @@ int8_t Pitch_Launch_Sequence(UP_t *u, LaunchContext_t *LaunchContext,fp32 StartP
if( is_HookDone ){ //当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;
} break;
case Launch_SHOOT_WAIT:
@@ -315,10 +319,12 @@ int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c)
LaunchContext_t *LaunchContext = &u->LaunchContext;
// 可根据实际需要传入不同的起始和末位置,起始：当前位置
LaunchContext->LaunchCfg.go_up_speed= cfg->go_up_speed;
- LaunchContext->LaunchCfg.go_down_speed= cfg->go_down_speed
- ;
+ LaunchContext->LaunchCfg.go_down_speed= cfg->go_down_speed;
+ abs_limit_min_max_fp(&cfg->go_release_pos ,-215.0f,0.0f);
+ abs_limit_min_max_fp(&cfg->Pitch_angle,48.0f,67.0f);
- Pitch_Launch_Sequence(u, LaunchContext, u->motorfeedback.go_data.Pos, cfg->go_release_pos, out);
+
+ Pitch_Launch_Sequence(u, c,LaunchContext, u->motorfeedback.go_data.Pos, cfg->go_release_pos, out);
switch(*state){
@@ -354,15 +360,27 @@ int8_t Pass_Process(UP_t *u,CAN_Output_t *out,CMD_t *c)
PassCfg_t *PassCfg = &u->PassContext.PassCfg;
PassState_t *state = &u->PassContext.PassState;
- up_motor_target_t *target=&u->motor_target ;
+ PassContext_t *PassContext = &u->PassContext;
+ up_motor_target_t *target=&u->motor_target ;
LaunchContext_t *LaunchContext = &u->LaunchContext;
Pass_Sequence_Check(u,c);
-
+ /*俯仰角度，力度转换*/
+ PassCfg->go_release_pos=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->cmd_MID360.dis),3.3,4.3,&u->PassContext.Curve);
+
+
+ if (u->PassContext.Curve == CURVE_58) {
+
+ target->Pitch_angle = 58;
+ PassCfg->Pitch_angle=58;
+ }
+ else {
+ target->Pitch_angle = 66;
+ PassCfg->Pitch_angle=66;
+
+ }
+ abs_limit_min_max_fp(&PassCfg->go_release_pos ,-215.0f,0.0f);
- PassCfg ->go_release_pos =
- CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->cmd_MID360.dis),3,4,&u->PassContext.Curve);
-
switch (*state) { //遥控器按键进行状态切换
case PASS_STOP:
@@ -373,7 +391,7 @@ int8_t Pass_Process(UP_t *u,CAN_Output_t *out,CMD_t *c)
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);
+ Pitch_Launch_Sequence(u,c,LaunchContext,u->motorfeedback.go_data.Pos,PassCfg->go_wait,out);
break;
@@ -384,7 +402,8 @@ int8_t Pass_Process(UP_t *u,CAN_Output_t *out,CMD_t *c)
}
break ;
case PASS_POS_PREPARE:
- target->Shoot_M2006_angle=LaunchContext->LaunchCfg.m2006_init;//发射
+ target->Shoot_M2006_angle=LaunchContext->LaunchCfg.m2006_init;//发射
+
break;
case PASS_COMPLETE:
break;
@@ -400,13 +419,14 @@ int8_t UP_AUTO_Control(UP_t *u,CAN_Output_t *out,CMD_t *c){
LaunchContext_t *LaunchContext = &u->LaunchContext;
MID360Context_t *MID360Context=&u->MID360Context;
MID360Cfg_t *MID360Cfg = &u->MID360Context.MID360Cfg;
-// MID360Cfg->go_release_pos=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->cmd_MID360.dis),3.2,4.3,&u->MID360Context.Curve);
+
+ /*俯仰角度，力度转换，加数值限位*/
if(u->MID360Context.Curve==CURVE_58){
- MID360Cfg->go_release_pos=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->cmd_MID360.dis),3.3,4.3,&u->MID360Context.Curve);
+ MID360Cfg->go_release_pos=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->cmd_MID360.dis)+0.1f,3.3,4.3,&u->MID360Context.Curve);
}
else {
- MID360Cfg->go_release_pos=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->cmd_MID360.dis)-0.05f,3.3,4.3,&u->MID360Context.Curve);
+ MID360Cfg->go_release_pos=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->cmd_MID360.dis),3.3,4.3,&u->MID360Context.Curve);
}
if (u->MID360Context.Curve == CURVE_58) {
@@ -414,6 +434,9 @@ int8_t UP_AUTO_Control(UP_t *u,CAN_Output_t *out,CMD_t *c){
} else {
target->Pitch_angle = 66;
}
+ abs_limit_min_max_fp(&MID360Cfg->go_release_pos ,-215.0f,0.0f);
+
+
LaunchContext->LaunchCfg.go_up_speed=MID360Cfg->go_up_speed;
LaunchContext->LaunchCfg.go_down_speed=MID360Cfg->go_down_speed;
@@ -426,13 +449,14 @@ int8_t UP_AUTO_Control(UP_t *u,CAN_Output_t *out,CMD_t *c){
MID360Context->IsLaunch=1;
LaunchContext->LaunchState = Launch_PREPARE;
}
- Pitch_Launch_Sequence(u,LaunchContext,u->motorfeedback.go_data.Pos,MID360Cfg->go_release_pos,out);
+ Pitch_Launch_Sequence(u,c,LaunchContext,u->motorfeedback.go_data.Pos,MID360Cfg->go_release_pos,out);
break ;
case AUTO_MID360:
target->Shoot_M2006_angle= LaunchContext->LaunchCfg.m2006_init;
MID360Context->IsLaunch=0;
+// u->cmd_from_nuc.lock=0;
break ;
default:
break;
diff --git a/User/Module/up.h b/User/Module/up.h
index 898692c..47f795b 100644
--- a/User/Module/up.h
+++ b/User/Module/up.h
@@ -190,8 +190,8 @@ typedef struct{
typedef struct{
- uint8_t up_task_run;
- const UP_Param_t *param;
+ uint8_t up_task_run;
+ const UP_Param_t *param;
LaunchContext_t LaunchContext;
/*投篮过程*/
PitchContext_t PitchContext;
@@ -215,6 +215,7 @@ typedef struct{
uint32_t ecd;
float angle;
}Encoder;
+
}motorfeedback;
@@ -234,7 +235,11 @@ typedef struct{
}final_out;
-
+// struct {
+// float angle;
+// float dis;
+// uint8_t lock;
+// }cmd_from_nuc;
LowPassFilter2p_t filled[6]; /* 输出滤波器滤波器数组 */
fp32 vofa_send[8];
@@ -254,7 +259,7 @@ int8_t DJ_Speed_Control(UP_t *u,int id,DJmotor_feedback_t *f,pid_type_def *Speed
int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c);
int8_t Pass_Sequence_Check(UP_t *u,CMD_t *c);
int8_t Pass_Process(UP_t *u,CAN_Output_t *out,CMD_t *c);
-int8_t Pitch_Launch_Sequence(UP_t *u, LaunchContext_t *LaunchContext,fp32 StartPos,fp32 EndPos,CAN_Output_t *out);
+int8_t Pitch_Launch_Sequence(UP_t *u,CMD_t *c, LaunchContext_t *LaunchContext,fp32 StartPos,fp32 EndPos,CAN_Output_t *out);
int8_t UP_AUTO_Control(UP_t *u,CAN_Output_t *out,CMD_t *c);
diff --git a/User/Module/up_utils.c b/User/Module/up_utils.c
index e579749..25a6db4 100644
--- a/User/Module/up_utils.c
+++ b/User/Module/up_utils.c
@@ -68,12 +68,14 @@ int8_t GO_SendData( GO_MotorData_t *go_data,GO_MotorCmd_t *go_cmd,float pos, flo
// 计算66度曲线（偏上）
static float curve_66(float d) {
- return 2.9851104*pow(d,4) -36.164382*pow(d,3) + 159.54844*pow(d,2) -273.62856*d + 43.092452; }
-// 计算58度曲线（偏下）
+ return 2.9851104*pow(d,4) -36.164382*pow(d,3) + 159.54844*pow(d,2) -273.62856*d + 43.092452;
+ }
// 计算58度曲线（偏下）
static float curve_58(float d) {
// return 0.9242f * d * d + 19.4246f * d - 154.9055f;
- return 2.638517*pow(d,4) -47.996138*pow(d,3) + 325.38515*pow(d,2) -950.18155*d + 919.86543;
+// return 2.638517*pow(d,4) -47.996138*pow(d,3) + 325.38515*pow(d,2) -950.18155*d + 919.86543;
+ return 2.7775f * d * d - 0.5798f * d - 113.1488;
+
}
/*
@@ -84,8 +86,10 @@ static float curve_58(float d) {
fp32 ang_58;
fp32 ang_66;
+
float CurveChange(float d, float x, float y, CurveType *cs)
{
+
if (*cs == CURVE_66) {
if (d > y) {
@@ -166,3 +170,63 @@ int8_t Pass_Sequence_Check(UP_t *u, CMD_t *c) //按键顺序检测，传球，
}
+
+#define BIN_SCALE 1000 // 精度：0.001米
+#define BIN_OFFSET 3000 // 最小支持3.000米 → bin 3000
+#define BIN_COUNT 5001 // 支持3.000 ~ 8.000米，(8000-3000+1)
+#define WINDOW_SIZE 50
+
+static float window[WINDOW_SIZE] = {0};
+static int index = 0;
+static int filled = 0;
+static uint16_t freq_map[BIN_COUNT] = {0}; // 用16位计数防止溢出
+
+static float last_output = 0.0f;
+static uint8_t first_run = 1;
+
+// 四舍五入保留3位小数
+static inline float round3f(float x) {
+ return (int)(x * BIN_SCALE + 0.5f) / (float)BIN_SCALE;
+}
+
+float stable_distance_filter(float new_value) {
+ // 1. 四舍五入到0.001
+ float rounded = round3f(new_value);
+ int new_bin = (int)(rounded * BIN_SCALE + 0.5f);
+
+ // 2. 移除旧值计数
+ int old_bin = (int)(round3f(window[index]) * BIN_SCALE + 0.5f);
+ if (filled && old_bin >= BIN_OFFSET && old_bin < BIN_OFFSET + BIN_COUNT) {
+ freq_map[old_bin - BIN_OFFSET]--;
+ }
+
+ // 3. 加入新值到窗口和计数
+ window[index++] = new_value;
+ if (index >= WINDOW_SIZE) {
+ index = 0;
+ filled = 1;
+ }
+ if (new_bin >= BIN_OFFSET && new_bin < BIN_OFFSET + BIN_COUNT) {
+ freq_map[new_bin - BIN_OFFSET]++;
+ }
+
+ // 4. 找出现频率最高的桶
+ int max_bin = -1;
+ uint16_t max_freq = 0;
+ for (int i = 0; i < BIN_COUNT; ++i) {
+ if (freq_map[i] > max_freq) {
+ max_freq = freq_map[i];
+ max_bin = i + BIN_OFFSET;
+ }
+ }
+
+ float current_fixed = max_bin >= 0 ? max_bin / (float)BIN_SCALE : rounded;
+
+ // 5. 判断是否更新输出，差值超过0.1才更新
+ if (first_run || fabsf(current_fixed - last_output) > 0.1f) {
+ last_output = current_fixed;
+ first_run = 0;
+ }
+
+ return last_output;
+}
diff --git a/User/Module/up_utils.h b/User/Module/up_utils.h
index 436e403..76e73f2 100644
--- a/User/Module/up_utils.h
+++ b/User/Module/up_utils.h
@@ -53,6 +53,9 @@ int8_t GO_SendData( GO_MotorData_t *go_data,GO_MotorCmd_t *go_cmd,float pos, flo
int8_t DJ_processdata(DJmotor_feedback_t *f,fp32 ecd_to_angle);
+static inline float round3f(float x) ;
+
+float stable_distance_filter(float new_value) ;
#endif
diff --git a/User/task/chassis_task.c b/User/task/chassis_task.c
index 8997ce1..1fe1803 100644
--- a/User/task/chassis_task.c
+++ b/User/task/chassis_task.c
@@ -34,7 +34,7 @@ static Chassis_Ctrl_t ctrl;
#endif
-
+fp32 freq;
/**
* \brief 底盘任务
@@ -50,7 +50,8 @@ void Task_Chassis(void *argument)
Chassis_init(&chassis,&(task_runtime.config.config->chassis),TASK_FREQ_CHASSIS);
uint32_t tick = osKernelGetTickCount();
-
+ uint32_t last_tick=tick;
+
while(1)
{
@@ -59,6 +60,8 @@ void Task_Chassis(void *argument)
task_runtime.freq.chassis=TASK_FREQ_CHASSIS;
task_runtime.last_up_time.chassis=tick;
#endif
+ freq=(tick-last_tick)/1000.0f;
+ last_tick =tick;
/*imu数据获取*/
osMessageQueueGet(task_runtime.msgq.imu.eulr, &chassis.pos088.imu_eulr, NULL, 0);
diff --git a/User/task/nuc_task.c b/User/task/nuc_task.c
index 5be2f44..29d2c01 100644
--- a/User/task/nuc_task.c
+++ b/User/task/nuc_task.c
@@ -24,7 +24,6 @@ int a=0;
-
void Task_nuc(void *argument){
(void)argument; /**/
diff --git a/User/task/user_task.h b/User/task/user_task.h
index 52e20d5..f0a5a57 100644
--- a/User/task/user_task.h
+++ b/User/task/user_task.h
@@ -13,7 +13,7 @@
/* 所有任务都要定义自己的任务运行频率 */
-// 分配的频率该如何给定?
+
#define TASK_FREQ_CHASSIS (900u)
#define TASK_FREQ_UP (900u)