sick改pid，远距离抖

MDK-ARM/.vscode/keil-assistant.log

@@ -475,3 +475,13 @@
 
 [info] Log at : 2025/6/24|03:28:50|GMT+0800
 
+[info] Log at : 2025/6/26|19:47:59|GMT+0800
+
+[info] Log at : 2025/6/27|16:40:58|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
+

MDK-ARM/.vscode/settings.json

@@ -97,6 +97,7 @@
     "gimbal_task.h": "c",
     "gimgal.h": "c",
     "navi.h": "c",
-    "up.h": "c"
+    "up.h": "c",
+    "bsp_usart.h": "c"
   }
 }

MDK-ARM/R2.uvoptx

@@ -158,42 +158,47 @@
         <Ww>
           <count>0</count>
           <WinNumber>1</WinNumber>
-          <ItemText>NUC_send,0x0A</ItemText>
+          <ItemText>tar_angle</ItemText>
         </Ww>
         <Ww>
           <count>1</count>
           <WinNumber>1</WinNumber>
-          <ItemText>SendBuffer,0x10</ItemText>
+          <ItemText>huart-&gt;ErrorCode</ItemText>
         </Ww>
         <Ww>
           <count>2</count>
           <WinNumber>1</WinNumber>
-          <ItemText>chassis,0x0A</ItemText>
+          <ItemText>error_code</ItemText>
         </Ww>
         <Ww>
           <count>3</count>
           <WinNumber>1</WinNumber>
-          <ItemText>cmd_fromnuc,0x0A</ItemText>
+          <ItemText>UP,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>4</count>
           <WinNumber>1</WinNumber>
-          <ItemText>nucbuf,0x10</ItemText>
+          <ItemText>chassis,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>5</count>
           <WinNumber>1</WinNumber>
-          <ItemText>UP,0x0A</ItemText>
+          <ItemText>\\R2\../User/task/chassis_task.c\chassis.sick_cali.sickparam</ItemText>
         </Ww>
         <Ww>
           <count>6</count>
           <WinNumber>1</WinNumber>
-          <ItemText>a,0x0A</ItemText>
+          <ItemText>cmd,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>7</count>
           <WinNumber>1</WinNumber>
-          <ItemText>posss</ItemText>
+          <ItemText>nucbuf,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>8</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>cmd_fromnuc</ItemText>
         </Ww>
       </WatchWindow1>
       <Tracepoint>

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)
@@ -32,9 +31,14 @@ 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]=can->sickfed.raw_dis[0] ;
+		c->sick_cali.sick_dis[0]=(fp32)(can->sickfed.raw_dis[0]) ;
-		c->sick_cali.sick_dis[1]=can->sickfed.raw_dis[1]+50 ;  //有点误差，手动补偿
+		c->sick_cali.sick_dis[1]=(fp32)(can->sickfed.raw_dis[1]+30) ;  //有点误差，手动补偿
-		c->sick_cali.sick_dis[2]=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[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;
 }
 
@@ -47,15 +51,25 @@ int8_t Chassis_init(Chassis_t *c, const Chassis_Param_t *param, float target_fre
     }
     PID_init(&(c->pid.chassis_PICKWzPID_HIGN), PID_POSITION, &(c->param->chassis_PICKWzPID_HIGN_param));
     PID_init(&(c->pid.chassis_PICKWzPID_LOW), PID_POSITION, &(c->param->chassis_PICKWzPID_LOW_param));
-    PID_init(&(c->pid.SickCaliWzPID), PID_POSITION, &(c->param->Sick_CaliWparam));
+    /*sick*/
-    PID_init(&(c->pid.SickCaliVxPID), PID_POSITION, &(c->param->Sick_CaliXparam));
+    PID_init(&(c->pid.SickCaliWzInPID), PID_POSITION, &(c->param->SickCali_WInparam));
-    PID_init(&(c->pid.SickCaliVyPID), PID_POSITION, &(c->param->Sick_CaliYparam));
+    PID_init(&(c->pid.SickCaliWzOutPID), PID_POSITION, &(c->param->SickCali_WOutparam));
+    PID_init(&(c->pid.SickCaliVxInPID), PID_POSITION, &(c->param->SickCali_XInparam));
+    PID_init(&(c->pid.SickCaliVxOutPID), PID_POSITION, &(c->param->SickCali_XOutparam));
+    PID_init(&(c->pid.SickCaliVyInPID), PID_POSITION, &(c->param->SickCali_YInparam));
+    PID_init(&(c->pid.SickCaliVyOutPID), PID_POSITION, &(c->param->SickCali_YOutparam));
+    /*修正 */
     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[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); // x滤波
+    LowPassFilter2p_Init(&(c->filled[3]), 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 ;
 
@@ -66,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;  // 左后
@@ -84,12 +98,17 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
     c->pos088.bmi088.filtered_gyro.z = LowPassFilter2p_Apply(&(c->filled[0]), c->pos088.bmi088.gyro.z);
 
     /*初始数据*/
-    c->move_vec.Vw = 0;
+
-    c->move_vec.Vx = 0;
-    c->move_vec.Vy = 0;
     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) {
@@ -112,9 +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:
+
-                    
-                    break;
             }
             break;
 
@@ -130,7 +147,12 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
 								   // c->NUC_send.send[0] = 1;
                     break;
                 case AUTO_MID360_Pitch:
-                    
+                    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;
                 case AUTO_MID360_Adjust:
                     c->move_vec.Vw = ctrl->Vw * 6000;
@@ -138,24 +160,48 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
                     c->move_vec.Vy = ctrl->Vx * 6000;
                     
                     break;
-								case PassBall:   //
+
-                    c->move_vec.Vw = ctrl->cmd_MID360.posw * 1000;
+                default:
+                    
+                    break;
+            }
+								case PASS_BALL:
+								   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.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 ;
-                default:
+											 
-                    
+									 }	
-                    break;
+									 break;
-            }
+										case RELAXED:
+                   c->move_vec.Vw = 0;
+                   c->move_vec.Vx = 0;
+                   c->move_vec.Vy = 0;											
+                    break		;									
+								
+								
+								
+								
+									break;
             break;
 
         default:
-            
+
             break;
     }
+		
 
     // 电机速度限幅
     abs_limit_fp(&c->move_vec.Vx, 6000.0f);
@@ -171,7 +217,7 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
         out->motor_CHASSIS3508.as_array[i] = c->final_out.final_3508out[i];
     }
 
-    c->vofa_send[0] = (fp32)c->sick_cali.sick_dis[1] - c->sick_cali.sick_dis[2];
+
 		
     return CHASSIS_OK;
 		
@@ -188,33 +234,50 @@ sick2,左1
 
 
 */
-
+fp32 diff_yaw,diff_y,diff_x;
 int8_t sick_calibration(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) 
 {
     if (c == NULL) return CHASSIS_ERR_NULL;
     if (ctrl == NULL) return CHASSIS_ERR_NULL;
-
+    fp32 delta_w,delta_x,delta_y; 
-    
+     
-    fp32 sick0 = c->sick_cali.sick_dis[0] ;
+    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 =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));
 
