diff --git a/MDK-ARM/AUTO_CHASSIS.uvoptx b/MDK-ARM/AUTO_CHASSIS.uvoptx
index 8492b49..80d1c51 100644
--- a/MDK-ARM/AUTO_CHASSIS.uvoptx
+++ b/MDK-ARM/AUTO_CHASSIS.uvoptx
@@ -103,7 +103,7 @@
         <bEvRecOn>1</bEvRecOn>
         <bSchkAxf>0</bSchkAxf>
         <bTchkAxf>0</bTchkAxf>
-        <nTsel>3</nTsel>
+        <nTsel>6</nTsel>
         <sDll></sDll>
         <sDllPa></sDllPa>
         <sDlgDll></sDlgDll>
@@ -114,7 +114,7 @@
         <tDlgDll></tDlgDll>
         <tDlgPa></tDlgPa>
         <tIfile></tIfile>
-        <pMon>BIN\CMSIS_AGDI.dll</pMon>
+        <pMon>STLink\ST-LINKIII-KEIL_SWO.dll</pMon>
       </DebugOpt>
       <TargetDriverDllRegistry>
         <SetRegEntry>
@@ -140,12 +140,12 @@
         <SetRegEntry>
           <Number>0</Number>
           <Key>CMSIS_AGDI</Key>
-          <Name>-X"Horco CMSIS-DAP" -U4626385832 -O206 -S0 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO65554 -TC168000000 -TT168000000 -TP20 -TDS8010 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)</Name>
+          <Name>-X"Horco CMSIS-DAP" -U8626380832 -O206 -S0 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO65554 -TC168000000 -TT168000000 -TP20 -TDS8010 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)</Name>
         </SetRegEntry>
         <SetRegEntry>
           <Number>0</Number>
           <Key>DLGUARM</Key>
-          <Name></Name>
+          <Name>(105=-1,-1,-1,-1,0)</Name>
         </SetRegEntry>
         <SetRegEntry>
           <Number>0</Number>
@@ -180,6 +180,11 @@
           <WinNumber>1</WinNumber>
           <ItemText>motor_send_data,0x0A</ItemText>
         </Ww>
+        <Ww>
+          <count>5</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>GO_calibration,0x0A</ItemText>
+        </Ww>
       </WatchWindow1>
       <WatchWindow2>
         <Ww>
@@ -194,7 +199,7 @@
       <DebugFlag>
         <trace>0</trace>
         <periodic>1</periodic>
-        <aLwin>1</aLwin>
+        <aLwin>0</aLwin>
         <aCover>0</aCover>
         <aSer1>0</aSer1>
         <aSer2>0</aSer2>
