底盘pid

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 */

MDK-ARM/R2.uvoptx

@@ -168,47 +168,37 @@
         <Ww>
           <count>3</count>
           <WinNumber>1</WinNumber>
-          <ItemText>UP,0x0A</ItemText>
+          <ItemText>posss</ItemText>
         </Ww>
         <Ww>
           <count>4</count>
           <WinNumber>1</WinNumber>
-          <ItemText>chassis,0x0A</ItemText>
+          <ItemText>UP,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>5</count>
           <WinNumber>1</WinNumber>
-          <ItemText>cmd,0x0A</ItemText>
+          <ItemText>chassis,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>6</count>
           <WinNumber>1</WinNumber>
-          <ItemText>nucbuf,0x0A</ItemText>
+          <ItemText>nuc_raw</ItemText>
         </Ww>
         <Ww>
           <count>7</count>
           <WinNumber>1</WinNumber>
-          <ItemText>cmd_fromnuc</ItemText>
+          <ItemText>cmd_fromnuc,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>8</count>
           <WinNumber>1</WinNumber>
-          <ItemText>NUC_send,0x0A</ItemText>
+          <ItemText>drop_message,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>9</count>
           <WinNumber>1</WinNumber>
-          <ItemText>\\R2\../User/task/up_task.c\UP.MID360Context.IsLaunch</ItemText>
+          <ItemText>\\R2\../User/task/chassis_task.c\chassis.param-&gt;M3508_param.d</ItemText>
-        </Ww>
-        <Ww>
-          <count>10</count>
-          <WinNumber>1</WinNumber>
-          <ItemText>ang_58,0x0A</ItemText>
-        </Ww>
-        <Ww>
-          <count>11</count>
-          <WinNumber>1</WinNumber>
-          <ItemText>ang_66,0x0A</ItemText>
         </Ww>
       </WatchWindow1>
       <Tracepoint>

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;

User/Module/config.c

@@ -111,11 +111,18 @@ static const ConfigParam_t param ={
 		
 
 	 	 .M3508_param = {
-		 .p = 15.1f,
+//		 .p = 15.1f,
-		 .i = 0.02f,
+//		 .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 ={ 

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,14 +360,26 @@ 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;
+	  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);
 
 
-	  PassCfg ->go_release_pos =
+			if (u->PassContext.Curve == CURVE_58) {
-		CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->cmd_MID360.dis),3,4,&u->PassContext.Curve);
+				
+        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);
 
     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;
@@ -385,6 +403,7 @@ 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;//发射 
+				
 					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;

User/Module/up.h

@@ -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);
 
 

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;	}
+    return 2.9851104*pow(d,4) -36.164382*pow(d,3) + 159.54844*pow(d,2)  -273.62856*d + 43.092452;
-// 计算58度曲线（偏下）
+	}
 // 计算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,9 +86,11 @@ 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) {
             *cs = CURVE_58;
@@ -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;
+}

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

User/task/chassis_task.c

@@ -34,7 +34,7 @@ static Chassis_Ctrl_t ctrl;
 
 #endif
 
-
+fp32 freq;
 
 /**
  * \brief 底盘任务
@@ -50,6 +50,7 @@ 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);

User/task/nuc_task.c

@@ -24,7 +24,6 @@ int a=0;
 
 
 
-
 void Task_nuc(void *argument){
  (void)argument; /**/
 

User/task/user_task.h

@@ -13,7 +13,7 @@
 
 /* 所有任务都要定义自己的任务运行频率 */
 
-// 分配的频率该如何给定?
+
 #define TASK_FREQ_CHASSIS (900u)
 #define TASK_FREQ_UP (900u)