-    fp32 diff_yaw = -(fp32)(sick1 - sick2);
+    delta_w=PID_calc(&c->pid.SickCaliWzOutPID, diff_yaw, 0);
-    fp32 diff_y  = -(fp32)(sick1 - c->sick_cali.sickparam.y_set);
+    delta_x=PID_calc(&c->pid.SickCaliVxOutPID, diff_x, 0);
-    fp32 diff_x  =  (fp32)(sick0 - c->sick_cali.sickparam.x_set);
+    delta_y=PID_calc(&c->pid.SickCaliVyOutPID, diff_y,0);
-
+	
-
+	if(fabs(diff_yaw)>param->w_error){
-    c->move_vec.Vw = (fabsf(diff_yaw) > param->w_error) ? PID_calc(&(c->pid.SickCaliWzPID), diff_yaw, 0) : 0;
+		
-    c->move_vec.Vx = (fabsf(diff_x)   > param->xy_error) ? PID_calc(&(c->pid.SickCaliVxPID), diff_x, 0) : 0;
+	  c->move_vec.Vw=PID_calc(&c->pid.SickCaliWzInPID,-delta_w,0);
-    c->move_vec.Vy = (fabsf(diff_y)   > param->xy_error) ? PID_calc(&(c->pid.SickCaliVyPID), diff_y, 0) : 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;
     if (fabsf(diff_yaw) <= param->w_error &&
         fabsf(diff_x)   <= param->xy_error &&
         fabsf(diff_y)   <= param->xy_error) {
         reach_cnt++;
-        if (reach_cnt >= 50) {
+        if (reach_cnt >= 20) {
             
             c->sick_cali.is_reach = 1;
             
@@ -235,7 +298,7 @@ int8_t Chassis_AngleCompensate(Chassis_t *c)
     {
 		   pian_angel=cur_angle-AngleChange(DEGREE,c->pos088.imu_eulr.yaw);		
 		}
-
+    
 		else	{
 			 cur_angle=AngleChange(DEGREE,c->pos088.imu_eulr.yaw);			
        pian_angel=0;

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)     
@@ -80,9 +81,13 @@ typedef struct
   pid_param_t NaviVy_param;
   pid_param_t NaviVw_param;	
 	
-	pid_param_t Sick_CaliWparam;
+	pid_param_t SickCali_WInparam;
-	pid_param_t Sick_CaliYparam;
+	pid_param_t SickCali_WOutparam;
-	pid_param_t Sick_CaliXparam;	
+	pid_param_t SickCali_YInparam;
+	pid_param_t SickCali_YOutparam;
+	pid_param_t SickCali_XInparam;
+	pid_param_t SickCali_XOutparam;
+	
 	pid_param_t Chassis_AngleAdjust_param;
 	
 	sickparam_t sickparam;
@@ -154,22 +159,27 @@ typedef struct{
 	pid_type_def chassis_PICKWzPID_HIGN;
 		/*存在较低误差*/
 	pid_type_def chassis_PICKWzPID_LOW;
-		
 	pid_type_def 	Chassis_AngleAdjust;
 	
-	  pid_type_def SickCaliWzPID;
+	/*sick */
-		pid_type_def SickCaliVxPID;
+	  pid_type_def SickCaliWzInPID;
-		pid_type_def SickCaliVyPID;
+	  pid_type_def SickCaliWzOutPID;
+	  pid_type_def SickCaliVxInPID;
+	  pid_type_def SickCaliVxOutPID;
+	  pid_type_def SickCaliVyInPID;
+	  pid_type_def SickCaliVyOutPID;
 		
 		
 	}pid;
 
    fp32 vofa_send[8];
+	/*卡尔曼滤波*/
+	 extKalman_t extKalman[3];
+	 LowPassFilter2p_t filled[8]; /* 输出滤波器滤波器数组 */
 	
-	 LowPassFilter2p_t filled[6]; /* 输出滤波器滤波器数组 */
 	 
 	struct {
-	int32_t sick_dis[4];   //获取到的sick激光值
+	fp32 sick_dis[4];   //获取到的sick激光值
   sickparam_t sickparam;
 	int is_reach;
 	}sick_cali;

User/Module/config.c

@@ -37,18 +37,18 @@ static const ConfigParam_t param ={
     .out_limit = 3000.0f,
 },
 .Pitch_M2006_angle_param = {  
-    .p = 600.0f,        
+    .p = 1600.0f,        
     .i = 0.0f,         
     .d = 3.0f,         
-    .i_limit = 2000.0f,
+    .i_limit = 4000.0f,
-    .out_limit = 3000.0f,
+    .out_limit = 20000.0f,
 },
 .Pitch_M2006_speed_param={
-    .p = 5.0f,         
+    .p = 8.5f,         
-    .i = 0.3f,         
+    .i = 0.03f,         
     .d = 0.0f,         
     .i_limit = 2000.0f,
-    .out_limit = 3000.0f,
+    .out_limit = 15000.0f,
 },
 .Pitch_Angle_M2006_speed_param={ //俯仰跑速度环
     .p = 20.0f,         
@@ -71,20 +71,40 @@ static const ConfigParam_t param ={
 },
 
 
-/*上层其他参数*/			
+/*上层其他参数,一些只是初始值，在运行过程中会被更改*/			
 
     /*投球*/
-    .PitchCfg = {
+    .LaunchCfg = {
-    .m2006_init =-150,    //释放的角度
+    .m2006_init = -150.0f,    // M2006初始角度
-	  .m2006_trig =0,       //可以拉住发射的角度
+    .m2006_trig = 0.0f,       // M2006触发角度
-		.go_init = -50,       //仅用在go上电，初始位置
+    .go_pull_pos = -214.0f,   // go上升去合并扳机，扳机位置
-	  .go_pull_pos =-214,
+    .go_up_speed = 12.0f,     // 上升速度
-		.Pitch_angle  =58,
+    .go_down_speed = 6.0f,     // 下降速度
-			
+		.Pitch_angle = 66,	           //俯仰角
-		.pull_speed  =5,
+		.go_init	 = -50
-
 },
+    .PitchCfg = {
 
+		.go_init = -50,       //仅用在go上电，初始位置
+	  .go_release_pos=-200,
+		.Pitch_angle  =66,
+
+}, 
+		.PassCfg={
+		.go_wait  =-10,   
+	  .go_release_pos	=-150,		
+	  .Pitch_angle=66, 
+    .go_up_speed	=12,
+    .go_down_speed =5,			
+		},
+    .MID360Cfg={
+			
+		.go_release_pos=-200, // GO电机发射位置
+		.go_up_speed=20, // GO电机上升速度
+		.go_down_speed=10, // GO电机下降速度
+
+   
+		},			
 	},
 	 .chassis = {/**/
 		
@@ -111,28 +131,102 @@ static const ConfigParam_t param ={
 			.i_limit = 50.0f,
 			.out_limit =1000.0f,	 
 		 },
-			.Sick_CaliWparam ={ 
+			
-			.p = 4.5f,     
+		 
-			.i = 0.005f,   
+//			.SickCali_WInparam = {
-			.d = 0.0f,    
+//				.p = 3.0f,
-			.i_limit = 500.0f,
+//				.i = 0.000f,
-			.out_limit =1000.0f,	 
+//				.d = 0.0f,
-		 },
+//				.i_limit = 500.0f,
-			.Sick_CaliYparam ={ 
+//				.out_limit = 2000.0f,
-			.p = 2.9f,     
+//			},
-			.i = 0.0051f,   
+//			.SickCali_WOutparam = {
-			.d = 0.0f,    
+//				.p = 2.6f,
-			.i_limit = 500.0f,
+//				.i = 0.0f,
-			.out_limit =3000.0f,	 
+//				.d = 0.0f,
-		 },		
+//				.i_limit = 0.0f,
-			.Sick_CaliXparam ={ 
+//				.out_limit = 1000.0f,
-			.p = 2.9f,     
+//			},
-			.i = 0.0065f,   
+//			.SickCali_YInparam = {
-			.d = 0.0f,    
+//				.p = 1.0f,
-			.i_limit = 500.0f,
+//				.i = 0.0f,
-			.out_limit =3000.0f,	 
+//				.d = 0.0f,
-		 },	
+//				.i_limit = 0.0f,
-			.Chassis_AngleAdjust_param={
+//				.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.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,
+			},
+
+					.Chassis_AngleAdjust_param={
 			.p = 10.0f,     
 			.i = 0.0f,   
 			.d = 0.0f,    
@@ -141,10 +235,10 @@ static const ConfigParam_t param ={
 				
 			},
 			.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,
 				
 			},
 

User/Module/up.c

@@ -43,13 +43,19 @@ int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
 	LowPassFilter2p_Init(&(u->filled[0]),target_freq,100.0f);  
 
 
-	if (!u->Pitch_Cfg  .is_init) { 
+	if (!u->PitchContext  .is_init) { 
-		  u->Pitch_Cfg .PitchConfig  = u->param ->PitchCfg  ;//赋值
+		  u->PitchContext .PitchConfig  = u->param ->PitchCfg  ;//赋值
-      u->Pitch_Cfg .PitchState   = PITCH_PREPARE;  //状态更新,开始夹球
+      u->PitchContext .PitchState   = PITCH_PREPARE;  //状态更新
-      u->Pitch_Cfg .is_init = 1;
+      u->PitchContext .is_init = 1;
     }
-
+   if(!u->PassContext.is_init)  {
-     BSP_UART_RegisterCallback(BSP_UART_RS485, BSP_UART_RX_CPLT_CB, USART6_RxCompleteCallback);//注册串口回调函数，bsp层
+		  u->PassContext .PassCfg  = u->param ->PassCfg  ;//赋值
+      u->PassContext .PassState   = PASS_STOP;  //状态更新
+      u->PassContext .is_init = 1;		  
+	 }
+	 u->LaunchContext.LaunchCfg = u->param->LaunchCfg;
+	 u->MID360Context.MID360Cfg = u->param->MID360Cfg;
+	 BSP_UART_RegisterCallback(BSP_UART_RS485, BSP_UART_RX_CPLT_CB, USART6_RxCompleteCallback);//注册串口回调函数，bsp层
 
 }
 
@@ -118,8 +124,6 @@ int8_t VESC_M5065_Control(UP_t *u,fp32 speed)
 
 
 
-
-
 int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
 {
 	//电机控制	，传进can
@@ -131,16 +135,17 @@ int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
 	/*俯仰2006，双环，内使用oid角度环，外电机速度环
 	  
 	*/
+	
 	DJ_Speed_Control(u,0x206,&u->motorfeedback .DJmotor_feedback[5],&u->pid .Pitch_M2006_speed,
 	(PID_calc (&(u->pid .Pitch_M2006_angle ),
-	u->motorfeedback .Encoder.angle,u->motor_target .Pitch_angle))
+	u->motorfeedback .Encoder.angle,u->motor_target .Pitch_angle)+0.14)
 	);
-  
+
 	GO_SendData(
 	&u->motorfeedback.go_data,
 	&u->go_cmd,u->motor_target .go_shoot,
-	u->Pitch_Cfg .PitchConfig .pull_speed
+	u->motor_target.go_pull_speed
-	);//对应的拉动速度
+	);
 
 	for(int i=0;i<4;i++){
 	out ->motor_UP3508_Dribble.as_array[i]=u->final_out.DJfinal_out [i] ;
@@ -166,167 +171,260 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
 		if(c ==NULL)  return 0;
 	
 	/*指针简化*/
-	PitchCfg_t *pitch_cfg = &u->Pitch_Cfg.PitchConfig;		
+	PitchCfg_t *pitch_cfg = &u->PitchContext.PitchConfig;	
+	LaunchCfg_t *LaunchCfg =&u->LaunchContext.LaunchCfg;
 	up_motor_target_t *target=&u->motor_target ;
 		
 	/*config值限位*/
-		abs_limit_min_max_fp(&u->Pitch_Cfg.PitchConfig.go_release_pos ,-215.0f,0.0f);
+		abs_limit_min_max_fp(&u->PitchContext.PitchConfig.go_release_pos ,-215.0f,0.0f);
-		abs_limit_min_max_fp(&u->Pitch_Cfg.PitchConfig.Pitch_angle ,48.0f,67.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->pos),3,4,&u->Pitch_Cfg.Curve);
+	posss=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos),3.4,4.2,&u->MID360Context.Curve);
-//	if (u->Pitch_Cfg.Curve == CURVE_58) {
+//	if (u->PitchContext.Curve == CURVE_58) {
 //    target->Pitch_angle = 58;
 //    } else {
 //    target->Pitch_angle = 66;
 //    }
-		
+		u->vofa_send [2]  = c->pos;
+		u->vofa_send [3]  =  LowPassFilter2p_Apply(&u->filled[0],c->pos);
+		u->vofa_send [4]   =1;
+  
     switch (c->CMD_CtrlType  )
 	{
 	 case RCcontrol:  //在手动模式下
-		 	 
+
+ 	 
    switch (c-> CMD_mode ) 
 			{
 				
-		case Normal :
+		 case Normal : //复位，go位置和俯仰角保持LaunchCfg最后修改后的位置，扳机2006角度复位用于发射
          /*投篮*/
 		if(is_pitch){
-		target->Shoot_M2006_angle = pitch_cfg->go_init ;
+		target->go_shoot = LaunchCfg->go_init ;
-		target->Pitch_angle  =		pitch_cfg->Pitch_angle;		
+		target->Pitch_angle  =  LaunchCfg->Pitch_angle;		
 		is_pitch=0;	
-			} //让初始go位置只读一次，后面按调整模式更改的来,后面稳定了之后，可以跟随调整模式下一块删
+		} //初始上电
-		
+		//LaunchCfg->go_up_speed=15;
-		target->Shoot_M2006_angle =u->Pitch_Cfg .PitchConfig .m2006_init ;
+		target->Pitch_angle =LaunchCfg->Pitch_angle; 
-		u->Pitch_Cfg .PitchState   = PITCH_PREPARE;  //状态更新,开始夹球
+		target->go_pull_speed=LaunchCfg->go_up_speed;	
-
+		target->Shoot_M2006_angle=LaunchCfg->m2006_init;
+		u->PitchContext .PitchState   = PITCH_PREPARE;  //状态更新
+    u->LaunchContext.LaunchState  = Launch_Stop;
 				break;
 			
 		case Pitch :
-				if (u->Pitch_Cfg .PitchState  ==PITCH_PREPARE) 
+				if (u->PitchContext .PitchState  ==PITCH_PREPARE) 
 				{
-		      u->Pitch_Cfg .PitchState=PITCH_START;//置标志位用于启动投篮
+		      u->PitchContext .PitchState=PITCH_START;//置标志位用于启动投篮
 				}
 
-			 Pitch_Process(u,out);
+			 Pitch_Process(u,out,c);
 
 
 				 break ;
-		case UP_Adjust: //测试用
+		case UP_Adjust: //测试用,手动使用pitch下的cfg
-			  pitch_cfg ->pull_speed=5;
+			  LaunchCfg->go_up_speed=5;
 			  pitch_cfg ->go_release_pos += c->Vx ;
-	      pitch_cfg->Pitch_angle += c->Vy/100;
+	      LaunchCfg->Pitch_angle += c->Vy/100;
 	
-		    target->go_shoot=u->Pitch_Cfg.PitchConfig.go_release_pos;
+		    target->go_shoot=pitch_cfg ->go_release_pos;
-		    target->Pitch_angle=u->Pitch_Cfg.PitchConfig.Pitch_angle;
+		    target->Pitch_angle=LaunchCfg->Pitch_angle;
 			break ;
-				
+				default:
+			break;
 	}
 			break;
 	case AUTO:
-		/*自动下数据更新，根据距离切换俯仰角,如需自动调整，放在*/	
+        UP_AUTO_Control(u,out,c);		
-	    if (u->Pitch_Cfg.Curve == CURVE_58) {
+		break ;	
-        target->Pitch_angle = 58;
+      case PASS_BALL:
-    } else {
+				Pass_Process(u,out,c);
-        target->Pitch_angle = 66;
+				break;
+			case RELAXED:
+//			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;
+			
+			
+			break ;
+	
+}
+
+return 0;
+}
+
+
+
+
+//复用发射，
+int8_t Pitch_Launch_Sequence(UP_t *u, LaunchContext_t *LaunchContext,fp32 StartPos,fp32 EndPos,CAN_Output_t *out){
+	
+	switch(LaunchContext->LaunchState){
+		case Launch_Stop: break;
+		case Launch_PREPARE:
+			u->motor_target.go_shoot = StartPos;
+		 if(is_reached(u->motorfeedback.go_data.Pos,StartPos,1.0f)&& 
+			 is_reached(u->motorfeedback.go_data.W,0,1.0f)){
+				 //根据位置和速度判断是否到达初始位置
+			 LaunchContext->LaunchState = Launch_START;
+				
+			 }break;
+			 
+		case Launch_START:
+			u->motor_target.go_pull_speed=LaunchContext->LaunchCfg.go_up_speed;
+		  u->motor_target.go_shoot = u->LaunchContext.LaunchCfg.go_pull_pos;
+			if(u->motorfeedback .go_data .Pos < -209){ //检测go位置到达最上面，这里的检测条件可以更改
+			u->motor_target.Shoot_M2006_angle = u->LaunchContext.LaunchCfg.m2006_trig ;//设置2006角度，关闭
+		    
+			LaunchContext->LaunchState = Launch_TRIGGER;
+			}break;
+		
+		case Launch_TRIGGER:
+			
+		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;
+		}	break;
+		
+		case Launch_SHOOT_WAIT:
+			if(u->motorfeedback.DJmotor_feedback[4].total_angle<-1) //认为发射
+				LaunchContext->LaunchState = Launch_DONE;
+			break;
+		case Launch_DONE:
+			break ;
+	}
+	
+}
+
+
+
+int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c)
+{
+	/*简化*/
+	PitchCfg_t *cfg = &u->PitchContext.PitchConfig;
+	PitchState_t *state =&u->PitchContext .PitchState;
+  up_motor_target_t *target=&u->motor_target ;
+  LaunchContext_t *LaunchContext = &u->LaunchContext;
+	// 可根据实际需要传入不同的起始和末位置,起始：当前位置
+  Pitch_Launch_Sequence(u, LaunchContext, u->motorfeedback.go_data.Pos, cfg->go_release_pos, out);
+
+    
+	switch(*state){
+		
+		case PITCH_START:
+			   LaunchContext->LaunchState = Launch_PREPARE;
+		
+		    *state=PITCH_WAIT;
+		break;
+		
+		case PITCH_WAIT:
+			 
+    if(LaunchContext->LaunchState==Launch_DONE)
+         *state=PITCH_COMPLETE;
+		break;
+		case PITCH_COMPLETE:
+			
+			break;
+		default:
+			break;
+		
+	}
+  
+	
+    return 0;
+	
+}
+
+
+
+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 ;
+    LaunchContext_t *LaunchContext = &u->LaunchContext;
+	
+	  Pass_Sequence_Check(u,c);
+	  
+
+	  PassCfg ->go_release_pos =
+		CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos)+0.15,3,4,&u->PassContext.Curve);
+		
+    switch (*state) { //遥控器按键进行状态切换
+			  case PASS_STOP:
+					
+				break;
+				case PASS_IDLE:				 
+				LaunchContext->LaunchState = Launch_PREPARE;
+				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_TRIGGER){
+ 				 target->go_pull_speed=PassCfg->go_down_speed;
+				 target->go_shoot = PassCfg->go_release_pos ;
+					}
+        break ;
+				case PASS_POS_PREPARE:
+         target->Shoot_M2006_angle=LaunchContext->LaunchCfg.m2006_init;//发射   					
+					break;
+        case PASS_COMPLETE:
+            break;
+        default:
+            break;
     }