diff --git a/MDK-ARM/ELE_CHASSIS/AUTO_CHASSIS.axf b/MDK-ARM/ELE_CHASSIS/AUTO_CHASSIS.axf
new file mode 100644
index 0000000..32d3906
Binary files /dev/null and b/MDK-ARM/ELE_CHASSIS/AUTO_CHASSIS.axf differ
diff --git a/User/Module/config.c b/User/Module/config.c
index 018fbc1..2372880 100644
--- a/User/Module/config.c
+++ b/User/Module/config.c
@@ -22,48 +22,33 @@ static const ConfigParam_t param_chassis ={
 		
 		
 		/*上层pid参数*/
-.M2006_angle_param = {  
+.Shoot_M2006_angle_param = {  
     .p = 25.0f,        
     .i = 0.0f,         
     .d = 1.5f,         
     .i_limit = 1000.0f,
     .out_limit = 3000.0f,
 },
-.M2006_speed_param = {  
+.Shoot_M2006_speed_param = {  
     .p = 5.0f,         
     .i = 0.3f,         
     .d = 0.0f,         
     .i_limit = 2000.0f,
     .out_limit = 3000.0f,
 },
-.M3508_angle_param = {  
-    .p = 10.0f,        
-    .i = 0.5f,         
-    .d = 0.0f,         
+.Pitch_M2006_angle_param = {  
+    .p = 25.0f,        
+    .i = 0.0f,         
+    .d = 1.5f,         
     .i_limit = 1000.0f,
     .out_limit = 3000.0f,
 },
-.M3508_speed_param={
-		 .p = 6.0f,
-		 .i = 0.0f,
-		 .d = 3.2f,
-		 .i_limit = 200.0f,
-		 .out_limit =6000.0f,
-},
-
-.UP_GM6020_angle_param={
-			.p = 30.0f,
-			.i = 20.0f,
-			.d =0.0f,
-			.i_limit = 200.0f,
-			.out_limit = 1500.0f,
-}	,
-.UP_GM6020_speed_param={
-      .p =3.0f,
-			.i =0.5f,
-			.d =0.0f,
-			.i_limit = 200.0f,
-			.out_limit = 3000.0f  			
+.Pitch_M2006_speed_param={
+    .p = 5.0f,         
+    .i = 0.3f,         
+    .d = 0.0f,         
+    .i_limit = 2000.0f,
+    .out_limit = 3000.0f,
 },
 
 
diff --git a/User/Module/up.c b/User/Module/up.c
index 2a137a6..5d1e45d 100644
--- a/User/Module/up.c
+++ b/User/Module/up.c
@@ -24,14 +24,12 @@ int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
 	PID_init (&u->pid.VESC_5065_M1 ,PID_POSITION ,&(u->param ->VESC_5065_M1_param  ));
 	PID_init (&u->pid.VESC_5065_M2 ,PID_POSITION ,&(u->param ->VESC_5065_M2_param ));
 
-	PID_init (&u->pid .M2006_angle ,PID_POSITION ,&(u->param->M2006_angle_param  ));
-	PID_init (&u->pid .M2006_speed ,PID_POSITION ,&(u->param->M2006_speed_param ));
+	PID_init (&u->pid .Shoot_M2006angle ,PID_POSITION ,&(u->param->Shoot_M2006_angle_param  ));
+	PID_init (&u->pid .Shoot_M2006speed ,PID_POSITION ,&(u->param->Shoot_M2006_speed_param ));
 	
-	PID_init (&u->pid .M3508_angle ,PID_POSITION ,&(u->param->M3508_angle_param  ));
-	PID_init (&u->pid .M3508_speed ,PID_POSITION ,&(u->param->M3508_speed_param ));
+	PID_init (&u->pid .Pitch_M2006_angle ,PID_POSITION ,&(u->param->Pitch_M2006_angle_param  ));
+	PID_init (&u->pid .Pitch_M2006_speed ,PID_POSITION ,&(u->param->Pitch_M2006_speed_param ));
 	
-	PID_init (&u->pid .GM6020_speed,PID_POSITION ,&(u->param ->UP_GM6020_speed_param ));
-	PID_init (&u->pid .GM6020_angle,PID_POSITION ,&(u->param ->UP_GM6020_angle_param ));
 	
 	u->go_cmd =u->param ->go_cmd ;
 	
@@ -56,20 +54,16 @@ int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
 /*can，上层状态更新*/
 int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *c) {
 
-	u->motorfeedback .go_data = get_GO_measure_point() ;
 	
-	u->motorfeedback .M2006 .motor =M2006;
-	u->motorfeedback .M3508 .motor =M3508;
 
 	u->motorfeedback .VESC_5065_M1_rpm =can ->motor .chassis5065 .as_array [0].rotor_speed ;
 	u->motorfeedback .VESC_5065_M2_rpm =can ->motor .chassis5065 .as_array [1].rotor_speed ;
 
-	u->motorfeedback .rotor_pit6020ecd =can ->motor .chassis6020.as_array [2].rotor_ecd ;
-	u->motorfeedback .rotor_pit6020rpm =can ->motor .chassis6020.as_array [2].rotor_speed  ;
 	
 	for(int i=0;i<4;i++){
-	u->motorfeedback .M3508_rpm[i] =can ->motor .motor3508 .as_array [i].rotor_speed ;
-	u->motorfeedback .M3508_angle [i]=can ->motor .motor3508 .as_array [i].rotor_ecd ;
+	u->motorfeedback .DJmotor_feedback[i].rpm =can ->motor .motor3508 .as_array [i].rotor_speed ;
+	u->motorfeedback .DJmotor_feedback[i].ecd =can ->motor .motor3508 .as_array [i].rotor_ecd ;
+	DJ_processdata(&u->motorfeedback .DJmotor_feedback [i],	MOTOR2006_ECD_TO_ANGLE);
 	}
 	
 	u->cmd =c;
@@ -77,90 +71,42 @@ int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *c) {
 	return 0;
 }
 
-
-
-/*上层大疆电机角度控制,使用can1的id1和2*/
-int8_t UP_angle_control(UP_t *u, fp32 target_angle,MotorType_t motor) {
-    // 获取当前编码器角度
-	int8_t cnt=0;
-	fp32 angle ,delta;
-
-	switch(motor)
-	{
-		case M2006 :  			
-    angle = u->motorfeedback.M3508_angle[0]; 
-    if (u->motorfeedback .M2006 .init_cnt < 50) {
-        u->motorfeedback .M2006.orig_angle = angle;    // 记录初始编码器值
-        u->motorfeedback .M2006.last_angle = angle;    
-			
-        u->motorfeedback .M2006.init_cnt++;            // 初始化计数器递增
-        return 0;                       
+int8_t DJ_processdata(DJmotor_feedback_t *f,fp32 ecd_to_angle)
+{
+	  int8_t cnt=0;
+	  fp32 angle ,delta;
+ 			
+    angle = f->ecd;  
+    if (f->init_cnt < 50) {
+		f->orig_angle= angle;    
+		f->last_angle = angle;
+		f->init_cnt++;            
+     return 0;			
     }
 		
-		    delta = angle - u->motorfeedback .M2006.last_angle;
-	    	delta = angle - u->motorfeedback .M2006.last_angle;
+	
+	  delta = angle - f->last_angle;
     if (delta > 4096) {
-        u->motorfeedback .M2006.round_cnt--;           // 逆时针跨圈
+        f->round_cnt--;           
     } else if (delta < -4096) {
-        u->motorfeedback .M2006.round_cnt++;           // 顺时针跨圈
+        f->round_cnt++;          
     }
-        u->motorfeedback.M2006.last_angle = angle;
-		    // 计算总角度
-    float total_angle = (u->motorfeedback.M2006 .round_cnt * 8191 + (angle - u->motorfeedback.M2006.orig_angle)) * MOTOR2006_ECD_TO_ANGLE;
-    u->motorfeedback.M2006.total_angle =total_angle;
-    
-    float delta_angle = PID_calc(&u->pid.M2006_angle, total_angle, target_angle);
-    float delta_speed = PID_calc(&u->pid.M2006_speed, u->motorfeedback.M3508_rpm [0], delta_angle);
-    u->motor_target.M2006_angle = target_angle;
-    u->final_out .final_3508out [0] =delta_speed;
-
-		break ;
-		
-		case M3508 : 
-			
-    angle = u->motorfeedback.M3508_angle[1]; 
-    if (u->motorfeedback .M3508 .init_cnt < 50) {
-        u->motorfeedback .M3508.orig_angle = angle;    // 记录初始编码器值
-        u->motorfeedback .M3508.last_angle = angle;    
-			
-        u->motorfeedback .M3508.init_cnt++;            // 初始化计数器递增
-        return 0;                       
-    }
-		
-		    delta = angle - u->motorfeedback .M3508.last_angle;
-	    	delta = angle - u->motorfeedback .M3508.last_angle;
-    if (delta > 4096) {
-        u->motorfeedback .M3508.round_cnt--;           // 逆时针跨圈
-    } else if (delta < -4096) {
-        u->motorfeedback .M3508.round_cnt++;           // 顺时针跨圈
-    }
-        u->motorfeedback.M3508.last_angle = angle;
-		    // 计算总角度
-     total_angle = (u->motorfeedback.M3508 .round_cnt * 8191 + (angle - u->motorfeedback.M3508.orig_angle)) * MOTOR2006_ECD_TO_ANGLE; 
-    u->motorfeedback.M3508.total_angle =total_angle;
-    
-     delta_angle = PID_calc(&u->pid.M3508_angle , total_angle, target_angle);
-     delta_speed = PID_calc(&u->pid.M3508_speed , u->motorfeedback.M3508_rpm [1], delta_angle);
-		
-    u->motor_target.M3508_angle  = target_angle;
-    u->final_out .final_3508out[1] =delta_speed;
-
-		break ;  
-
-	}
-    return 0;
+	    f->last_angle = angle;
+		  f->total_angle=(f->round_cnt*8191+(angle -f->orig_angle ))*ecd_to_angle;
+	
+}
+/*
+  这里id范围为1-4，
+*/
+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)
+{
+	fp32 delta_angle,delta_speed;
+  delta_angle = PID_calc(Angle_pid , f->total_angle , target_angle);
+  delta_speed = PID_calc(Speed_pid , f->rpm , delta_angle);
+	
+	u->final_out .DJfinal_out [id-1]=delta_speed;
+	return 0;
 }
-
-//int8_t UP_M3508_speed(UP_t *u,fp32 speed)
-//{
-//	u->motor_target .M3508_speed [] =speed;
-//	for(int i=0;i<3;i++){
-//	    u->final_out .final_3508out [i] =
-//	        PID_calc (&(u->pid .M3508_speed[i] ),u->motorfeedback .M3508_speed [i],speed );	
-//	}
-//	
-//}
-
 
 int8_t VESC_M5065_Control(UP_t *u,fp32 speed)
 {
@@ -175,73 +121,75 @@ int8_t VESC_M5065_Control(UP_t *u,fp32 speed)
 }
 
 
-int8_t GM6020_control(UP_t *u,fp32 angle)
-{	
-	fp32 delat_speed;
-//	Clip(&angle,90,270);
 
-	delat_speed =
-	PID_calc (&(u->pid .GM6020_angle ),u->motorfeedback .rotor_pit6020ecd ,(angle /360*8191));
-	u->final_out .final_pitchout =
-	PID_calc (&(u->pid .GM6020_speed  ),u->motorfeedback.rotor_pit6020rpm ,delat_speed);
-	u->motor_target .rotor_pit6020angle =angle ;
-	return 0;
-
-}
 
 /*go电机控制*/	 
 
-int8_t GO_SendData(UP_t *u,float pos,float limit)
+int8_t GO_SendData(UP_t *u, float pos, float limit)
 { 
-	 	u->go_cmd .Pos =pos;
-	    // 读取参数
-    float tff = u->go_cmd.T ;         // 前馈力矩
-    float kp = u->go_cmd .K_P;        // 刚度
-    float kd = u->go_cmd.K_W;        // 阻尼（原代码中kdd/kid混淆，需统一为K_W）
-    float q_desired = u->go_cmd.Pos; // 期望位置（rad）
-    float q_current = u->motorfeedback.go_data ->Pos ; // 当前位置（rad）
-    float dq_desired = u->go_cmd.W;  // 期望速度（rad/s）
-    float dq_current = u->motorfeedback.go_data ->W ; // 当前速度（rad/s）
+	static int is_calibration=0;
+	static fp32 error=0; //误差量
 
+	GO_MotorData_t *GO_calibration;//校准前，原始数据
+	GO_calibration = get_GO_measure_point() ;
+	if(is_calibration==0)
+	{
+		is_calibration=HAL_GPIO_ReadPin (GPIOE ,GPIO_PIN_9 );
+		u->go_cmd.Pos = -50; //上电之后跑		
+	  error= GO_calibration->Pos ;
+	}	  
+    u->motorfeedback .go_data  = GO_calibration;
+	  u->motorfeedback .go_data ->Pos= error  ;
+    u->go_cmd.Pos = pos;
+
+    // 读取参数
+    float tff = u->go_cmd.T;          // 前馈力矩
+    float kp = u->go_cmd.K_P;         // 位置刚度
+    float kd = u->go_cmd.K_W;         // 速度阻尼
+    float q_desired = u->go_cmd.Pos;  // 期望位置（rad）
+    float q_current = u->motorfeedback.go_data->Pos; // 当前角度位置（rad）
+    float dq_desired = u->go_cmd.W;  // 期望角速度（rad/s）
+    float dq_current = u->motorfeedback.go_data->W;  // 当前角速度（rad/s）
+
+    // 计算输出力矩 tau
     float tau = tff + kp * (q_desired - q_current) + kd * (dq_desired - dq_current);
-	
 
-    
-    if (tau > limit) {
-       
-        tff = limit - kp * (q_desired - q_current) - kd * (dq_desired - dq_current) - (tau - limit) / 2.0;
-        tau = limit;
-    } else if (tau < -limit) {
+	  /*限制最大输入来限制最大输出*/
+	if (pos - q_current > limit) {
+		u->go_cmd.Pos = q_current + limit; // 限制位置
+	}else if (pos - q_current < -limit) {
+		u->go_cmd.Pos = q_current - limit; // 限制位置
+	}else {
+		u->go_cmd.Pos = pos; // 允许位置
+	}
 
-        tff = limit - kp * (q_desired - q_current) - kd * (dq_desired - dq_current) + (tau - limit) / 2.0;
-        tau = -limit;
-    }	
-		u->go_cmd .T =tff;
-	  GO_M8010_send_data(&u->go_cmd );	
-		u->go_cmd .T =0;
+    // 发送数据
+    GO_M8010_send_data(&u->go_cmd);
 
-   return 0;
+
+    return 0;
 }
 
 int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
 {
 	//电机控制	，传进can
-	
-	UP_angle_control(u,u->motor_target .M2006_angle ,M2006);
-	UP_angle_control(u,u->motor_target .M3508_angle ,M3508 );
-	
-	
-
-	GO_SendData(u,u->motor_target .go_shoot,0.08 );
+  DJ_Angle_Control(u,1,&u->motorfeedback .DJmotor_feedback[0] ,
+	                 &u->pid .Shoot_M2006angle ,
+	                 &u->pid .Shoot_M2006speed ,
+	                 u->motor_target .Shoot_M2006_angle );
+	DJ_Angle_Control(u,2,&u->motorfeedback .DJmotor_feedback[1] ,
+	                 &u->pid .Pitch_M2006_angle ,
+	                 &u->pid .Pitch_M2006_speed ,
+	                 u->motor_target .Pitch_M2006_angle );
+	GO_SendData(u,u->motor_target .go_shoot,1 );
 
 	
 	for(int i=0;i<4;i++){
-	out ->motor3508 .as_array[i]=u->final_out.final_3508out [i] ;
+	out ->motor3508 .as_array[i]=u->final_out.DJfinal_out [i] ;
 	}
 	
 	out ->chassis5065 .erpm [0]=  u->final_out .final_VESC_5065_M1out ;
 	out ->chassis5065 .erpm [1]=  -u->final_out .final_VESC_5065_M2out ;
-	out ->chassis6020 .as_array [2]=u->final_out .final_pitchout ;
 	
 	return 0;
 
@@ -270,8 +218,8 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
 		u->motor_target .go_shoot =u->PitchContext .PitchConfig .go1_init_position ;
 		is_pitch=0;	
 		}
-		u->motor_target .M2006_angle =u->PitchContext .PitchConfig .m2006_init_angle ;
-		u->PitchContext .PitchState   = PITCH_PREPARE;  //状态更新,开始夹球
+//		u->motor_target .M2006_angle =u->PitchContext .PitchConfig .m2006_init_angle ;
+//		u->PitchContext .PitchState   = PITCH_PREPARE;  //状态更新,开始夹球
 
 //		    /*运球*/
 //		u->motor_target .go_spin = u->DribbleContext.DribbleConfig .go2_init_angle ;
@@ -291,7 +239,7 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
 		      u->PitchContext .PitchState=PITCH_START;//置标志位用于启动投篮
 				}
 
-			 Pitch_Process(u,out,c);
+//			 Pitch_Process(u,out,c);
 
 				 break ;
 		 
@@ -303,13 +251,13 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
 				
 				u->DribbleContext .DribbleState =STATE_GRAB_BALL;
 			}
-			 Dribble_Process(u,out);
+//			 Dribble_Process(u,out);
 
 
 		}break ;
 		case Dribbl_transfer:
 			
-        Dribbl_transfer_a(u,out);
+//        Dribbl_transfer_a(u,out);
 			}
 			break;
 						
@@ -338,184 +286,184 @@ return 0;
 
 
 
-int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c)
-{
-	static fp32 go1_position,M2006_Screw_position ;
-	static int is_initialize=1;
-	if(is_initialize)
-	{
-		go1_position=u->PitchContext .PitchConfig .go1_init_position ;
-		M2006_Screw_position=u->PitchContext .PitchConfig .m2006_Screw_init;
-		is_initialize=0;
-	}
+//int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c)
+//{
+//	static fp32 go1_position,M2006_Screw_position ;
+//	static int is_initialize=1;
+//	if(is_initialize)
+//	{
+//		go1_position=u->PitchContext .PitchConfig .go1_init_position ;
+//		M2006_Screw_position=u->PitchContext .PitchConfig .m2006_Screw_init;
+//		is_initialize=0;
+//	}
 
-	switch(u->PitchContext .PitchState){
-	
-		case PITCH_START:
-			u->motor_target .go_shoot = u->PitchContext .PitchConfig .go1_release_threshold;
-		
-//		if(u->motorfeedback .GO_motor_info[0]->Pos < -32.40){ //检测go位置到达最上面，这里的检测条件可以更改
-//			u->motor_target .M2006_angle = u->PitchContext .PitchConfig .m2006_trigger_angle ;//设置2006角度，0
-		
-				  u->PitchContext .PitchState=PITCH_PULL_TRIGGER;
-//}//更改标志位
-
-			break ;
-		case PITCH_PULL_TRIGGER:
-			
-		
-			if(u->motorfeedback .M2006 .total_angle >-1)    //当2006的总角度小于1，可以认为已经勾上,误差为1
-			{
-				u->motor_target .go_shoot = go1_position;//go下拉
-		    u->motor_target .M3508_angle = M2006_Screw_position;//丝杠上的2006
-				go1_position = go1_position + c->Vx ;
-		    M2006_Screw_position=M2006_Screw_position+ c->Vy;
-		
-				
-			}
-		
-			break ;
-
-		
-	
-	
-	}
-		
-return 0;
-	
-}
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
-{
-    
-    switch (u->DribbleContext.DribbleState) {
-    case STATE_GRAB_BALL://开始
-			
-			RELAY1_TOGGLE (0);//夹球
-		
-			u->motor_target.M3508_angle =u->DribbleContext .DribbleConfig .m3508_high_angle;//3508升起
-			u->motor_target.go_spin     =u->DribbleContext .DribbleConfig .go2_flip_angle;
-
-		
-		if((u->motorfeedback .M3508 .total_angle >400)) {
-				RELAY2_TOGGLE (1); 
-		
-
-//		if(((u->motorfeedback.GO_motor_info[1]->Pos ) <-1.1)&&(u->motorfeedback.M3508 .total_angle >1190)){
-//			
-//				u->DribbleContext .DribbleState  = STATE_RELEASE_BALL;  //当go2到标准位置，标志位改变		
-//			}
-		
-		}
-      break;
-
-
-    case STATE_RELEASE_BALL:
-				RELAY1_TOGGLE (1);//松球
-
-		
-		
-		
-//		if((u->motorfeedback.GO_motor_info[1]->Pos ) <-1.3){
-//				u->motor_target.go_spin     =-40; //恢复go2位置	
-//      	u->DribbleContext .DribbleState  = STATE_CATCH_PREP;  //当go2到标准位置，标志位改变		
-
-//			}
-
-        break;
-
-    case STATE_CATCH_PREP:
-//			if((u->motorfeedback.GO_motor_info[1]->Pos )> -0.4){
-//				u->motor_target.M3508_angle =0 ;  //当go2到初始位置，3508降       
+//	switch(u->PitchContext .PitchState){
+//	
+//		case PITCH_START:
+//			u->motor_target .go_shoot = u->PitchContext .PitchConfig .go1_release_threshold;
 //		
-//			  RELAY2_TOGGLE (0);//接球
-//			}
-			if(u->motorfeedback .M3508 .total_angle <51.0f){
-				      	RELAY1_TOGGLE (0);//夹球，0夹1开		
+////		if(u->motorfeedback .GO_motor_info[0]->Pos < -32.40){ //检测go位置到达最上面，这里的检测条件可以更改
+////			u->motor_target .M2006_angle = u->PitchContext .PitchConfig .m2006_trigger_angle ;//设置2006角度，0
+//		
+//				  u->PitchContext .PitchState=PITCH_PULL_TRIGGER;
+////}//更改标志位
 
-			  u->DribbleContext .DribbleState  = STATE_TRANSFER;
-			}
-			
-        break;
-			
-
-		
-			
-		break ;
-
-
-
-    default:
-        break;
-    }
-
-    return 0;
-}
-
-void Dribbl_transfer_a(UP_t *u, CAN_Output_t *out)
-{
-    switch (u->DribbleContext.DribbleState) {
-			
-			
-			
-			case STATE_TRANSFER:
-			if((u->motorfeedback.M3508 .total_angle <52.0f )) //满足这个状态时认为go和3508到达初始位置，再夹上球
-			{
-				
-        u->motor_target .go_spin =u->DribbleContext .DribbleConfig .go2_release_threshold ;			
-			}
-			
-//			if(u->motorfeedback .GO_motor_info [1]->Pos < -4.9) 
+//			break ;
+//		case PITCH_PULL_TRIGGER:
+//			
+//		
+//			if(u->motorfeedback .M2006 .total_angle >-1)    //当2006的总角度小于1，可以认为已经勾上,误差为1
 //			{
-//	      	RELAY1_TOGGLE (1);//夹球，0夹1开		
+//				u->motor_target .go_shoot = go1_position;//go下拉
+//		    u->motor_target .M3508_angle = M2006_Screw_position;//丝杠上的2006
+//				go1_position = go1_position + c->Vx ;
+//		    M2006_Screw_position=M2006_Screw_position+ c->Vy;
+//		
 //				
-//				if(u->motorfeedback .GO_motor_info [1]->Pos > -4.8)
-//				{
-//	      	u->DribbleContext .DribbleState  = STATE_CATCH_DONE;  //当go2到标准位置，标志位改变		
-//				}
 //			}
-		
-			
-		break ;
+//		
+//			break ;
 
-    case STATE_CATCH_DONE:
-			
-		    RELAY1_TOGGLE (1);//夹球，0夹1开
-      	RELAY2_TOGGLE (0);//夹球，0接1收
+//		
+//	
+//	
+//	}
+//		
+//return 0;
+//	
+//}
 
-		    	u->motor_target.go_spin=u->DribbleContext .DribbleConfig.go2_init_angle ;
 
-        break;
 
-        break;
 
-		}	
-}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+//int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
+//{
+//    
+//    switch (u->DribbleContext.DribbleState) {
+//    case STATE_GRAB_BALL://开始
+//			
+//			RELAY1_TOGGLE (0);//夹球
+//		
+//			u->motor_target.M3508_angle =u->DribbleContext .DribbleConfig .m3508_high_angle;//3508升起
+//			u->motor_target.go_spin     =u->DribbleContext .DribbleConfig .go2_flip_angle;
+
+//		
+//		if((u->motorfeedback .M3508 .total_angle >400)) {
+//				RELAY2_TOGGLE (1); 
+//		
+
+////		if(((u->motorfeedback.GO_motor_info[1]->Pos ) <-1.1)&&(u->motorfeedback.M3508 .total_angle >1190)){
+////			
+////				u->DribbleContext .DribbleState  = STATE_RELEASE_BALL;  //当go2到标准位置，标志位改变		
+////			}
+//		
+//		}
+//      break;
+
+
+//    case STATE_RELEASE_BALL:
+//				RELAY1_TOGGLE (1);//松球
+
+//		
+//		
+//		
+////		if((u->motorfeedback.GO_motor_info[1]->Pos ) <-1.3){
+////				u->motor_target.go_spin     =-40; //恢复go2位置	
+////      	u->DribbleContext .DribbleState  = STATE_CATCH_PREP;  //当go2到标准位置，标志位改变		
+
+////			}
+
+//        break;
+
+//    case STATE_CATCH_PREP:
+////			if((u->motorfeedback.GO_motor_info[1]->Pos )> -0.4){
+////				u->motor_target.M3508_angle =0 ;  //当go2到初始位置，3508降       
+////		
+////			  RELAY2_TOGGLE (0);//接球
+////			}
+//			if(u->motorfeedback .M3508 .total_angle <51.0f){
+//				      	RELAY1_TOGGLE (0);//夹球，0夹1开		
+
+//			  u->DribbleContext .DribbleState  = STATE_TRANSFER;
+//			}
+//			
+//        break;
+//			
+
+//		
+//			
+//		break ;
+
+
+
+//    default:
+//        break;
+//    }
+
+//    return 0;
+//}
+
+//void Dribbl_transfer_a(UP_t *u, CAN_Output_t *out)
+//{
+//    switch (u->DribbleContext.DribbleState) {
+//			
+//			
+//			
+//			case STATE_TRANSFER:
+//			if((u->motorfeedback.M3508 .total_angle <52.0f )) //满足这个状态时认为go和3508到达初始位置，再夹上球
+//			{
+//				
+//        u->motor_target .go_spin =u->DribbleContext .DribbleConfig .go2_release_threshold ;			
+//			}
+//			
+////			if(u->motorfeedback .GO_motor_info [1]->Pos < -4.9) 
+////			{
+////	      	RELAY1_TOGGLE (1);//夹球，0夹1开		
+////				
+////				if(u->motorfeedback .GO_motor_info [1]->Pos > -4.8)
+////				{
+////	      	u->DribbleContext .DribbleState  = STATE_CATCH_DONE;  //当go2到标准位置，标志位改变		
+////				}
+////			}
+//		
+//			
+//		break ;
+
+//    case STATE_CATCH_DONE:
+//			
+//		    RELAY1_TOGGLE (1);//夹球，0夹1开
+//      	RELAY2_TOGGLE (0);//夹球，0接1收
+
+//		    	u->motor_target.go_spin=u->DribbleContext .DribbleConfig.go2_init_angle ;
+
+//        break;
+
+//        break;
+
+//		}	
+//}
diff --git a/User/Module/up.h b/User/Module/up.h
index 70be9d9..cc81c4f 100644
--- a/User/Module/up.h
+++ b/User/Module/up.h
@@ -91,7 +91,6 @@ typedef struct {
     fp32 go2_init_angle;       // GO2初始角度
     fp32 go2_flip_angle;       // GO2翻转角度
     fp32 flip_timing;       // 翻转触发时机
-//	  fp32 
     fp32 go2_release_threshold;    // 释放球
 } DribbleConfig_t;
 