+    return 0;
+}
+
+int8_t UP_AUTO_Control(UP_t *u,CAN_Output_t *out,CMD_t *c){
+		/*自动下数据更新，根据距离切换俯仰角,如需自动调整，放在*/
+		  up_motor_target_t *target=&u->motor_target ;	
+	    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->pos)+0.15,3.2,4.3,&u->MID360Context.Curve);
+			if (u->MID360Context.Curve == CURVE_58) {
+        target->Pitch_angle = 58;
+				} else {
+				target->Pitch_angle = 66;
+				}
+			
+       LaunchContext->LaunchCfg.go_up_speed=MID360Cfg->go_up_speed;
+       LaunchContext->LaunchCfg.go_down_speed=MID360Cfg->go_down_speed;
+				
 		
 		switch(c-> CMD_mode)
 		{
+			
 			case 	AUTO_MID360_Pitch:
-
+				if(MID360Context->IsLaunch==0){
-			if (u->Pitch_Cfg .PitchState  ==PITCH_PREPARE) 
+				MID360Context->IsLaunch=1;
-				{
+				LaunchContext->LaunchState = Launch_PREPARE;
-		      u->Pitch_Cfg .PitchState=PITCH_START;//置标志位用于启动投篮
 				}
-				/*根据距离实时计算所需pos*/	
+			  Pitch_Launch_Sequence(u,LaunchContext,u->motorfeedback.go_data.Pos,MID360Cfg->go_release_pos,out);
-				
+			
-        pitch_cfg ->go_release_pos=
-				CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos),3,4,&u->Pitch_Cfg.Curve);
-
-			 Pitch_Process(u,out);
 			break ;
 				
 			case AUTO_MID360:
-						target->Shoot_M2006_angle =pitch_cfg ->m2006_init ;
+					target->Shoot_M2006_angle=	LaunchContext->LaunchCfg.m2006_init;
-
+				MID360Context->IsLaunch=0;
-						u->Pitch_Cfg .PitchState   = PITCH_PREPARE;  
-				
 				break ;
+					default:
+			break;
 			
 		}
-			
-		break ;	
-
-			case RELAXED:
-//			target->go_shoot=	pitch_cfg .
-			
-			target->go_shoot = pitch_cfg->go_init;
-			target->Pitch_angle = 58;
-			target->Shoot_M2006_angle =pitch_cfg->m2006_init;
-			
-			break ;
-	
-}
-
-return 0;
-}
-
-
-
-
-
-
-
-int8_t Pitch_Process(UP_t *u, CAN_Output_t *out)
-{
-	
-	PitchCfg_t *cfg = &u->Pitch_Cfg.PitchConfig;
-	PitchState_t *state =&u->Pitch_Cfg .PitchState;
-  up_motor_target_t *target=&u->motor_target ;
-	
-	switch(*state){
-	
-		case PITCH_START:
-//			u->motor_target .Dribble_translate_M3508_angle =u->DribbleCont .DribbleConfig.m3508_translate_angle;//平行移动 
-      cfg->pull_speed=12;
-			target->go_shoot = cfg->go_pull_pos;
 		
-		if(u->motorfeedback .go_data .Pos < -209){ //检测go位置到达最上面，这里的检测条件可以更改
+	}
-			target->Shoot_M2006_angle = cfg->m2006_trig ;//设置2006角度，0
-		  u->Pitch_Cfg .PitchConfig .pull_speed=6;
-			*state=PITCH_PULL_TRIGGER;
-}//更改标志位
-
-			break ;
-		case PITCH_PULL_TRIGGER:
-			
-		
-			if( u->motorfeedback .DJmotor_feedback [4].total_angle>-1)  //当2006的总角度小于1，可以认为已经勾上,误差为1
-			{
-         target->go_shoot=cfg->go_release_pos; 
-				if(is_reached (u->motorfeedback .go_data .Pos ,target->go_shoot ,1.0f))
-			  {
-				*state=PITCH_LAUNCHING;			
-			  }
-			}
-			break ;
-		
-		case PITCH_LAUNCHING: //等待发射	
-//			*state=PITCH_COMPLETE;	
-		break ;
-		case PITCH_COMPLETE:  //发射完成
-			
-		
-	  break ;
-		
-	
-	}
-		
-return 0;
-	
-}
-
-
-
-