@@ -112,47 +111,18 @@ typedef struct {
  AHRS_Eulr_t imu_eulr;//解算后存放欧拉角（弧度制）
 	
 }UP_Imu_t;
-	
-/**
-	*@input[in]		rev 暂时不知道干啥用的，github为回答issue
-	*@input[in]		T	力矩，单位N·m
-	*@input[in]		Pos 目标位置，单位rad
-	*@input[in]		W 目标速度，单位rad/s
-	*@input[in]		K_P 位置环环kp
-	*@input[in]		K_W 速度环kp
-	*@note				控制公式 ： t = T + （设定位置 - 实际位置）*K_P + （设定速度 - 实际速度）*K_W
-	*/
-typedef struct {
-                       
-	int rev;         //暂时不知何用
-	float T;        
-	float W;
-	float K_P;
-	float K_W;
-	
-}GO_param_t;
-/*角度环控制电机类型*/
-typedef enum {
-    M2006 = 1,
-    M3508	
-} MotorType_t;
-
-
-
+	 
 typedef struct
 {	
 
 	pid_param_t VESC_5065_M1_param;
 	pid_param_t VESC_5065_M2_param;
 	
-	pid_param_t UP_GM6020_speed_param;
-	pid_param_t UP_GM6020_angle_param;
+	pid_param_t Shoot_M2006_speed_param;	
+	pid_param_t Shoot_M2006_angle_param;	
 	
-	pid_param_t M2006_speed_param;	
-	pid_param_t M2006_angle_param;	
-	
-	pid_param_t M3508_speed_param;	
-	pid_param_t M3508_angle_param;	
+	pid_param_t Pitch_M2006_speed_param;	
+	pid_param_t Pitch_M2006_angle_param;	
 	
 	DribbleConfig_t DribbleConfig_Config;
 	PitchConfig_t  PitchConfig_Config;
@@ -161,88 +131,69 @@ typedef struct
 }UP_Param_t;
 
 typedef struct