User/Module/up.h

@@ -28,26 +28,50 @@
 
 
 */
+/*共用发射，重复部分*/
+typedef struct {
+	  fp32 m2006_init;   // M2006初始角度
+	  fp32 m2006_trig;   // M2006触发角度0,用来合并扳机
+	  fp32 go_init;
+	  fp32 go_pull_pos; // go上升去合并扳机，扳机位置  
+    fp32 go_up_speed;	
+	  fp32 go_down_speed;
+	 
+	  fp32 Pitch_angle;
+} LaunchCfg_t;
+typedef enum  {
+	  Launch_Stop,
+    Launch_PREPARE,        //
+	  Launch_START,          //启动，从指定位置上升，扣动扳机
+    Launch_TRIGGER,        //拉动到指定位置  
+    Launch_SHOOT_WAIT,           // 发射等待
+	  Launch_SHOOT,
+	  Launch_DONE,
+    
+	
+}LaunchState_t;
+typedef struct {
+	
+    LaunchCfg_t LaunchCfg;
+	  LaunchState_t LaunchState;
+} LaunchContext_t;
+
 
 /* 投球状态定义 */
 typedef enum {
+	
     PITCH_PREPARE,          // 准备阶段
-	  PITCH_START,            //启动，拉扳机
+	  PITCH_START,            //启动，拉扳机 
-    PITCH_PULL_TRIGGER,  
+	  PITCH_WAIT,             // 发射等待
-    PITCH_LAUNCHING,        // 发射中
     PITCH_COMPLETE          // 完成
 	
 } PitchState_t;
 
 /* 投球参数配置 */
 typedef struct {
-    fp32 m2006_init;   // M2006初始角度
+    
-	  fp32 m2006_trig;   // M2006触发角度0,用来合并扳机
+    fp32 go_init;     // GO电机触发位置,初始位置，后续更改用于发射的位置	  
-    fp32 go_init;     // GO电机触发位置,初始位置，后续更改用于发射的位置
+	  fp32 Pitch_angle;  //俯仰角度，以oid为准	  
-	  fp32 go_pull_pos; // go上升去合并扳机，扳机位置
-	  fp32 Pitch_angle;  //俯仰角度，以oid为准
-	
-	  fp32 pull_speed;//go速度
 	  fp32 go_release_pos;//go松开，发射位置
 	
 } PitchCfg_t;
@@ -55,19 +79,53 @@ typedef struct {
 typedef struct {
     PitchState_t PitchState;
 	  PitchCfg_t PitchConfig;
+    uint8_t is_init ;
+} PitchContext_t;
+
+/*传球过程 */
+typedef enum {
+	  PASS_STOP,         // 停止状态,未进入传球模式
+	  PASS_IDLE,         // 空闲状态，进入未开始
+    PASS_PREPARE,      // 传球准备
+    PASS_START,        // 启动传球,拉动到等球位置
+    PASS_POS_PREPARE,  // 传球位置准备，对准篮筐，go的位置
+    PASS_COMPLETE      // 发射
+} PassState_t;
+typedef struct {
+	
+	fp32 go_wait;     // GO等待位置
+	fp32 go_release_pos; // GO电机发射位置
+	fp32 Pitch_angle;  // 俯仰角度，以oid为准
+	fp32 go_up_speed; // GO电机上升速度
+	fp32 go_down_speed; // GO电机下降速度
+	
+} PassCfg_t;
+typedef struct {
+    PassState_t PassState;
+	  PassCfg_t PassCfg;
     CurveType Curve;  //当前函数，雷达距离->pos
     uint8_t is_init ;
-} Pitch_Cfg_t;
+} PassContext_t;
+
+/*雷达自动*/
+
+typedef struct {
+
+	fp32 go_release_pos; // GO电机发射位置
+	fp32 go_up_speed; // GO电机上升速度
+	fp32 go_down_speed; // GO电机下降速度
+	
+} MID360Cfg_t;
+
+typedef struct {
+	  MID360Cfg_t MID360Cfg;
+	  
+    CurveType Curve;  //当前函数，雷达距离->pos
+	  int IsLaunch;  //是否在发射过程
+} MID360Context_t;
+
+
 
-/*运球*/
-typedef enum {
-    DRIBBLE_PREPARE,        // 准备阶段
-	  DRIBBLE_START,
-    DRIBBLE_TRANSLATE,      // 平移过程
-		DRIBBLE_PROCESS_DOWN,        // 运球过程，球下落
-	  DRIBBLE_PROCESS_UP,        // 运球过程，球上升
-    DRIBBLE_DONE            // 运球结束
-} DribbleState_t;
 
 typedef struct {
 
@@ -91,7 +149,10 @@ typedef struct
 
   pid_param_t Pitch_Angle_M2006_speed_param;
 	 
+	LaunchCfg_t LaunchCfg;
 	PitchCfg_t  PitchCfg;
+	PassCfg_t   PassCfg;
+	MID360Cfg_t MID360Cfg;
 	
 	GO_MotorCmd_t go_cmd;
 }UP_Param_t;