-{		
-        MotorType_t motor;
-	
-        float orig_angle;     
-        float last_angle;     
-        int32_t round_cnt;    
-        uint16_t init_cnt;   
-				float total_angle;
-	
-}motor_angle_data;
+{	
+	  fp32 ecd;
+	  fp32 rpm;
+		 
+    fp32 orig_angle;     
+    fp32 last_angle;     
+    int32_t round_cnt;    
+    uint16_t init_cnt;   
+	  fp32 total_angle;	
+}DJmotor_feedback_t;
 
 
 typedef struct{
 	
    uint8_t up_task_run; 
    const	UP_Param_t *param;  
-	
-	 int state;
-	
-	
-	
 		/*运球过程*/
   DribbleContext_t DribbleContext;
     /*投篮过程*/
 	PitchContext_t PitchContext;
 	
 	
-	
-	
 	 UP_Imu_t pos088; 
 	  
 	 CMD_t *cmd;	
 
 	 struct{					
-	  fp32 rotor_pit6020ecd;
-	  fp32 rotor_pit6020rpm;
 
 	  fp32 VESC_5065_M1_rpm;
 	  fp32 VESC_5065_M2_rpm;
 
-    motor_angle_data M2006;
-		motor_angle_data M3508;
-		 
-		const GO_MotorData_t *go_data;//存放go电机数据
+	  GO_MotorData_t *go_data;//存放go电机数据
 
-	  fp32 M3508_angle[4];
-	  fp32 M3508_rpm  [4];
+    DJmotor_feedback_t DJmotor_feedback[4];
 		 
-		 int flag;
 		
 	}motorfeedback;
  
 
 	
 	 struct{
-					
-	  fp32 rotor_pit6020angle;
 
 	  fp32 VESC_5065_M1_rpm;
 	  fp32 VESC_5065_M2_rpm;
 
-	  fp32 M2006_angle;
-		fp32 M3508_angle;
+	  fp32 Shoot_M2006_angle;
+		fp32 Pitch_M2006_angle;
 		 
 		fp32 go_shoot;
-		fp32 go_spin;
 
 	}motor_target;
   
 	 struct{
 		 
-		 pid_type_def GM6020_speed;
-		 pid_type_def GM6020_angle;
 		 
 		 pid_type_def VESC_5065_M1;
 		 pid_type_def VESC_5065_M2;
 		 
-		 pid_type_def M2006_angle;
-		 pid_type_def M2006_speed;
+		 pid_type_def Shoot_M2006angle;
+		 pid_type_def Shoot_M2006speed;
 		 
-		 pid_type_def M3508_angle;
-		 pid_type_def M3508_speed;
+		 pid_type_def Pitch_M2006_angle;
+		 pid_type_def Pitch_M2006_speed;
 		 
 	}pid;
  	   GO_MotorCmd_t go_cmd;
@@ -253,8 +204,7 @@ typedef struct{
 		/*经PID计算后的实际发送给电机的实时输出值*/
     struct 
 	{
-		fp32 final_3508out[4];
-		fp32 final_pitchout;
+		fp32 DJfinal_out[4];
 		fp32 final_VESC_5065_M1out;
 		fp32 final_VESC_5065_M2out;
 		
@@ -271,20 +221,13 @@ typedef struct{
 
 
 int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq);
-
 int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *c) ;
-int8_t GM6020_control(UP_t *u,fp32 angle);
 int8_t VESC_M5065_Control(UP_t *u,fp32 speed);
 int8_t GO_SendData(UP_t *u,float pos,float limit);
-int8_t UP_angle_control(UP_t *u, fp32 target_angle,MotorType_t motor);
 int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c);
 int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out);