@@ -103,7 +164,7 @@ typedef struct{
 	  fp32 Shoot_M2006_angle;
 		 
 		fp32 go_shoot;
-
+    fp32 go_pull_speed;
 	  fp32 Pitch_angle;  //以oid的角度为准,目标俯仰角
 		 
 	}up_motor_target_t;
@@ -128,10 +189,14 @@ typedef struct{
 typedef struct{
 	
    uint8_t up_task_run; 
-   const	UP_Param_t *param;  
+   const	UP_Param_t *param; 
+  LaunchContext_t LaunchContext;	
     /*投篮过程*/
-	Pitch_Cfg_t Pitch_Cfg;
+	PitchContext_t PitchContext;
-		  
+	/*传球过程*/
+	PassContext_t PassContext;
+  /*自动过程*/	
+	MID360Context_t MID360Context;
 	 CMD_t *cmd;	
 
 	 struct{	
@@ -184,7 +249,11 @@ int8_t DJ_processdata(DJmotor_feedback_t *f,fp32 ecd_to_angle);
 int8_t DJ_Angle_Control(UP_t *u,int id,DJmotor_feedback_t *f,pid_type_def *Angle_pid,pid_type_def *Speed_pid,fp32 target_angle);
 int8_t DJ_Speed_Control(UP_t *u,int id,DJmotor_feedback_t *f,pid_type_def *Speed_pid,fp32 target_speed);
 
-int8_t Pitch_Process(UP_t *u, CAN_Output_t *out);
+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 UP_AUTO_Control(UP_t *u,CAN_Output_t *out,CMD_t *c);
 
 
 #endif

User/Module/up_utils.c

@@ -1,5 +1,5 @@
 #include "up_utils.h"
-
+#include "up.h"
 
 int8_t DJ_processdata(DJmotor_feedback_t *f,fp32 ecd_to_angle)
 {
@@ -62,25 +62,27 @@ int8_t GO_SendData( GO_MotorData_t *go_data,GO_MotorCmd_t *go_cmd,float pos, flo
 }
 
 
-// 计算58度曲线
+// 计算66度曲线（偏上）
-static float curve_58(float d) {
+// 计算66度曲线（偏上）
-    return 0.448f * d * d + 24.8604f * d - 177.99552f;
-}
-
-// 计算66度曲线
 static float curve_66(float d) {
-    return 8.84935f * d * d - 10.5424f * d - 126.791f;
+    return  4.0310f * d * d + 8.9026f * d -139.5156;
 }
 
+// 计算58度曲线（偏下）
+// 计算58度曲线（偏下）
+static float curve_58(float d) {
+    return -1.9776f * d * d + 42.8499f * d - 204.2442f;
+}
 
 /*
    曲线切换,用于距离和pos拟合
    迟滞区x-y
    曲线x重合区，根据当前函数和变化方向切换
 */
+int abdddd=0;
 float CurveChange(float d, float x, float y, CurveType *cs)
 {
-    
+    if (d<3.2) abdddd++;
     if (*cs == CURVE_66) {
         if (d > y) {
             *cs = CURVE_58;
@@ -99,3 +101,62 @@ float CurveChange(float d, float x, float y, CurveType *cs)
     }
 }
 
+int8_t Pass_Sequence_Check(UP_t *u, CMD_t *c) //按键顺序检测，传球，按需求改
+{
+    PassState_t *state = &u->PassContext.PassState;
+    static enum {
+        SEQ_IDLE,
+        SEQ_MID1,
+        SEQ_UP,
+        SEQ_MID2,
+        SEQ_DOWN
+    } seq = SEQ_IDLE;
+
+    switch (seq) {
+        case SEQ_IDLE:
+            if (c->CMD_mode == PB_MID) {
+                seq = SEQ_MID1;
+                *state = PASS_IDLE;
+            }
+            break;
+        case SEQ_MID1:
+            if (c->CMD_mode == PB_UP) {
+                seq = SEQ_UP;
+                *state = PASS_PREPARE;
+            } else if (c->CMD_mode != PB_MID) {
+                seq = SEQ_IDLE;
+                *state = PASS_STOP;
+            }
+            break;
+        case SEQ_UP:
+            if (c->CMD_mode == PB_MID) {
+                seq = SEQ_MID2;
+                *state = PASS_START;
+            } else if (c->CMD_mode != PB_UP) {
+                seq = SEQ_IDLE;
+                *state = PASS_STOP;
+            }
+            break;
+        case SEQ_MID2:
+            if (c->CMD_mode == PB_DOWN) {
+                seq = SEQ_DOWN;
+                *state = PASS_POS_PREPARE;
+            } else if (c->CMD_mode != PB_MID) {
+                seq = SEQ_IDLE;
+                *state = PASS_STOP;
+            }
+            break;
+        case SEQ_DOWN:
+            if (c->CMD_mode == PB_MID) {
+                seq = SEQ_IDLE;
+                *state = PASS_IDLE; // 或 PASS_COMPLETE，看你需求
+            } else if (c->CMD_mode != PB_DOWN) {
+                seq = SEQ_IDLE;
+                *state = PASS_STOP;
+            }
+            break;
+    }
+    return 0;
+}
+
+

User/device/cmd.c

@@ -164,10 +164,10 @@ int8_t CMD_DR16(CMD_t *cmd,const CMD_RC_t *rc) {
 	}
 	else if(rc->dr16.sw_l==CMD_SW_DOWN)
 	{
-		cmd ->CMD_CtrlType =RCcontrol;
+		cmd ->CMD_CtrlType =PASS_BALL;
-		if(rc->dr16.sw_r ==CMD_SW_UP) cmd ->CMD_mode =Normal; //左下，右上，无模式
+		if(rc->dr16.sw_r ==CMD_SW_UP) cmd ->CMD_mode =PB_UP; //左下，右上，无模式
-		if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode =Normal; //左下，右中，无模式
+		if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode = PB_MID; //左下，右中，无模式
-	  if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Normal; //左下，右上，无模式
+	  if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =PB_DOWN; //左下，右上，无模式
 	}	
     
 

User/device/cmd.h

@@ -14,6 +14,7 @@
 typedef enum{
 	RCcontrol,//遥控器控制，左按键上，控制上层
 	AUTO,
+  PASS_BALL,//传球模式
 	RELAXED,//异常模式
 
  }CMD_CtrlType_t;
@@ -29,11 +30,13 @@ typedef  enum{
 	
 	  UP_Adjust,//上层调整
    	Chassis_Adjust_Sick,
-	
+	/*传球占三个,逻辑在up.h里，简化就这样写了*/
-	  PassBall,
+	  PB_UP,
+    PB_MID,
+    PB_DOWN,
 
    
-	  Pitch,     //投篮,底盘锁定
+	  Pitch,     
 	
 	  
     Dribbl_transfer

User/device/nuc.c

@@ -2,176 +2,181 @@
 #include "crc16.h"
 #include <string.h>
 #include "error_detect.h"
-static volatile uint32_t drop_message = 0;
+volatile uint32_t drop_message = 0;
 
 static osThreadId_t thread_alert;
 
- uint8_t nucbuf[31];
+uint8_t nucbuf[25];
- char SendBuffer[7];
+char SendBuffer[7];
 
-
+static void NUC_CBLTCallback(void)
-static void NUC_IdleCallback(void) {	
+{
-	osThreadFlagsSet(thread_alert,SIGNAL_NUC_RAW_REDY);
+  osThreadFlagsSet(thread_alert, SIGNAL_NUC_RAW_REDY);
-	detect_hook(NUC_TOE);
+  // detect_hook(NUC_TOE);
 }
-int8_t NUC_Init(NUC_t *nuc){
+static void NUC_ERRORCALLBACK(void)
-    if(nuc == NULL) return DEVICE_ERR_NULL;
+{
-    if((thread_alert = osThreadGetId()) == NULL ) return DEVICE_ERR_NULL;
+  NUC_Restart();
-   BSP_UART_RegisterCallback(BSP_UART_NUC,BSP_UART_IDLE_LINE_CB,
+  uint32_t error_code = HAL_UART_GetError(BSP_UART_GetHandle(BSP_UART_NUC));
-                            NUC_IdleCallback);
+  // osThreadFlagsSet(thread_alert, SIGNAL_NUC_ERROR);
-	
-   return DEVICE_OK;
 }
-int8_t NUC_StartReceiving() {
+int8_t NUC_Init(NUC_t *nuc)
-  if (HAL_UARTEx_ReceiveToIdle_DMA(BSP_UART_GetHandle(BSP_UART_NUC),
+{
+  if (nuc == NULL) 
+    return DEVICE_ERR_NULL;
+  if ((thread_alert = osThreadGetId()) == NULL)
+    return DEVICE_ERR_NULL;
+  BSP_UART_RegisterCallback(BSP_UART_NUC, BSP_UART_RX_CPLT_CB,
+                            NUC_CBLTCallback);
+  BSP_UART_RegisterCallback(BSP_UART_NUC, BSP_UART_ERROR_CB,
+                            NUC_ERRORCALLBACK);
+  return DEVICE_OK;
+}
+int8_t NUC_StartReceiving()
+{
+  if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_NUC),
                            (uint8_t *)nucbuf,
                            sizeof(nucbuf)) == HAL_OK)
     return DEVICE_OK;
   return DEVICE_ERR;
 }
-int8_t NUC_StartSending(fp32 *data) {
+int8_t NUC_StartSending(fp32 *data)
+{
 
-   	union 
+  union
-	{
-		float x[1];
-		char data[4];
-	}instance;
-  
-
-//      for (int i = 0; i < 1; i++) {
-        instance.x[0] = data[0];
-//    }
-
-  SendBuffer[0] = 0xFC; //帧头
-  SendBuffer[1] = 0x01; //控制帧
-  for(int i = 2; i < 6; i++)
   {
-    SendBuffer[i] = instance.data[i-2];
+    float x[1];
-  }
+    char data[4];
-  SendBuffer[6] = 0xFD; //帧尾
+  } instance;
 
-    if (HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_NUC),
+  //      for (int i = 0; i < 1; i++) {
-                              (uint8_t *)SendBuffer,
+  instance.x[0] = data[0];
-                              sizeof(SendBuffer)) == HAL_OK)
+  //    }
+
+  SendBuffer[0] = 0xFC; // 帧头
+  SendBuffer[1] = 0x01; // 控制帧
+  for (int i = 2; i < 6; i++)
+  {
+    SendBuffer[i] = instance.data[i - 2];
+  }
+  SendBuffer[6] = 0xFD; // 帧尾
+
+  if (HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_NUC),
+                            (uint8_t *)SendBuffer,
+                            sizeof(SendBuffer)) == HAL_OK)
     return DEVICE_OK;
   return DEVICE_ERR;
 }
-int8_t NUC_Restart(void) {
+int8_t NUC_Restart(void)
+{
   __HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_NUC));
   __HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_NUC));
   return DEVICE_OK;
 }
-bool_t NUC_WaitDmaCplt(void) {
+bool_t NUC_WaitDmaCplt(void)
-  return (osThreadFlagsWait(SIGNAL_NUC_RAW_REDY, osFlagsWaitAll,20) ==
+{
+  return (osThreadFlagsWait(SIGNAL_NUC_RAW_REDY, osFlagsWaitAll, 20) ==
           SIGNAL_NUC_RAW_REDY);
 }
 
-
+int8_t NUC_RawParse(CMD_NUC_t *n)
-int8_t NUC_RawParse(CMD_NUC_t *n){
+{
-	if(n ==NULL) return DEVICE_ERR_NULL;
+  if (n == NULL)
- 	union 
+    return DEVICE_ERR_NULL;
-	{
+  union
-		float x[5];
+  {
-		char data[20];
+    float x[5];
-	}instance;
+    char data[20];
-  if(nucbuf[0]!=HEAD) goto error;  //发送ID不是底盘
+  } instance;
-  else{
+  if (nucbuf[0] != HEAD)
-    n->status_fromnuc =nucbuf[1];
+    goto error; // 发送ID不是底盘
-		n->ctrl_status =nucbuf[2];
+  else
+  {
+    n->status_fromnuc = nucbuf[1];
+    n->ctrl_status = nucbuf[2];
     switch (n->status_fromnuc)
     {
-		case VISION://控制帧0x02
+    case VISION: // 控制帧0x02
-				/* 协议格式 
+      /* 协议格式
-		   0xFF  HEAD
+     0xFF  HEAD
-		   0x02  控制帧
+     0x02  控制帧
-		   0x01  相机帧
+     0x01  相机帧
-		   vx    fp32
+     vx    fp32
-		   vy    fp32
+     vy    fp32
-		   wz    fp32
+     wz    fp32
-		   0xFE  TAIL
+     0xFE  TAIL
-		  */
+    */
 
-
+      instance.data[3] = nucbuf[3];
-		instance.data[3] = nucbuf[3];
+      instance.data[2] = nucbuf[4];
-       instance.data[2] = nucbuf[4];
+      instance.data[1] = nucbuf[5];
-       instance.data[1] = nucbuf[5];
+      instance.data[0] = nucbuf[6];
-       instance.data[0] = nucbuf[6];
+      n->camera.data1 = instance.x[0]; //
-      n->camera .data1  = instance.x[0]; //
+      instance.data[7] = nucbuf[7];
-       instance.data[7] = nucbuf[7];
+      instance.data[6] = nucbuf[8];
-       instance.data[6] = nucbuf[8];
+      instance.data[5] = nucbuf[9];
-       instance.data[5] = nucbuf[9];
+      instance.data[4] = nucbuf[10];
-       instance.data[4] = nucbuf[10];
+      n->camera.data2 = instance.x[1]; //
-      n->camera .data2 = instance.x[1];//
+      instance.data[11] = nucbuf[11];
-       instance.data[11] = nucbuf[11];
+      instance.data[10] = nucbuf[12];
-       instance.data[10] = nucbuf[12];
+      instance.data[9] = nucbuf[13];
-       instance.data[9] = nucbuf[13];
+      instance.data[8] = nucbuf[14];
-       instance.data[8] = nucbuf[14];
+      n->camera.data3 = instance.x[2]; //
-      n->camera .data3 = instance.x[2];//
       break;
-		case MID ://控制帧0x08
+    case MID: // 控制帧0x08
-				/* 协议格式 
+      /* 协议格式
-		   0xFF  HEAD
+     0xFF  HEAD
-		   0x09  控制帧
+     0x09  控制帧
-		   0x01  相机帧
+     0x01  相机帧
-		   vx    fp32
+     vx    fp32
-		   vy    fp32
+     vy    fp32
-		   wz    fp32
+     wz    fp32
-		   0xFE  TAIL
+     0xFE  TAIL
-		  */
+    */
-		if(nucbuf[24]!=TAIL)goto error;
+      if (nucbuf[24] != TAIL)
-       instance.data[3] = nucbuf[6];
+        goto error;
-       instance.data[2] = nucbuf[5];
+      instance.data[3] = nucbuf[6];
-       instance.data[1] = nucbuf[4];
+      instance.data[2] = nucbuf[5];
-       instance.data[0] = nucbuf[3];
+      instance.data[1] = nucbuf[4];
+      instance.data[0] = nucbuf[3];
       n->MID360.vx = instance.x[0]; //
-       instance.data[7] = nucbuf[10];
+      instance.data[7] = nucbuf[10];
-       instance.data[6] = nucbuf[9];
+      instance.data[6] = nucbuf[9];
-       instance.data[5] = nucbuf[8];
+      instance.data[5] = nucbuf[8];
-       instance.data[4] = nucbuf[7];
+      instance.data[4] = nucbuf[7];
-      n->MID360.vy = instance.x[1];//
+      n->MID360.vy = instance.x[1]; //
-       instance.data[11] = nucbuf[14];
+      instance.data[11] = nucbuf[14];
-       instance.data[10] = nucbuf[13];
+      instance.data[10] = nucbuf[13];
-       instance.data[9] = nucbuf[12];
+      instance.data[9] = nucbuf[12];
-       instance.data[8] = nucbuf[11];
+      instance.data[8] = nucbuf[11];
-      n->MID360.wz = instance.x[2];//
+      n->MID360.wz = instance.x[2]; //
-			 instance.data[15] = nucbuf[18];
+      instance.data[15] = nucbuf[18];
-       instance.data[14] = nucbuf[17];
+      instance.data[14] = nucbuf[17];
-       instance.data[13] = nucbuf[16];
+      instance.data[13] = nucbuf[16];
-       instance.data[12] = nucbuf[15];
+      instance.data[12] = nucbuf[15];
-      n->MID360.pos = instance.x[3];//
+      n->MID360.pos = instance.x[3]; //
-			 instance.data[19] = nucbuf[22];
+      instance.data[19] = nucbuf[22];
-       instance.data[18] = nucbuf[21];
+      instance.data[18] = nucbuf[21];
-       instance.data[17] = nucbuf[20];
+      instance.data[17] = nucbuf[20];
-       instance.data[16] = nucbuf[19];
+      instance.data[16] = nucbuf[19];
-      n->MID360.angle  = instance.x[4];//
+      n->MID360.angle = instance.x[4]; //
+
+      n->MID360.flag = nucbuf[23]; //
 
-      n->MID360.flag = nucbuf[23];//
-			
-			
-			
-			
-			
-			
-			
-			
-			
       break;
-
-
     }
-     return DEVICE_OK;
+    return DEVICE_OK;
   }
-    error:
+error:
-    drop_message++;
+  drop_message++;
-    return DEVICE_ERR;
+  NUC_Restart();
+  return DEVICE_ERR;
 }
 
 int8_t NUC_HandleOffline(CMD_NUC_t *cmd)
 {
-	if(cmd == NULL) return DEVICE_ERR_NULL;
+  if (cmd == NULL)
-	 memset(cmd, 0, sizeof(*cmd));
+    return DEVICE_ERR_NULL;
-	return 0;
+  //	 memset(cmd, 0, sizeof(*cmd));
+  return 0;
 }
-
-
-

User/device/nuc.h

@@ -33,5 +33,7 @@ int8_t NUC_StartSending(fp32 *data) ;
 bool_t NUC_WaitDmaCplt(void);
 int8_t NUC_RawParse(CMD_NUC_t *n);
 int8_t NUC_HandleOffline(CMD_NUC_t *cmd);
+int8_t NUC_Restart(void);
+
 #endif
 

User/device/vofa.c

@@ -59,7 +59,7 @@ void vofa_tx_main(float *data)
 
     /*在下面使用对应的串口发送函数*/
 //		HAL_UART_Transmit_DMA(&huart6, packet, sizeof(packet));
-//    CDC_Transmit_FS( packet, sizeof(packet));
+    CDC_Transmit_FS( packet, sizeof(packet));
-//      osDelay(1);
+      osDelay(1);
    
 }

User/task/chassis_task.c

@@ -98,7 +98,7 @@ void Task_Chassis(void *argument)
 	vofa_send[6] = chassis.vofa_send[6];
 	vofa_send[7] = chassis.vofa_send[7];
 		 
-		vofa_tx_main(vofa_send);
+//		vofa_tx_main(vofa_send);
 	tick += delay_tick;
         osDelayUntil(tick);
 		 

User/task/nuc_task.c

@@ -18,7 +18,7 @@ int a=0;
 void Task_nuc(void *argument){
  (void)argument; /**/
 
-//	 osDelay(TASK_INIT_DELAY_NUC); 
+	 osDelay(TASK_INIT_DELAY_NUC); 
   const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_NUC;
 
    NUC_Init(&nuc_raw);
@@ -35,8 +35,8 @@ void Task_nuc(void *argument){
 #endif
 			 a++;
 			NUC_StartReceiving();
-			NUC_StartSending(NUC_send.send);
+			// NUC_StartSending(NUC_send.send);
-//			 NUC_StartSending (send);
+			//  NUC_StartSending (send);
       if (NUC_WaitDmaCplt()){ 
          NUC_RawParse(&cmd_fromnuc);
       }

User/task/up_task.c

@@ -91,8 +91,11 @@ void Task_up(void *argument)
 		osMessageQueuePut(task_runtime.msgq.can.output.shoot5065 ,&UP_CAN_out.chassis5065, 0, 0); 
 	
 
-//   vofa_send [0]=UP.vofa_send [0];
+   vofa_send [0]=UP.vofa_send [0];
-//   vofa_send [1]=UP.vofa_send [1];
+   vofa_send [1]=UP.vofa_send [1];
+   vofa_send [2]=UP.vofa_send [2];
+   vofa_send [3]=UP.vofa_send [3];
+   vofa_send [4]=UP.vofa_send [4];
 	 vofa_tx_main (vofa_send);
 	 
 	tick += delay_tick;

User/task/user_task.h

@@ -18,7 +18,7 @@
 #define TASK_FREQ_UP (900u)
 
 #define TASK_FREQ_CTRL_CMD (500u)
-#define TASK_FREQ_NUC (50u)
+#define TASK_FREQ_NUC (500u)
 #define TASK_FREQ_CAN (1000u)
 #define TASK_FREQ_RC (1000u)