-int8_t UP_M2006_angle(UP_t *u,fp32 target_angle);
-int8_t UP_M3508_speed(UP_t *u,fp32 speed);
-int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c);
+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 Dribble_Process(UP_t *u, CAN_Output_t *out);
-
-void Dribbl_transfer_a(UP_t *u, CAN_Output_t *out);
 
 #endif
diff --git a/User/device/GO_M8010_6_Driver.c b/User/device/GO_M8010_6_Driver.c
index 4bff220..7a16ce6 100644
--- a/User/device/GO_M8010_6_Driver.c
+++ b/User/device/GO_M8010_6_Driver.c
@@ -15,7 +15,6 @@
 		else if ((_IN) >= (_MAX)) \
 			(_IN) = (_MAX);       \
 	}
-GO_MotorCmd_t cmd_gogogo ;
 RIS_ControlData_t motor_send_data;
 	
 	
@@ -102,12 +101,7 @@ void USART6_RxCompleteCallback(void )
 	  memcpy(&data.motor_recv_data, rx_buffer, sizeof(data.motor_recv_data)); // Copy received data
     extract_data(&data); 
 }
-const GO_MotorData_t *get_GO_measure_point(void)
+GO_MotorData_t *get_GO_measure_point(void)
 {
     return &data;
 }
-void gogogo(void )
-{
-
-	
-}
\ No newline at end of file
diff --git a/User/device/GO_M8010_6_Driver.h b/User/device/GO_M8010_6_Driver.h
index 32de5f3..99cc0cc 100644
--- a/User/device/GO_M8010_6_Driver.h
+++ b/User/device/GO_M8010_6_Driver.h
@@ -107,10 +107,9 @@ typedef struct
 
 void modify_data(GO_MotorCmd_t *motor_s);
 void extract_data(GO_MotorData_t *motor_r);
-const GO_MotorData_t *get_GO_measure_point(void);
+GO_MotorData_t *get_GO_measure_point(void);
 void GO_M8010_send_data( GO_MotorCmd_t *cmd);
 void USART6_RxCompleteCallback(void );
-void gogogo(void );
 
 
 #endif /*__GO_M8010_6_H */
diff --git a/User/task/up_task.c b/User/task/up_task.c
index 74e1a88..1f96fc2 100644
--- a/User/task/up_task.c
+++ b/User/task/up_task.c
@@ -31,7 +31,7 @@ static UP_t UP;
 static CMD_t up_cmd;
 
 #endif
-float  aaa=0;
+float  aaa=1;
 float bbb=0;
 float CCC=0;
 float ddd=0;
@@ -65,11 +65,11 @@ void Task_up(void *argument)
 //			
 //			VESC_M5065_Control(&UP,  20000);
 			
-			
+		    	
 //			
 
-      UP_control(&UP,&UP_CAN_out,&up_cmd);
-			UP.motor_target .go_shoot =aaa;
+//      UP_control(&UP,&UP_CAN_out,&up_cmd);
+			UP.motor_target .Pitch_M2006_angle =aaa;
 //			UP.motor_target .M2006_angle =bbb ;
 
 //			UP.motor_target .M3508_angle =CCC;