diff --git a/MDK-ARM/R2.uvoptx b/MDK-ARM/R2.uvoptx
index 3f23285..b9480f8 100644
--- a/MDK-ARM/R2.uvoptx
+++ b/MDK-ARM/R2.uvoptx
@@ -117,6 +117,26 @@
         <pMon>BIN\CMSIS_AGDI.dll</pMon>
       </DebugOpt>
       <TargetDriverDllRegistry>
+        <SetRegEntry>
+          <Number>0</Number>
+          <Key>ARMRTXEVENTFLAGS</Key>
+          <Name>-L70 -Z18 -C0 -M0 -T1</Name>
+        </SetRegEntry>
+        <SetRegEntry>
+          <Number>0</Number>
+          <Key>DLGTARM</Key>
+          <Name>(1010=-1,-1,-1,-1,0)(1007=-1,-1,-1,-1,0)(1008=-1,-1,-1,-1,0)(1009=-1,-1,-1,-1,0)(1012=-1,-1,-1,-1,0)</Name>
+        </SetRegEntry>
+        <SetRegEntry>
+          <Number>0</Number>
+          <Key>ARMDBGFLAGS</Key>
+          <Name></Name>
+        </SetRegEntry>
+        <SetRegEntry>
+          <Number>0</Number>
+          <Key>DLGUARM</Key>
+          <Name></Name>
+        </SetRegEntry>
         <SetRegEntry>
           <Number>0</Number>
           <Key>CMSIS_AGDI</Key>
@@ -129,18 +149,35 @@
         </SetRegEntry>
       </TargetDriverDllRegistry>
       <Breakpoint/>
+      <WatchWindow1>
+        <Ww>
+          <count>0</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>chassis</ItemText>
+        </Ww>
+        <Ww>
+          <count>1</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>ctrl</ItemText>
+        </Ww>
+        <Ww>
+          <count>2</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>dr16,0x0A</ItemText>
+        </Ww>
+      </WatchWindow1>
       <Tracepoint>
         <THDelay>0</THDelay>
       </Tracepoint>
       <DebugFlag>
         <trace>0</trace>
-        <periodic>0</periodic>
-        <aLwin>0</aLwin>
+        <periodic>1</periodic>
+        <aLwin>1</aLwin>
         <aCover>0</aCover>
         <aSer1>0</aSer1>
         <aSer2>0</aSer2>
         <aPa>0</aPa>
-        <viewmode>0</viewmode>
+        <viewmode>1</viewmode>
         <vrSel>0</vrSel>
         <aSym>0</aSym>
         <aTbox>0</aTbox>
@@ -775,7 +812,7 @@
 
   <Group>
     <GroupName>User/Module</GroupName>
-    <tvExp>0</tvExp>
+    <tvExp>1</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
@@ -803,17 +840,29 @@
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
+    <File>
+      <GroupNumber>6</GroupNumber>
+      <FileNumber>49</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>..\User\Module\up.c</PathWithFileName>
+      <FilenameWithoutPath>up.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
   </Group>
 
   <Group>
     <GroupName>User/bsp</GroupName>
-    <tvExp>0</tvExp>
+    <tvExp>1</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>49</FileNumber>
+      <FileNumber>50</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -825,7 +874,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>50</FileNumber>
+      <FileNumber>51</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -837,7 +886,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>51</FileNumber>
+      <FileNumber>52</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -849,7 +898,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>52</FileNumber>
+      <FileNumber>53</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -861,7 +910,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>53</FileNumber>
+      <FileNumber>54</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -873,7 +922,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>54</FileNumber>
+      <FileNumber>55</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -885,7 +934,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>55</FileNumber>
+      <FileNumber>56</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -897,7 +946,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>56</FileNumber>
+      <FileNumber>57</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -909,7 +958,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>57</FileNumber>
+      <FileNumber>58</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -921,7 +970,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>58</FileNumber>
+      <FileNumber>59</FileNumber>
       <FileType>5</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -933,7 +982,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>59</FileNumber>
+      <FileNumber>60</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -945,7 +994,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>60</FileNumber>
+      <FileNumber>61</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -957,7 +1006,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>61</FileNumber>
+      <FileNumber>62</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -971,13 +1020,13 @@
 
   <Group>
     <GroupName>User/Task</GroupName>
-    <tvExp>0</tvExp>
+    <tvExp>1</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>62</FileNumber>
+      <FileNumber>63</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -989,7 +1038,7 @@
     </File>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>63</FileNumber>
+      <FileNumber>64</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1001,7 +1050,7 @@
     </File>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>64</FileNumber>
+      <FileNumber>65</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1013,7 +1062,7 @@
     </File>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>65</FileNumber>
+      <FileNumber>66</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1025,7 +1074,7 @@
     </File>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>66</FileNumber>
+      <FileNumber>67</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1037,7 +1086,7 @@
     </File>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>67</FileNumber>
+      <FileNumber>68</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1049,7 +1098,7 @@
     </File>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>68</FileNumber>
+      <FileNumber>69</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1061,7 +1110,7 @@
     </File>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>69</FileNumber>
+      <FileNumber>70</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1073,7 +1122,7 @@
     </File>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>70</FileNumber>
+      <FileNumber>71</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1085,7 +1134,7 @@
     </File>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>71</FileNumber>
+      <FileNumber>72</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1095,17 +1144,29 @@
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
+    <File>
+      <GroupNumber>8</GroupNumber>
+      <FileNumber>73</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>..\User\task\up_task.c</PathWithFileName>
+      <FilenameWithoutPath>up_task.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
   </Group>
 
   <Group>
     <GroupName>User/Algorithm</GroupName>
-    <tvExp>0</tvExp>
+    <tvExp>1</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>72</FileNumber>
+      <FileNumber>74</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1117,7 +1178,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>73</FileNumber>
+      <FileNumber>75</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1129,7 +1190,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>74</FileNumber>
+      <FileNumber>76</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1141,7 +1202,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>75</FileNumber>
+      <FileNumber>77</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1153,7 +1214,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>76</FileNumber>
+      <FileNumber>78</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1165,7 +1226,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>77</FileNumber>
+      <FileNumber>79</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1179,13 +1240,13 @@
 
   <Group>
     <GroupName>User/Device</GroupName>
-    <tvExp>0</tvExp>
+    <tvExp>1</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>78</FileNumber>
+      <FileNumber>80</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1197,7 +1258,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>79</FileNumber>
+      <FileNumber>81</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1209,7 +1270,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>80</FileNumber>
+      <FileNumber>82</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1221,7 +1282,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>81</FileNumber>
+      <FileNumber>83</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1233,7 +1294,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>82</FileNumber>
+      <FileNumber>84</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1245,7 +1306,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>83</FileNumber>
+      <FileNumber>85</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1257,7 +1318,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>84</FileNumber>
+      <FileNumber>86</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1267,6 +1328,18 @@
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
+    <File>
+      <GroupNumber>10</GroupNumber>
+      <FileNumber>87</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>..\User\device\GO_M8010_6_Driver.c</PathWithFileName>
+      <FilenameWithoutPath>GO_M8010_6_Driver.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
   </Group>
 
   <Group>
@@ -1277,7 +1350,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>11</GroupNumber>
-      <FileNumber>85</FileNumber>
+      <FileNumber>88</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1289,7 +1362,7 @@
     </File>
     <File>
       <GroupNumber>11</GroupNumber>
-      <FileNumber>86</FileNumber>
+      <FileNumber>89</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1301,7 +1374,7 @@
     </File>
     <File>
       <GroupNumber>11</GroupNumber>
-      <FileNumber>87</FileNumber>
+      <FileNumber>90</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1321,7 +1394,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>12</GroupNumber>
-      <FileNumber>88</FileNumber>
+      <FileNumber>91</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1341,7 +1414,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>13</GroupNumber>
-      <FileNumber>89</FileNumber>
+      <FileNumber>92</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1353,7 +1426,7 @@
     </File>
     <File>
       <GroupNumber>13</GroupNumber>
-      <FileNumber>90</FileNumber>
+      <FileNumber>93</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1365,7 +1438,7 @@
     </File>
     <File>
       <GroupNumber>13</GroupNumber>
-      <FileNumber>91</FileNumber>
+      <FileNumber>94</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1377,7 +1450,7 @@
     </File>
     <File>
       <GroupNumber>13</GroupNumber>
-      <FileNumber>92</FileNumber>
+      <FileNumber>95</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
diff --git a/MDK-ARM/R2.uvprojx b/MDK-ARM/R2.uvprojx
index a04509f..f9d4bc2 100644
--- a/MDK-ARM/R2.uvprojx
+++ b/MDK-ARM/R2.uvprojx
@@ -1108,6 +1108,11 @@
               <FileType>1</FileType>
               <FilePath>..\User\Module\config.c</FilePath>
             </File>
+            <File>
+              <FileName>up.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\Module\up.c</FilePath>
+            </File>
           </Files>
         </Group>
         <Group>
@@ -1233,6 +1238,11 @@
               <FileType>1</FileType>
               <FilePath>..\User\task\r12ds_task.c</FilePath>
             </File>
+            <File>
+              <FileName>up_task.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\task\up_task.c</FilePath>
+            </File>
           </Files>
         </Group>
         <Group>
@@ -1308,6 +1318,11 @@
               <FileType>1</FileType>
               <FilePath>..\User\device\r12ds.c</FilePath>
             </File>
+            <File>
+              <FileName>GO_M8010_6_Driver.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\device\GO_M8010_6_Driver.c</FilePath>
+            </File>
           </Files>
         </Group>
         <Group>
diff --git a/MDK-ARM/R2/R2.axf b/MDK-ARM/R2/R2.axf
deleted file mode 100644
index 67ed57f..0000000
Binary files a/MDK-ARM/R2/R2.axf and /dev/null differ
diff --git a/User/Module/Chassis.c b/User/Module/Chassis.c
index 00e291a..1ed3fe0 100644
--- a/User/Module/Chassis.c
+++ b/User/Module/Chassis.c
@@ -21,10 +21,10 @@
 
 static int8_t Chassis_SetCtrl(Chassis_t *c,CMD_t *ctrl){
 	if (c == NULL) return CHASSIS_ERR_NULL; /*主结构体不能为空 */
-  if (ctrl->C_cmd.type== c->ctrl && ctrl->C_cmd.mode== c->mode) return CHASSIS_OK; /*模式未改变直接返回*/
+  if (ctrl->CMD_CtrlType== c->ctrl && ctrl->CMD_mode == c->mode) return CHASSIS_OK; /*模式未改变直接返回*/
   //此处源代码处做了pid的reset 待添加
-   c->ctrl =ctrl->C_cmd.type;
-   c->mode =ctrl->C_cmd.mode;
+   c->ctrl =ctrl->CMD_CtrlType ;
+   c->mode =ctrl->CMD_mode ;
    
    return CHASSIS_OK;
 }    //设置控制模式
@@ -42,13 +42,13 @@ int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can) {
  }
  
  
-   c->motorfeedback.rotor_pit6020angle = can->motor.pit6020.as_array[0].rotor_angle;
+   c->motorfeedback.rotor_pit6020angle = can->motor.pit6020.as_array[0].rotor_ecd;
    c->motorfeedback.rotor_pit6020rpm   = can->motor.pit6020.as_array[0].rotor_speed;
  
-   c->motorfeedback.rotor_gimbal_yawangle = can->motor.chassis6020.as_array[0].rotor_angle;
+   c->motorfeedback.rotor_gimbal_yawangle = can->motor.chassis6020.as_array[0].rotor_ecd;
    c->motorfeedback.rotor_gimbal_yawrpm = can->motor.chassis6020.as_array[0].rotor_speed;
  
-   c->motorfeedback.rotor_gimbal_pitchangle = can->motor.chassis6020.as_array[1].rotor_angle;
+   c->motorfeedback.rotor_gimbal_pitchangle = can->motor.chassis6020.as_array[1].rotor_ecd;
    c->motorfeedback.rotor_gimbal_pitchrpm = can->motor.chassis6020.as_array[1].rotor_speed;
  
 // c->sick_dis[0] = can->sickfed.raw_dis[0];
@@ -126,7 +126,7 @@ int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out)
 	  
 	switch (c->ctrl)
 	{
-	case RC: //手动控制
+	case RCcontrol: //手动控制
 		
 /*     
 	在cmd里对数据进行处理 包括方向和油门	  
@@ -137,12 +137,7 @@ int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out)
 	
 
 	  break;
-	case UP_CONTROL://上层在控制时，底盘速度为0
-			c->move_vec.Vw = 0;
-		  c->move_vec.Vx = 0;
-		  c->move_vec.Vy = 0;
-	 break;
-	
+
   case AUTO : //在自动模式下
 		
 		switch(c->mode ){	 
@@ -150,9 +145,9 @@ int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out)
 
 	case AUTO_NAVI:  //自动雷达
 //		  //这套是全向轮的方向,一定要注意这里的xy方向
-	    c->move_vec.Vw =ctrl->C_navi.wz*1000 ;
-	    c->move_vec.Vy =-ctrl->C_navi.vy*1000 ;
-	    c->move_vec.Vx =-ctrl->C_navi.vx*1000 ;
+	    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 ;
 //	    c->move_vec.Vw =LowPassFilter2p_Apply(&(c->filled[1]),c->move_vec.Vw);
 //	    c->move_vec.Vy =LowPassFilter2p_Apply(&(c->filled[2]),c->move_vec.Vy);
 //	    c->move_vec.Vx =LowPassFilter2p_Apply(&(c->filled[3]),c->move_vec.Vx);
@@ -182,16 +177,16 @@ int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out)
 	    
 	    c->move_vec.Vx =ctrl->Vx*6000 ;
 	    c->move_vec.Vy =ctrl->Vy *6000;	
-	    c->move_vec .Vw = -ctrl->C_pick .posx;
+	    c->move_vec .Vw = -ctrl->cmd_pick  .posx;
 	
 	
 	
-      if(abs_value(ctrl ->C_pick .posx )>20)
+      if(abs_value(ctrl ->cmd_pick .posx )>20)
 			{
 			  c->move_vec.Vw =PID_calc(&(c->pid.chassis_PICKWzPID_HIGN),(c->move_vec .Vw) ,0);
 			
 			}
-			else if(abs_value(ctrl ->C_pick .posx )<0.1)
+			else if(abs_value(ctrl ->cmd_pick .posx )<0.1)
 			{
 				c->move_vec.Vw =0;
 			}
@@ -202,7 +197,7 @@ int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out)
 
 			
        	c->vofa_send[0]=c->move_vec.Vw;
-	      c->vofa_send[1]=-ctrl->C_pick .posx;
+	      c->vofa_send[1]=-ctrl->cmd_pick .posx;
 		
 
 
@@ -229,7 +224,7 @@ int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out)
 	{
 		 c->final_out.final_3508out[i] = PID_calc(&(c->pid.chassis_3508VecPID[i]),c->motorfeedback.rotor_rpm3508[i],c->hopemotorout.OmniSpeedOut[i]);
 	 
-		 out->motor3508.as_array[i] = c->final_out.final_3508out[i];
+		 out->motor_CHASSIS3508 .as_array[i] = c->final_out.final_3508out[i];
 	}
 	
 
diff --git a/User/Module/Chassis.h b/User/Module/Chassis.h
index d5131e3..138cde6 100644
--- a/User/Module/Chassis.h
+++ b/User/Module/Chassis.h
@@ -118,8 +118,8 @@ typedef struct{
    const	Chassis_Param_t *param;  //一些固定的参数
 
     ChassisImu_t pos088; //088的实时姿态
-    CMD_Chassis_CtrlType_e ctrl;            
-    CMD_Chassis_mode_e mode;                 
+    CMD_CtrlType_t ctrl;            
+    CMD_mode_t mode;                 
 
     ChassisMove_Vec move_vec;         //由控制任务决定
 
diff --git a/User/Module/config.c b/User/Module/config.c
index fa243e0..b4acbaf 100644
--- a/User/Module/config.c
+++ b/User/Module/config.c
@@ -8,19 +8,84 @@
 #define DEG_TO_RAD(x) ((x) * (3.141592653 / 180.0)) //角度转弧度
 
 
-
-
 #ifdef DEBUG
 
-ConfigParam_t param_chassis ={
+ConfigParam_t param ={
 
 #else
 	
-static const ConfigParam_t param_chassis ={
+static const ConfigParam_t param ={
+
 
 #endif
+	.up={
+		
+		
+		/*上层pid参数*/
+.Shoot_M2006_angle_param = {  
+    .p = 25.0f,        
+    .i = 0.0f,         
+    .d = 1.5f,         
+    .i_limit = 1000.0f,
+    .out_limit = 3000.0f,
+},
+.Shoot_M2006_speed_param = {  
+    .p = 5.0f,         
+    .i = 0.3f,         
+    .d = 0.0f,         
+    .i_limit = 2000.0f,
+    .out_limit = 3000.0f,
+},
+.Pitch_M2006_angle_param = {  
+    .p = 25.0f,        
+    .i = 0.0f,         
+    .d = 1.5f,         
+    .i_limit = 1000.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,
+},
 
-   .chassis = {/**/
+
+.go_cmd={
+    .id =0,
+    .mode = 1,
+    .K_P = 1.0f,
+    .K_W = 0.05,
+    .Pos = 0,
+    .W = 0,
+    .T = 0,
+
+},
+
+
+/*上层其他参数*/
+    /*运球*/
+    .DribbleConfig_Config = {
+    .m3508_init_angle = 50,
+    .m3508_high_angle = 1200,
+    .go2_init_angle =  0,
+    .go2_flip_angle = -250,
+    .flip_timing = 200,
+    .go2_release_threshold = -550.0f,
+},
+    /*投球*/
+    .PitchConfig_Config = {
+    .m2006_init_angle =-150,    
+	  .m2006_trigger_angle =0,
+    .go1_init_position = 0,   
+	  .go1_release_threshold =0,
+		.m2006_Screw_init=0	
+},
+	
+	
+	},
+	 .chassis = {/**/
 		
 	 
 	 	 .M3508_param = {
@@ -47,93 +112,31 @@ static const ConfigParam_t param_chassis ={
 		 },
 			
 				 
-//	 	 .M3508_param = {
-//		 .p = 10.0f,
-//		 .i = 0.0f,
-//		 .d = 0.0f,
-//		 .i_limit = 0.0f,
-//		 .out_limit =10000.0f,
-//	 },	
-
-////高速那一套	 
-    .NaviVx_param ={
-			.p = 1.15f,
-			.i = 0.0f,
-			.d = 0.15f,
-			.i_limit = 0.0f,
-			.out_limit =5000.0f,			
-		},
-		 .NaviVy_param ={
-			 .p = 1.15f,
-			.i = 0.0f,
-			.d = 0.15f,
-			.i_limit = 0.0f,
-			.out_limit =5000.0f,	
-		 },
-		 .NaviVw_param ={
-			.p = 1.15f,
-			.i = 0.0f,
-			.d = 0.15f,
-			.i_limit = 0.0f,
-			.out_limit =5000.0f,	 
-		 },
-//  
-
-//低速那一套	 
-//    .NaviVx_param ={
-//			.p = 1.04f,
-//			.i = 0.0f,
-//			.d = 0.15f,
-//			.i_limit = 0.0f,
-//			.out_limit =5000.0f,			
-//		},
-//		 .NaviVy_param ={
-//			 .p = 1.1f,
-//			.i = 0.0f,
-//			.d = 0.15f,
-//			.i_limit = 0.0f,
-//			.out_limit =5000.0f,	
-//		 },
-//		 .NaviVw_param ={
-//			.p = 1.5f,
-//			.i = 0.0f,
-//			.d = 0.15f,
-//			.i_limit = 0.0f,
-//			.out_limit =5000.0f,	 
-//		 },
-		 .Sick_CaliYparam ={
-			 .p =2.5f,
-			 .i =0.001f,
-			 .d =0.1f,
-			 .i_limit =15.0f,
-			 .out_limit =500.0f,		 
-		 },
-		 .Sick_CaliXparam ={
-			 .p =2.5f,
-			 .i =0.001f,
-			 .d =0.1f,
-			 .i_limit =15.0f,
-			 .out_limit =500.0f,	
-      			 
-		 }
-
    },
 	 
-	  
+
    .can = {
-		   .pitch6020 =  BSP_CAN_1,
-       .motor3508 = BSP_CAN_1,  
-		   .chassis6020 = BSP_CAN_2,
+		   .pitch6020 =  BSP_CAN_1, 
+       .motor_CHASSIS3508 = BSP_CAN_1, 
+       .motor_UP3508 =  BSP_CAN_2, 
+      		 
+//		   .chassis6020 = BSP_CAN_1,//禁用
+       .chassis5065 = BSP_CAN_1,
+		 
 		   .sick = BSP_CAN_2,
    },
 
 };
 const ConfigParam_t *Config_ChassisGet(void)
 {
-  return &param_chassis;
+  return &param;
 }
 
-
+///*获取导航地图*/
+//void set_ops_path(ConfigParam_t *config, const point_t *path, int8_t path_num) {
+//    config->ops.path = path;
+//    config->ops.path_num = path_num;
+//}
 /**
  * \brief 从Flash读取配置信息
  *
@@ -142,11 +145,11 @@ const ConfigParam_t *Config_ChassisGet(void)
 void Config_Get(Config_t *cfg) {
   BSP_Flash_ReadBytes(CONFIG_BASE_ADDRESS, (uint8_t *)cfg, sizeof(*cfg));
 //  /* 防止第一次烧写后访问NULL指针 */
-	 cfg->chassis_config = &param_chassis;
-  if (cfg->chassis_config == NULL) cfg->chassis_config = &param_chassis;
+	 cfg->config = &param;
+  if (cfg->config == NULL) cfg->config = &param;
   /* 防止擦除后全为1 */
-  if ((uint32_t)(cfg->chassis_config) == UINT32_MAX)
-    cfg->chassis_config = &param_chassis;
+  if ((uint32_t)(cfg->config) == UINT32_MAX)
+    cfg->config = &param;
 	
 	  if (memcmp(&cfg->cali_088.gyro_offset.x, "\xFF\xFF\xFF\xFF", 4) == 0) {
         cfg->cali_088.gyro_offset.x = 0.0f;
diff --git a/User/Module/config.h b/User/Module/config.h
index 9ba1c14..11df62a 100644
--- a/User/Module/config.h
+++ b/User/Module/config.h
@@ -1,14 +1,16 @@
 #ifndef _CONFIG_H
 #define _CONFIG_H
 
-#include "Chassis.h"
 #include "can_use.h"
 #include "ahrs.h"
 #include "map.h"
-
+#include "up.h"
+#include "chassis.h"
 
 typedef struct{
-Chassis_Param_t chassis; /**/
+UP_Param_t up;
+Chassis_Param_t chassis;
+
 CAN_Params_t can;
 AHRS_Eulr_t mech_zero[4];
 	
@@ -16,7 +18,7 @@ AHRS_Eulr_t mech_zero[4];
 
 
 typedef struct{
-const ConfigParam_t *chassis_config;
+const ConfigParam_t *config;
 
 BMI088_Cali_t cali_088;
 	
diff --git a/User/Module/up.c b/User/Module/up.c
new file mode 100644
index 0000000..43b1812
--- /dev/null
+++ b/User/Module/up.c
@@ -0,0 +1,253 @@
+#include "up.h"
+#include "gpio.h"
+#include "user_math.h"
+#include "bsp_buzzer.h"
+#include "bsp_delay.h"
+
+#define GEAR_RATIO_2006         (36)   // 2006减速比
+#define GEAR_RATIO_3508         (19)
+
+#define CAN_MOTOR_ENC_RES  8191   // 编码器分辨率
+#define MOTOR2006_ECD_TO_ANGLE  (360.0 / 8191.0 / (GEAR_RATIO_2006))   //2006编码值转轴角度
+#define MOTOR3508_ECD_TO_ANGLE  (360.0 / 8191.0 / (GEAR_RATIO_3508))   //3508编码值转轴角度
+
+
+// 定义继电器控制
+#define RELAY1_TOGGLE(state) HAL_GPIO_WritePin(GPIOE, GPIO_PIN_9, (state) ? GPIO_PIN_SET : GPIO_PIN_RESET)
+#define RELAY2_TOGGLE(state) HAL_GPIO_WritePin(GPIOE, GPIO_PIN_11, (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; 	/*初始化参数 */
+
+	/*pid初始化*/
+	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 .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 .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 ));
+	
+	
+	u->go_cmd =u->param ->go_cmd ;
+	
+	// 初始化上层状态机
+	if (!u->DribbleContext .is_initialized) { //检查是否为第一次运行状态机，运球
+		  u->DribbleContext .DribbleConfig = u->param ->DribbleConfig_Config ;//赋值
+      u->DribbleContext .DribbleState  = Dribble_PREPARE;  
+      u->DribbleContext .is_initialized = 1;
+		
+    }
+
+	if (!u->PitchContext  .is_initialized) { 
+		  u->PitchContext .PitchConfig  = u->param ->PitchConfig_Config  ;//赋值
+      u->PitchContext .PitchState   = PITCH_PREPARE;  //状态更新,开始夹球
+      u->PitchContext .is_initialized = 1;
+    }
+
+     BSP_UART_RegisterCallback(BSP_UART_RS485, BSP_UART_RX_CPLT_CB, USART6_RxCompleteCallback);//注册串口回调函数，bsp层
+
+}
+
+/*can，上层状态更新*/
+int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *c) {
+
+	
+
+	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 ;
+
+	
+	for(int i=0;i<4;i++){
+	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;
+	
+	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 - f->last_angle;
+    if (delta > 4096) {
+        f->round_cnt--;           
+    } else if (delta < -4096) {
+        f->round_cnt++;          
+    }
+	    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 VESC_M5065_Control(UP_t *u,fp32 speed)
+{
+	u->motor_target .VESC_5065_M1_rpm =speed;
+	u->motor_target .VESC_5065_M2_rpm =speed;
+	
+	u->final_out .final_VESC_5065_M1out =-u->motor_target .VESC_5065_M1_rpm;
+  u->final_out .final_VESC_5065_M2out =u->motor_target .VESC_5065_M2_rpm;
+	
+	return 0;
+
+}
+
+
+
+
+/*go电机控制*/	 
+
+int8_t GO_SendData(UP_t *u, float pos, float limit)
+{ 
+	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 (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; // 允许位置
+	}
+
+    // 发送数据
+    GO_M8010_send_data(&u->go_cmd);
+
+
+    return 0;
+}
+
+int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
+{
+	//电机控制	，传进can
+  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 ->motor_UP3508.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 ;
+	
+	return 0;
+
+	
+}
+
+
+
+
+
+int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
+{
+	
+	if(u ==NULL)  return 0;
+	static int is_pitch=1;
+    switch (c->CMD_CtrlType  )
+	{
+	case RCcontrol:  //在手动模式下
+		
+   switch (c-> CMD_mode ) 
+			{
+				
+		case Normal :
+         /*投篮*/
+		if(is_pitch){
+		u->motor_target .go_shoot =u->PitchContext .PitchConfig .go1_init_position ;
+		is_pitch=0;	
+		}
+
+//    				
+				
+				break;
+				
+		case Pitch :
+				if (u->PitchContext .PitchState  ==PITCH_PREPARE) //首次启动
+				{
+		      u->PitchContext .PitchState=PITCH_START;//置标志位用于启动投篮
+				}
+
+
+
+				 break ;
+		 
+			
+			
+return 0;
+
+	
+}
+
+}
+	}
+
+
+
+
+
+
+
+
+
diff --git a/User/Module/up.h b/User/Module/up.h
new file mode 100644
index 0000000..cc81c4f
--- /dev/null
+++ b/User/Module/up.h
@@ -0,0 +1,233 @@
+#ifndef UP_H
+#define UP_H
+
+
+#include "struct_typedef.h"
+#include "pid.h"
+#include "bmi088.h"
+#include "user_math.h"
+#include "ahrs.h"
+#include "can_use.h"
+#include "cmd.h"
+#include "filter.h"
+#include "vofa.h"	
+#include "GO_M8010_6_Driver.h"
+#include "bsp_usart.h"
+
+/*
+ 状态机
+
+状态结构体，触发标志位结构体
+
+配置层，-->config.c
+  |
+中间层，-->up.h,统一UP_t
+  |
+运行函数，switch(状态) 运行相应函数
+
+
+
+
+
+
+
+
+
+
+
+*/
+
+
+
+
+/* 投球状态定义 */
+typedef enum {
+    PITCH_PREPARE,          // 准备阶段
+	  PITCH_START,            //启动，拉扳机
+    PITCH_PULL_TRIGGER,  
+    PITCH_LAUNCHING,        // 发射中
+    PITCH_COMPLETE          // 完成
+} PitchState_t;
+
+/* 投球参数配置 */
+typedef struct {
+    fp32 m2006_init_angle;      // M2006初始角度-140
+	  fp32 m2006_trigger_angle;   // M2006触发角度0,用来合并扳机
+    fp32 go1_init_position;     // GO电机触发位置,0,初始位置
+	  fp32 go1_release_threshold; // go上升去合并扳机，扳机位置
+	  fp32 m2006_Screw_init;
+} PitchConfig_t;
+
+/* 投球控制上下文 */
+typedef struct {
+    PitchState_t PitchState;
+	  PitchConfig_t PitchConfig;
+
+    uint8_t is_initialized ;
+} PitchContext_t;
+
+
+
+
+
+
+
+
+/*运球*/
+typedef enum {
+	  Dribble_PREPARE,
+    STATE_GRAB_BALL,        // 夹球升起阶段
+    STATE_RELEASE_BALL,     // 释放球体
+    STATE_CATCH_PREP,       // 接球准备
+    STATE_CATCH_BALL,       // 接球动作
+	  STATE_TRANSFER,         //转移球
+	  STATE_CATCH_DONE        //完成
+} DribbleState_t;
+
+/* 参数配置结构体 */
+typedef struct {
+    fp32 m3508_init_angle;     // 3508初始角度
+    fp32 m3508_high_angle;     // 3508升起角度
+    fp32 go2_init_angle;       // GO2初始角度
+    fp32 go2_flip_angle;       // GO2翻转角度
+    fp32 flip_timing;       // 翻转触发时机
+    fp32 go2_release_threshold;    // 释放球
+} DribbleConfig_t;
+
+/* 状态机上下文 */
+typedef struct {
+    DribbleState_t DribbleState;
+	  DribbleConfig_t DribbleConfig;
+	   
+    uint8_t is_initialized;
+
+} DribbleContext_t;
+
+
+typedef struct {
+
+ BMI088_t bmi088;
+
+ AHRS_Eulr_t imu_eulr;//解算后存放欧拉角（弧度制）
+	
+}UP_Imu_t;
+	 
+typedef struct
+{	
+
+	pid_param_t VESC_5065_M1_param;
+	pid_param_t VESC_5065_M2_param;
+	
+	pid_param_t Shoot_M2006_speed_param;	
+	pid_param_t Shoot_M2006_angle_param;	
+	
+	pid_param_t Pitch_M2006_speed_param;	
+	pid_param_t Pitch_M2006_angle_param;	
+	
+	DribbleConfig_t DribbleConfig_Config;
+	PitchConfig_t  PitchConfig_Config;
+	
+	GO_MotorCmd_t go_cmd;
+}UP_Param_t;
+
+typedef struct
+{	
+	  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;  
+		/*运球过程*/
+  DribbleContext_t DribbleContext;
+    /*投篮过程*/
+	PitchContext_t PitchContext;
+	
+	
+	 UP_Imu_t pos088; 
+	  
+	 CMD_t *cmd;	
+
+	 struct{					
+
+	  fp32 VESC_5065_M1_rpm;
+	  fp32 VESC_5065_M2_rpm;
+
+	  GO_MotorData_t *go_data;//存放go电机数据
+
+    DJmotor_feedback_t DJmotor_feedback[4];
+		 
+		
+	}motorfeedback;
+ 
+
+	
+	 struct{
+
+	  fp32 VESC_5065_M1_rpm;
+	  fp32 VESC_5065_M2_rpm;
+
+	  fp32 Shoot_M2006_angle;
+		fp32 Pitch_M2006_angle;
+		 
+		fp32 go_shoot;
+
+	}motor_target;
+  
+	 struct{
+		 
+		 
+		 pid_type_def VESC_5065_M1;
+		 pid_type_def VESC_5065_M2;
+		 
+		 pid_type_def Shoot_M2006angle;
+		 pid_type_def Shoot_M2006speed;
+		 
+		 pid_type_def Pitch_M2006_angle;
+		 pid_type_def Pitch_M2006_speed;
+		 
+	}pid;
+ 	   GO_MotorCmd_t go_cmd;
+
+		 
+
+	
+		/*经PID计算后的实际发送给电机的实时输出值*/
+    struct 
+	{
+		fp32 DJfinal_out[4];
+		fp32 final_VESC_5065_M1out;
+		fp32 final_VESC_5065_M2out;
+		
+
+	}final_out;
+	
+	 LowPassFilter2p_t filled[6]; /* 输出滤波器滤波器数组 */
+	
+	fp32 vofa_send[8];
+	
+
+	
+}	UP_t;
+
+
+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 VESC_M5065_Control(UP_t *u,fp32 speed);
+int8_t GO_SendData(UP_t *u,float pos,float limit);
+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 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);
+
+
+#endif
diff --git a/User/bsp/bsp_usart.c b/User/bsp/bsp_usart.c
index 369a7d7..4843fac 100644
--- a/User/bsp/bsp_usart.c
+++ b/User/bsp/bsp_usart.c
@@ -9,9 +9,9 @@ static BSP_UART_t UART_Get(UART_HandleTypeDef *huart) {
   if (huart->Instance == USART3)
     return BSP_UART_REMOTE;
   else if (huart->Instance == USART1)
-    return BSP_UART_VOFA;
-  else if (huart->Instance == USART6)
     return BSP_UART_NUC;
+  else if (huart->Instance == USART6)
+    return BSP_UART_RS485;
   /*
   else if (huart->Instance == USARTX)
                   return BSP_UART_XXX;
@@ -94,10 +94,10 @@ UART_HandleTypeDef *BSP_UART_GetHandle(BSP_UART_t uart) {
   switch (uart) {
     case BSP_UART_REMOTE:
       return &huart3;
-    case BSP_UART_VOFA:
-      return &huart1;
-    case BSP_UART_NUC:
+    case BSP_UART_RS485:
       return &huart6;
+    case BSP_UART_NUC:
+      return &huart1;
     /*
     case BSP_UART_XXX:
             return &huartX;
@@ -108,7 +108,6 @@ UART_HandleTypeDef *BSP_UART_GetHandle(BSP_UART_t uart) {
 }
 
 
-
 int8_t BSP_UART_RegisterCallback(BSP_UART_t uart, BSP_UART_Callback_t type,
                                  void (*callback)(void)) {
   if (callback == NULL) return BSP_ERR_NULL;
diff --git a/User/bsp/bsp_usart.h b/User/bsp/bsp_usart.h
index cc8d13f..cef04b6 100644
--- a/User/bsp/bsp_usart.h
+++ b/User/bsp/bsp_usart.h
@@ -12,7 +12,7 @@
 
 typedef enum {
   BSP_UART_REMOTE,
-  BSP_UART_VOFA,
+  BSP_UART_RS485,
   BSP_UART_NUC,
   /* BSP_UART_XXX, */
   BSP_UART_NUM,
diff --git a/User/device/GO_M8010_6_Driver.c b/User/device/GO_M8010_6_Driver.c
new file mode 100644
index 0000000..626c84b
--- /dev/null
+++ b/User/device/GO_M8010_6_Driver.c
@@ -0,0 +1,108 @@
+/**
+	*go控制程序
+	*/
+#include "GO_M8010_6_Driver.h"
+#include "usart.h"
+#include "bsp_usart.h"
+#include <cmsis_os2.h>
+#include "crc16.h"
+#include <string.h>
+
+#define SATURATE(_IN, _MIN, _MAX) \
+	{                             \
+		if ((_IN) <= (_MIN))      \
+			(_IN) = (_MIN);       \
+		else if ((_IN) >= (_MAX)) \
+			(_IN) = (_MAX);       \
+	}
+GO_MotorCmd_t cmd_gogogo ;
+RIS_ControlData_t motor_send_data;
+	
+	
+GO_MotorData_t data = {0}; // 确保 data 定义在此处
+uint8_t rx_buffer[sizeof(data.motor_recv_data)] = {0}; // 确保 rx_buffer 定义在此处
+
+
+
+
+
+
+/// @brief 将发送给电机的浮点参数转换为定点类型参数
+/// @param motor_s 要转换的电机指令结构体
+void modify_data(GO_MotorCmd_t *motor_s)
+{
+	motor_send_data.head[0] = 0xFE;
+	motor_send_data.head[1] = 0xEE;
+
+	SATURATE(motor_s->id, 0, 15);
+	SATURATE(motor_s->mode, 0, 7);
+	SATURATE(motor_s->K_P, 0.0f, 25.599f);
+	SATURATE(motor_s->K_W, 0.0f, 25.599f);
+	SATURATE(motor_s->T, -127.99f, 127.99f);
+	SATURATE(motor_s->W, -804.00f, 804.00f);
+	SATURATE(motor_s->Pos, -411774.0f, 411774.0f);
+
+	motor_send_data.mode.id = motor_s->id;
+	motor_send_data.mode.status = motor_s->mode;
+	motor_send_data.comd.k_pos = motor_s->K_P / 25.6f * 32768.0f;
+	motor_send_data.comd.k_spd = motor_s->K_W / 25.6f * 32768.0f;
+	motor_send_data.comd.pos_des = motor_s->Pos / 6.28318f * 32768.0f;
+	motor_send_data.comd.spd_des = motor_s->W / 6.28318f * 256.0f;
+	motor_send_data.comd.tor_des = motor_s->T * 256.0f;
+	motor_send_data.CRC16 = CRC16_Calc( (uint8_t *)&motor_send_data, sizeof(RIS_ControlData_t) - sizeof(motor_send_data.CRC16),0);
+}
+
+/// @brief 将接收到的定点类型原始数据转换为浮点参数类型
+/// @param motor_r 要转换的电机反馈结构体
+void extract_data(GO_MotorData_t *motor_r)
+{
+	if (motor_r->motor_recv_data.head[0] != 0xFD || motor_r->motor_recv_data.head[1] != 0xEE)
+	{
+		motor_r->correct = 0;
+		return;
+	}
+	motor_r->calc_crc = CRC16_Calc((uint8_t *)&motor_r->motor_recv_data, sizeof(RIS_MotorData_t) - sizeof(motor_r->motor_recv_data.CRC16),0);
+	if (motor_r->motor_recv_data.CRC16 != motor_r->calc_crc)
+	{
+		memset(&motor_r->motor_recv_data, 0, sizeof(RIS_MotorData_t));
+		motor_r->correct = 0;
+		motor_r->bad_msg++;
+		return;
+	}
+	else
+	{
+		motor_r->motor_id = motor_r->motor_recv_data.mode.id;
+		motor_r->mode = motor_r->motor_recv_data.mode.status;
+		motor_r->Temp = motor_r->motor_recv_data.fbk.temp;
+		motor_r->MError = motor_r->motor_recv_data.fbk.MError;
+		motor_r->W = ((float)motor_r->motor_recv_data.fbk.speed / 256.0f) * 6.28318f;
+		motor_r->T = ((float)motor_r->motor_recv_data.fbk.torque) / 256.0f;
+		motor_r->Pos = 6.28318f * ((float)motor_r->motor_recv_data.fbk.pos) / 32768.0f;
+		motor_r->footForce = motor_r->motor_recv_data.fbk.force;
+		motor_r->correct = 1;
+		return;
+	}
+}
+void GO_M8010_send_data(GO_MotorCmd_t *cmd)
+{
+    modify_data(cmd);
+	
+    HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_RS485), (uint8_t *)(&motor_send_data), sizeof(motor_send_data));   
+    osDelay (1);
+	  HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_RS485), rx_buffer, sizeof(rx_buffer));
+
+}
+
+void USART6_RxCompleteCallback(void ) 
+{	
+
+	  UART_HandleTypeDef *huart6 = BSP_UART_GetHandle(BSP_UART_RS485);
+    uint16_t crc = CRC16_Calc(rx_buffer,sizeof(rx_buffer)-2,0x0000);
+
+	  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)
+{
+    return &data;
+}
diff --git a/User/device/GO_M8010_6_Driver.h b/User/device/GO_M8010_6_Driver.h
new file mode 100644
index 0000000..d6b418e
--- /dev/null
+++ b/User/device/GO_M8010_6_Driver.h
@@ -0,0 +1,116 @@
+#ifndef __GO_M8010_6_H
+#define __GO_M8010_6_H
+
+#include "struct_typedef.h"
+#pragma pack(1)
+/**
+ * @brief ���ģʽ������Ϣ
+ *
+ */
+typedef struct
+{
+    uint8_t id : 4;      // ���ID: 0,1...,13,14 15��ʾ�����е���㲥����(��ʱ�޷���)
+    uint8_t status : 3;  // ����ģʽ: 0.���� 1.FOC�ջ� 2.������У׼ 3.����
+    uint8_t reserve : 1; // ����λ
+} RIS_Mode_t;            // ����ģʽ 1Byte
+
+/**
+ * @brief ���״̬������Ϣ
+ *
+ */
+typedef struct
+{
+    int16_t tor_des; // �����ؽ����Ť�� unit: N.m      (q8)
+    int16_t spd_des; // �����ؽ�����ٶ� unit: rad/s    (q8)
+    int32_t pos_des; // �����ؽ����λ�� unit: rad      (q15)
+    int16_t k_pos;   // �����ؽڸն�ϵ�� unit: -1.0-1.0 (q15)
+    int16_t k_spd;   // �����ؽ�����ϵ�� unit: -1.0-1.0 (q15)
+
+} RIS_Comd_t; // ���Ʋ��� 12Byte
+
+/**
+ * @brief ���״̬������Ϣ
+ *
+ */
+typedef struct
+{
+    int16_t torque;      // ʵ�ʹؽ����Ť�� unit: N.m     (q8)
+    int16_t speed;       // ʵ�ʹؽ�����ٶ� unit: rad/s   (q8)
+    int32_t pos;         // ʵ�ʹؽ����λ�� unit: rad     (q15)
+    int8_t temp;         // ����¶�: -128~127��C
+    uint8_t MError : 3;  // ��������ʶ: 0.���� 1.���� 2.���� 3.��ѹ 4.���������� 5-7.����
+    uint16_t force : 12; // �����ѹ���������� 12bit (0-4095)
+    uint8_t none : 1;    // ����λ
+} RIS_Fbk_t;             // ״̬���� 11Byte
+
+/**
+ * @brief �������ݰ���ʽ
+ *
+ */
+typedef struct
+{
+    uint8_t head[2]; // ��ͷ         2Byte
+    RIS_Mode_t mode; // �������ģʽ  1Byte
+    RIS_Comd_t comd; // ����������� 12Byte
+    uint16_t CRC16;  // CRC          2Byte
+
+} RIS_ControlData_t; // ������������     17Byte
+
+/**
+ * @brief ����������ݰ���ʽ
+ *
+ */
+typedef struct
+{
+    uint8_t head[2]; // ��ͷ         2Byte
+    RIS_Mode_t mode; // �������ģʽ  1Byte
+    RIS_Fbk_t fbk;   // ����������� 11Byte
+    uint16_t CRC16;  // CRC          2Byte
+
+} RIS_MotorData_t; // �����������     16Byte
+
+#pragma pack()
+
+/// @brief ���ָ��ṹ��
+typedef struct
+{
+    unsigned short id;   // ���ID��15�����㲥���ݰ�
+    unsigned short mode; // 0:���� 1:FOC���� 2:����궨
+    float T;             // �����ؽڵ��������(�������������)(Nm)
+    float W;             // �����ؽ��ٶ�(����������ٶ�)(rad/s)
+    float Pos;           // �����ؽ�λ��(rad)
+    float K_P;           // �ؽڸն�ϵ��(0-25.599)
+    float K_W;           // �ؽ��ٶ�ϵ��(0-25.599)
+
+
+} GO_MotorCmd_t;
+
+/// @brief ��������ṹ��
+typedef struct
+{
+    unsigned char motor_id; // ���ID
+    unsigned char mode;     // 0:���� 1:FOC���� 2:����궨
+    int Temp;               // �¶�
+    int MError;             // ������
+    float T;                // ��ǰʵ�ʵ���������(�������������)(Nm)
+    float W;                // ��ǰʵ�ʵ���ٶ�(����������ٶ�)(rad/s)
+    float Pos;              // ��ǰ���λ��(rad)
+    int correct;            // ���������Ƿ�����(1������0������)
+    int footForce;          // �����������ԭʼ��ֵ
+
+    uint16_t calc_crc;
+    uint32_t timeout;       // ͨѶ��ʱ ����
+    uint32_t bad_msg;       // CRC����� ����
+    RIS_MotorData_t motor_recv_data; // ����������ݽṹ��
+
+} GO_MotorData_t;
+
+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);
+void GO_M8010_send_data( GO_MotorCmd_t *cmd);
+void USART6_RxCompleteCallback(void );
+
+
+
+#endif /*__GO_M8010_6_H */
diff --git a/User/device/can_use.c b/User/device/can_use.c
index b48339e..5d48677 100644
--- a/User/device/can_use.c
+++ b/User/device/can_use.c
@@ -12,10 +12,12 @@
 #define CAN_MOTOR_CUR_RES (16384) 
 
 
-#define CAN_GM6020_CTRL_ID_BASE (0x1ff)
+//#define CAN_GM6020_CTRL_ID_BASE (0x1ff)  //禁用
 #define CAN_PITCH6020_CTRL_ID (0x2ff)
 
-#define CAN_G3508_CTRL_ALL_ID (0x200)
+#define CAN_G3508_CTRL_ALL_CHASSIS_ID (0x200)
+#define CAN_G3508_CTRL_ALL_UP_ID (0x1ff)
+
 #define CAN_VESC_CTRL_ID_BASE (0x300)
 
 
@@ -41,12 +43,12 @@ static CAN_t *gcan;
 static void CAN_DJIMotor_Decode(CAN_MotorFeedback_t *feedback,
                              const uint8_t *raw) {
   if (feedback == NULL || raw == NULL) return;
-  uint16_t raw_angle = (uint16_t)((raw[0] << 8) | raw[1]);
+  uint16_t raw_ecd = (uint16_t)((raw[0] << 8) | raw[1]);
   int16_t raw_current = (int16_t)((raw[4] << 8) | raw[5]);
 	feedback->rotor_speed = (int16_t)((raw[2] << 8) | raw[3]);
   feedback->temp = raw[6]; 															 
 														 
-  feedback->rotor_angle = raw_angle / (float)CAN_MOTOR_ENC_RES * 360.0f;
+  feedback->rotor_ecd = raw_ecd / (float)CAN_MOTOR_ENC_RES * 360.0f;
   feedback->torque_current =
   raw_current * CAN_GM6020_MAX_ABS_CUR / (float)CAN_MOTOR_CUR_RES;														 
 }
@@ -144,49 +146,49 @@ int8_t CAN_DJIMotor_Control(CAN_MotorGroup_e group, CAN_Output_t *output,
                          CAN_t *can) {
   if (output == NULL) return DEVICE_ERR_NULL;
 
-  int16_t motor1, motor2, motor3, motor4, motor5 ;
+  int16_t motor1, motor2, motor3, motor4, motor5, motor6, motor7;
 
   switch (group) {
 		
-    case CAN_MOTOR_CHASSIS6020:
-      motor1 =
-          (int16_t)(output->chassis6020.named.m1);
-      motor2 =
-          (int16_t)(output->chassis6020.named.m2);
-      motor3 =
-          (int16_t)(output->chassis6020.named.m3);
-      motor4 =
-          (int16_t)(output->chassis6020.named.m4);
+//    case CAN_MOTOR_CHASSIS6020:  //(总线0x1ff的6020禁用，怕和3508冲突)
+//      motor1 =
+//          (int16_t)(output->chassis6020.named.m1);
+//      motor2 =
+//          (int16_t)(output->chassis6020.named.m2);
+//      motor3 =
+//          (int16_t)(output->chassis6020.named.m3);
+//      motor4 =
+//          (int16_t)(output->chassis6020.named.m4);
 
-      raw_tx.tx_header.StdId = CAN_GM6020_CTRL_ID_BASE;
-      raw_tx.tx_header.IDE = CAN_ID_STD;
-      raw_tx.tx_header.RTR = CAN_RTR_DATA;
-      raw_tx.tx_header.DLC = CAN_MOTOR_TX_BUF_SIZE;
+//      raw_tx.tx_header.StdId = CAN_GM6020_CTRL_ID_BASE;
+//      raw_tx.tx_header.IDE = CAN_ID_STD;
+//      raw_tx.tx_header.RTR = CAN_RTR_DATA;
+//      raw_tx.tx_header.DLC = CAN_MOTOR_TX_BUF_SIZE;
 
-      raw_tx.tx_data[0] = (uint8_t)((motor1 >> 8) & 0xFF);
-      raw_tx.tx_data[1] = (uint8_t)(motor1 & 0xFF);
-      raw_tx.tx_data[2] = (uint8_t)((motor2 >> 8) & 0xFF);
-      raw_tx.tx_data[3] = (uint8_t)(motor2 & 0xFF);
-      raw_tx.tx_data[4] = (uint8_t)((motor3 >> 8) & 0xFF);
-      raw_tx.tx_data[5] = (uint8_t)(motor3 & 0xFF);
-      raw_tx.tx_data[6] = (uint8_t)((motor4 >> 8) & 0xFF);
-      raw_tx.tx_data[7] = (uint8_t)(motor4 & 0xFF);
+//      raw_tx.tx_data[0] = (uint8_t)((motor1 >> 8) & 0xFF);
+//      raw_tx.tx_data[1] = (uint8_t)(motor1 & 0xFF);
+//      raw_tx.tx_data[2] = (uint8_t)((motor2 >> 8) & 0xFF);
+//      raw_tx.tx_data[3] = (uint8_t)(motor2 & 0xFF);
+//      raw_tx.tx_data[4] = (uint8_t)((motor3 >> 8) & 0xFF);
+//      raw_tx.tx_data[5] = (uint8_t)(motor3 & 0xFF);
+//      raw_tx.tx_data[6] = (uint8_t)((motor4 >> 8) & 0xFF);
+//      raw_tx.tx_data[7] = (uint8_t)(motor4 & 0xFF);
 
-      HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->chassis6020),
-                           &raw_tx.tx_header, raw_tx.tx_data,
-                           &(can->mailbox.chassis));
-      break;
-		case CAN_MOTOR_3508:
+//      HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->chassis6020),
+//                           &raw_tx.tx_header, raw_tx.tx_data,
+//                           &(can->mailbox.chassis));
+//      break;
+		case CAN_MOTOR_CHASSIS3508:
       motor1 = 
-          (int16_t)(output->motor3508.named.m1);
+          (int16_t)(output->motor_CHASSIS3508.named.m1);
       motor2 = 
-          (int16_t)(output->motor3508.named.m2);
+          (int16_t)(output->motor_CHASSIS3508.named.m2);
       motor3 = 
-          (int16_t)(output->motor3508.named.m3);
+          (int16_t)(output->motor_CHASSIS3508.named.m3);
       motor4 = 
-          (int16_t)(output->motor3508.named.m4);
+          (int16_t)(output->motor_CHASSIS3508.named.m4);
 
-      raw_tx.tx_header.StdId =CAN_G3508_CTRL_ALL_ID;
+      raw_tx.tx_header.StdId =CAN_G3508_CTRL_ALL_CHASSIS_ID;
       raw_tx.tx_header.IDE = CAN_ID_STD;
       raw_tx.tx_header.RTR = CAN_RTR_DATA;
       raw_tx.tx_header.DLC = CAN_MOTOR_TX_BUF_SIZE;
@@ -200,13 +202,45 @@ int8_t CAN_DJIMotor_Control(CAN_MotorGroup_e group, CAN_Output_t *output,
       raw_tx.tx_data[6] = (uint8_t)((motor4 >> 8) & 0xFF);
       raw_tx.tx_data[7] = (uint8_t)(motor4 & 0xFF);
 
-      HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->motor3508),
+      HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->motor_CHASSIS3508),        		 
                            &raw_tx.tx_header, raw_tx.tx_data,
                            &(can->mailbox.chassis));
 			break;
+		case CAN_MOTOR_UP3508:
+      motor1 = 
+          (int16_t)(output->motor_UP3508.named.m1);
+      motor2 = 
+          (int16_t)(output->motor_UP3508.named.m2);
+      motor3 = 
+          (int16_t)(output->motor_UP3508.named.m3);
+      motor4 = 
+          (int16_t)(output->motor_UP3508.named.m4);
+
+      raw_tx.tx_header.StdId =CAN_G3508_CTRL_ALL_UP_ID;
+      raw_tx.tx_header.IDE = CAN_ID_STD;
+      raw_tx.tx_header.RTR = CAN_RTR_DATA;
+      raw_tx.tx_header.DLC = CAN_MOTOR_TX_BUF_SIZE;
+
+      raw_tx.tx_data[0] = (uint8_t)((motor1 >> 8) & 0xFF);
+      raw_tx.tx_data[1] = (uint8_t)(motor1 & 0xFF);
+      raw_tx.tx_data[2] = (uint8_t)((motor2 >> 8) & 0xFF);
+      raw_tx.tx_data[3] = (uint8_t)(motor2 & 0xFF);
+      raw_tx.tx_data[4] = (uint8_t)((motor3 >> 8) & 0xFF);
+      raw_tx.tx_data[5] = (uint8_t)(motor3 & 0xFF);
+      raw_tx.tx_data[6] = (uint8_t)((motor4 >> 8) & 0xFF);
+      raw_tx.tx_data[7] = (uint8_t)(motor4 & 0xFF);
+
+      HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->motor_UP3508),
+                           &raw_tx.tx_header, raw_tx.tx_data,
+                           &(can->mailbox.up));
+			break;			
 		case CAN_MOTOR_PITCH6020:
 			 motor5 = 
           (int16_t)(output->pitch6020.named.m1);
+			 motor6 = 
+          (int16_t)(output->pitch6020.named.m2);			 
+		   motor7 = 
+          (int16_t)(output->pitch6020.named.m3);		
 		  raw_tx.tx_header.StdId = CAN_PITCH6020_CTRL_ID;
       raw_tx.tx_header.IDE = CAN_ID_STD;
       raw_tx.tx_header.RTR = CAN_RTR_DATA;
@@ -214,10 +248,10 @@ int8_t CAN_DJIMotor_Control(CAN_MotorGroup_e group, CAN_Output_t *output,
 		  
 		  raw_tx.tx_data[0] = (uint8_t)((motor5 >> 8) & 0xFF);
       raw_tx.tx_data[1] = (uint8_t)(motor5 & 0xFF);
-      raw_tx.tx_data[2] = 0;
-      raw_tx.tx_data[3] = 0;
-      raw_tx.tx_data[4] = 0;
-      raw_tx.tx_data[5] = 0;
+      raw_tx.tx_data[2] = (uint8_t)((motor6 >> 8) & 0xFF);
+      raw_tx.tx_data[3] = (uint8_t)(motor6 & 0xFF);
+      raw_tx.tx_data[4] = (uint8_t)((motor7 >> 8) & 0xFF);
+      raw_tx.tx_data[5] = (uint8_t)(motor7 & 0xFF);
       raw_tx.tx_data[6] = 0;
       raw_tx.tx_data[7] = 0;
 		
@@ -256,7 +290,7 @@ int8_t CAN_VESC_Control(int id,CAN_MotorGroup_e group, CAN_Output_t *output,CAN_
     int Byte[4];
    Vesc_ByteGet raw[4];
     switch (group) {
-     case CAN_MOTOR_CHASSIS5065:
+     case CAN_MOTOR_5065:
 			 //将期望的四个电机输出值分别对应到四个联合体 为了下面的拆分字节
 			raw[0].as_array = output->chassis5065.named.m1;
 		  raw[1].as_array = output->chassis5065.named.m2;
@@ -322,12 +356,12 @@ int8_t CAN_StoreMsg(CAN_t *can, CAN_RawRx_t *can_rx) {
         CAN_DJIMotor_Decode(&(can->motor.chassis6020.as_array[index]), can_rx->rx_data);
         break;
 
-      case CAN_M3508_M1_ID:
-      case CAN_M3508_M2_ID:
-      case CAN_M3508_M3_ID:
-      case CAN_M3508_M4_ID:
+      case CAN_CHASSIS_M3508_M1_ID:
+      case CAN_CHASSIS_M3508_M2_ID:
+      case CAN_CHASSIS_M3508_M3_ID:
+      case CAN_CHASSIS_M3508_M4_ID:
         // 存储消息到对应的电机结构体中
-        index = can_rx->rx_header.StdId - CAN_M3508_M1_ID;
+        index = can_rx->rx_header.StdId - CAN_CHASSIS_M3508_M1_ID;
         can->recive_flag |= 1 << (index);
         CAN_DJIMotor_Decode(&(can->motor.motor3508.as_array[index]), can_rx->rx_data);
         detect_hook(CHASSIS3508M1_TOE + index);
@@ -348,6 +382,17 @@ int8_t CAN_StoreMsg(CAN_t *can, CAN_RawRx_t *can_rx) {
         CAN_DJIMotor_Decode(&(can->motor.pit6020.as_array[index]), can_rx->rx_data);
         detect_hook(PITCH6020_TOE + index);
         break;
+			case CAN_UP_M3508_M1_ID:
+      case CAN_UP_M3508_M2_ID:
+      case CAN_UP_M3508_M3_ID:
+      case CAN_UP_M3508_M4_ID:
+        // 存储消息到对应的电机结构体中
+        index = can_rx->rx_header.StdId - CAN_UP_M3508_M1_ID;
+        can->recive_flag |= 1 << (index);
+        CAN_DJIMotor_Decode(&(can->motor.motor3508.as_array[index]), can_rx->rx_data);
+        detect_hook(CHASSIS3508M1_TOE + index);
+        break;
+
         case CAN_SICK_ID:
          // 存储消息到sickfed结构体中
          CAN_Sick_Receive(&(can->sickfed), can_rx->rx_data);
diff --git a/User/device/can_use.h b/User/device/can_use.h
index ff1f5ab..182db2b 100644
--- a/User/device/can_use.h
+++ b/User/device/can_use.h
@@ -5,16 +5,22 @@
 #include "bsp_can.h"
 
 typedef enum {
-  CAN_M3508_M1_ID = 0x201, /* 1 */
-  CAN_M3508_M2_ID = 0x202, /* 2 */
-  CAN_M3508_M3_ID = 0x203, /* 3 */
-  CAN_M3508_M4_ID = 0x204, /* 4 */
+  CAN_CHASSIS_M3508_M1_ID = 0x201, /* 1 */
+  CAN_CHASSIS_M3508_M2_ID = 0x202, /* 2 */
+  CAN_CHASSIS_M3508_M3_ID = 0x203, /* 3 */
+  CAN_CHASSIS_M3508_M4_ID = 0x204, /* 4 */
 
+	/**/
 	CAN_G6020_AgvM1 =0x205,
 	CAN_G6020_AgvM2 =0x206,
 	CAN_G6020_AgvM3 =0x207,
 	CAN_G6020_AgvM4 =0x208,
 	
+  CAN_UP_M3508_M1_ID = 0x205, /* 1 */
+  CAN_UP_M3508_M2_ID = 0x206, /* 2 */
+  CAN_UP_M3508_M3_ID = 0x207, /* 3 */
+  CAN_UP_M3508_M4_ID = 0x208, /* 4 */
+	
 	CAN_SICK_ID=0x301,
 	
 	CAN_G6020_Pitch =0x209,
@@ -38,10 +44,10 @@ typedef enum {
 
 
 typedef enum {
-  CAN_MOTOR_CHASSIS6020 = 0,
 	CAN_MOTOR_PITCH6020,
-  CAN_MOTOR_CHASSIS5065,
-	CAN_MOTOR_3508,
+  CAN_MOTOR_5065,
+	CAN_MOTOR_CHASSIS3508,
+	CAN_MOTOR_UP3508,
   CAN_MOTOR_GROUT_NUM,
 } CAN_MotorGroup_e;
 
@@ -77,9 +83,10 @@ typedef union{
 }Vesc_ByteGet;
 
 typedef struct {
-  CAN_DJIOutput_t chassis6020;
+//  CAN_DJIOutput_t chassis6020;
   CAN_VescOutput_t chassis5065;
-	CAN_DJIOutput_t motor3508;
+	CAN_DJIOutput_t motor_CHASSIS3508;
+	CAN_DJIOutput_t motor_UP3508;
 	CAN_DJIOutput_t pitch6020;
 
 } CAN_Output_t;
@@ -98,17 +105,18 @@ typedef struct {
 } CAN_RawTx_t;
 
 typedef struct {
-  BSP_CAN_t chassis6020;
+//  BSP_CAN_t chassis6020;//禁用
   BSP_CAN_t chassis5065;
-	BSP_CAN_t motor3508;
-	BSP_CAN_t pitch6020;
+	BSP_CAN_t motor_CHASSIS3508;
+	BSP_CAN_t motor_UP3508;
+	BSP_CAN_t pitch6020; 
 	BSP_CAN_t sick;
 } CAN_Params_t;
 
 
 /* 电机反馈信息 */
 typedef struct {
-  float rotor_angle;
+  float rotor_ecd;
   float rotor_speed;
   float torque_current;
   float temp;
@@ -157,6 +165,8 @@ typedef struct {
   const CAN_Params_t *param;
   struct {
     uint32_t chassis;
+    uint32_t up;
+		
   } mailbox;
   osMessageQueueId_t msgq_raw;
 	
diff --git a/User/device/cmd.c b/User/device/cmd.c
index eb45f94..0a1f114 100644
--- a/User/device/cmd.c
+++ b/User/device/cmd.c
@@ -14,54 +14,44 @@
 /* Private function  -------------------------------------------------------- */
 /*Export function --------------------------------------------------------------*/
 
-
-
-
-#ifdef dr16_t
-
-
-#else
-
-
-#endif
 int8_t CMD_Init(CMD_t *cmd){
    /*若主结构体为空 自动返回错误 */
   if(cmd == NULL) return-1;
  /**/
-  cmd->C_cmd.type =RC;
-	cmd->C_cmd.mode =NORMAL;
+  cmd->CMD_CtrlType =RCcontrol;
+	cmd->CMD_mode =Normal;
 
 return 0;
 }
 
 
+
+
 static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd) {
 
   /* 将操纵杆的对应值转换为底盘的控制向量和云台变化的欧拉角 */
 //	    RC_dr16 ,  
 //    RC_r12ds  
 //	
-#ifdef r12ds_t	
+
 	/*乐迪反馈值转换(-1  --  1 )范围*/
 	
-	if(rc->ch_x<0) cmd->Vx =map_fp32(rc->ch_x,-337,0,-1,0);
-	else cmd->Vx =map_fp32(rc->ch_x,0,310,0,1);
-	
-	
-	if(rc->ch_y<0) cmd->Vy =map_fp32(rc->ch_y,-260,0,-1,0);
-	else cmd->Vy =map_fp32(rc->ch_y,0,312,0,1);
+//	if(rc->ch_x<0) cmd->Vx =map_fp32(rc->ch_x,-337,0,-1,0);
+//	else cmd->Vx =map_fp32(rc->ch_x,0,310,0,1);
+//	
+//	
+//	if(rc->ch_y<0) cmd->Vy =map_fp32(rc->ch_y,-260,0,-1,0);
+//	else cmd->Vy =map_fp32(rc->ch_y,0,312,0,1);
 
-	if(rc->ch_w<0) cmd->Vw =map_fp32(rc->ch_w,-308,0,-1,0);
-	else cmd->Vw =map_fp32(rc->ch_w,0,291,0,1);
+//	if(rc->ch_w<0) cmd->Vw =map_fp32(rc->ch_w,-308,0,-1,0);
+//	else cmd->Vw =map_fp32(rc->ch_w,0,291,0,1);
 
 
-  cmd->key_ctrl_l = rc->key [0];
-  cmd->key_ctrl_r = rc->key [1];
-	
-#elif defined(dr16_t)	
+//  cmd->key_ctrl_l = rc->key [0];
+//  cmd->key_ctrl_r = rc->key [1];
+//	
 
 
-	
 	cmd->Vx = rc->ch_r_x;  
   cmd->Vy = rc->ch_r_y;  
   cmd->Vw = rc->ch_l_x;
@@ -70,14 +60,7 @@ static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd) {
 
   cmd->key_ctrl_l = rc->sw_l;
   cmd->key_ctrl_r = rc->sw_r ;
-  
-#endif 
-	
-	
-	
-	
-	
-	
+
 	
 }
 
@@ -90,19 +73,18 @@ static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd) {
  */
 static void CMD_RcLostLogic(CMD_t *cmd){
   /* 机器人底盘运行模式恢复至放松模式 */
-  cmd->C_cmd.mode = RELAXED;
+  cmd->CMD_CtrlType = RELAXED;
 }
 int8_t CMD_ParseRc(CMD_t *cmd,CMD_RC_t *rc){   
    if (cmd == NULL) return -1;
    if (rc == NULL) return -1;
-#ifdef dr16_t	
+
   /*c当rc丢控时，恢复机器人至默认状态 */
   if ((rc->sw_l == CMD_SW_ERR) || (rc->sw_r == CMD_SW_ERR)) {
     CMD_RcLostLogic(cmd);
   } else {
       CMD_RcLogic(rc, cmd);
   }
-#endif	
 return 0;
 }
 
@@ -122,15 +104,15 @@ int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n){
     switch(cmd->cmd_status){
 
 			case MID:
-			cmd->C_navi.vx = n->navi.vx;
-			cmd->C_navi.vy = n->navi.vy;
-			cmd->C_navi.wz = n->navi.wz;
+			cmd->cmd_MID360.posx = n->navi.vx;
+			cmd->cmd_MID360.posy = n->navi.vy;
+			cmd->cmd_MID360.posw = n->navi.wz;
 			break;
 			
 			case PICK :
-				cmd ->C_pick .posx =n->pick .posx ;
-				cmd ->C_pick .posy =n->pick .posy ;
-				cmd ->C_pick .posw =n->pick .posw ;
+				cmd ->cmd_pick .posx =n->pick .posx ;
+				cmd ->cmd_pick .posy =n->pick .posy ;
+				cmd ->cmd_pick .posw =n->pick .posw ;
 
 			break;
 						
@@ -147,81 +129,68 @@ int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n){
   这一部分没有设置具体的模式名，后期根据需要修改
 		遥控器模式(RC):
 		左按键  ---  右按键
-		             mode1
-          up     	 no_mode	
+		                 mode1
+          up     	   no_mode	
                      mode2
 
 
-		             mode3
+		                 mode3
           down     	 no_mode	
                      mode4
 
           mid        auto_navi(0x09)雷达导航
 
 */
-int8_t CMD_CtrlSet(CMD_t *cmd) {
+/*遥控器，上下层通用，按键控制，统一到cmd*/
+int8_t CMD_ParseRC(CMD_t *cmd,const CMD_RC_t *rc) {
+	
 
  if(cmd == NULL) return -1;
-  if(cmd->key_ctrl_l == CMD_SW_UP)//当左拨杆打到最上面时 强制使用遥控器控制
-	{
-       /*遥控器模式下，右按键三种状态分配*/	
 
-		if(cmd->key_ctrl_r==CMD_SW_UP)
-		{
-		     cmd->C_cmd.type = UP_CONTROL;
-		     cmd->C_cmd.mode = RC_MODE1;			
-		}
-		if(cmd->key_ctrl_r==CMD_SW_MID)
-		{
-		    cmd->C_cmd.type = RC;
-		    cmd->C_cmd.mode = RC_NO_MODE;			
-		}		
-		if(cmd->key_ctrl_r==CMD_SW_DOWN)//遥控器控制模式下，右拨杆上时启动
-		{
-		     cmd->C_cmd.type = UP_CONTROL;
-		     cmd->C_cmd.mode =RC_MODE2;			
-		}
-		
-	}
-	else if(cmd->key_ctrl_l ==CMD_SW_DOWN)
+	cmd->Vx = rc->ch_r_x;  
+  cmd->Vy = rc->ch_r_y;  
+  cmd->Vw = rc->ch_l_x;
+
+  cmd->poscamear = rc->ch_l_y;
+
+  cmd->key_ctrl_l = rc->sw_l;
+  cmd->key_ctrl_r = rc->sw_r ;
+	
+  if ((rc->sw_l == CMD_SW_ERR) || (rc->sw_r == CMD_SW_ERR)) {
+     cmd->CMD_CtrlType =RELAXED;
+  }	
+	else if(rc->sw_l==CMD_SW_UP)
 	{
-		if(cmd->key_ctrl_r==CMD_SW_UP)
-		{
-		     cmd->C_cmd.type = RC;
-		     cmd->C_cmd.mode = RC_MODE3;			
-		}
-		if(cmd->key_ctrl_r==CMD_SW_MID)
-		{
-		    cmd->C_cmd.type = RC;
-		    cmd->C_cmd.mode = RC_NO_MODE;			
-		}		
-		if(cmd->key_ctrl_r==CMD_SW_DOWN)//遥控器控制模式下，右拨杆上时启动
-		{
-		     cmd->C_cmd.type = RC;
-		     cmd->C_cmd.mode = RC_MODE4;			
-		}
-		
+		cmd ->CMD_CtrlType =RCcontrol;
+		if(rc->sw_r ==CMD_SW_UP) cmd ->CMD_mode =Pitch; //左上，右上，投篮，设置好的
+		if(rc->sw_r ==CMD_SW_MID) cmd ->CMD_mode =Normal; //左上，右中，无模式
+	  if(rc->sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Pitch_HAND; //左上，右上，手动调整
 	}
-	else //左按键打到中间，自动模式
-	{	
-      if(	cmd->key_ctrl_l==CMD_SW_MID	){
+	
+	else if(rc->sw_l==CMD_SW_MID)
+	{
 				
-		   cmd->C_cmd.type = AUTO; 
- 		   cmd->C_cmd.mode = RC_NO_MODE ; //雷达导航			
-			}  		 
-		if(cmd->key_ctrl_r==CMD_SW_UP)
+		if(rc->sw_r ==CMD_SW_UP) cmd ->CMD_CtrlType =MID_NAVI;; //左中，右上，雷达
+		if(rc->sw_r ==CMD_SW_MID) 
 		{
-		    cmd->C_cmd.type = AUTO;
-		    cmd->C_cmd.mode = AUTO_NAVI;			
-		}		
-		if(cmd->key_ctrl_r==CMD_SW_DOWN)//遥控器控制模式下，右拨杆上时启动
-		{
-		     cmd->C_cmd.type = AUTO;
-		     cmd->C_cmd.mode = AUTO_PICK;			
+		  cmd ->CMD_CtrlType =RCcontrol;
+      cmd ->CMD_mode =Normal; //左中，右中，无模式	
 		}
- 
- 
-  }
-return 0;
+	  if(rc->sw_r ==CMD_SW_DOWN) 
+		{
+		cmd ->CMD_mode =Normal; //左中，右下，无模式
+	  cmd ->CMD_CtrlType =RCcontrol;
+		}
+	}
+	else if(rc->sw_l==CMD_SW_DOWN)
+	{
+		cmd ->CMD_CtrlType =RCcontrol;
+		if(rc->sw_r ==CMD_SW_UP) cmd ->CMD_mode =Normal; //左下，右上，投篮
+		if(rc->sw_r ==CMD_SW_MID) cmd ->CMD_mode =Normal; //左下，右中，无模式
+	  if(rc->sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Normal; //左下，右上，无模式
+	}	
+    
+
+	return 0;
 }
 
diff --git a/User/device/cmd.h b/User/device/cmd.h
index 3780e5a..7e80722 100644
--- a/User/device/cmd.h
+++ b/User/device/cmd.h
@@ -1,21 +1,3 @@
-/*        
-
-该任务用于接收来自各个不同的控制方式所期望的控制指令 将其集中统一化后分发给各个模块
-
-*/			
-					
-		
-/*
-     按键控制逻辑
-	RC模式，左按键打到最上，右按键启用，中间无状态，上下各代表模式1、模式2
-	雷达导航，左按键打到中间，右按键禁用
-	左按键打到下面，保留，未启用
-
-*/
-
-
-
-
 #ifndef _CMD_H
 #define _CMD_H
 #include "struct_typedef.h"
@@ -25,95 +7,29 @@
 #define MID (0x09)
 #define PICK (0x06)
 
-
-/*选择遥控器，else为r12ds
-更改遥控器时，建议取消相应的task
-
-
-*/
-#define dr16_t  
-//#define r12ds_t
-
-
-
-
 typedef enum{
-	RC,//遥控器控制，左按键上
+	RCcontrol,//遥控器控制，左按键上，控制上层
+	MID_NAVI,//雷达导航
+	PICK_t,
 	AUTO,
-	UP_CONTROL
- }CMD_Chassis_CtrlType_e;
+	RELAXED,//异常模式
+
+ }CMD_CtrlType_t;
 
 typedef  enum{
-	 RELAXED,//异常模式
-	 NORMAL,
-	 GYRO_STAY,
-		
-	 RC_MODE1,
-	 RC_NO_MODE,
-	 RC_MODE2,
+			
+     
+		Normal,    //无模式
+	  AUTO_NAVI,
+	  AUTO_PICK,
+
+    Dribble ,  //运球
+	  Pitch,     //投篮
+	  /*视觉辅助下的投篮*/
+    Pitch_HAND,
 	
-	 RC_MODE3,
-	 RC_MODE4,
-	  
-	 AUTO_NAVI,
-	 AUTO_PICK
-  }CMD_Chassis_mode_e;
-/*该结构体负责接收和解析地盘模块所需要的控制指令*/
-typedef struct{
-
- CMD_Chassis_CtrlType_e type;
-
- CMD_Chassis_mode_e mode;
-
-
-}CMD_Chassis_Ctrl_t;
-
-
-
-/* 拨杆位置 */
-typedef enum {
-  CMD_SW_ERR = 0,
-  CMD_SW_UP = 1,
-  CMD_SW_MID = 3,
-  CMD_SW_DOWN = 2,
-} CMD_SwitchPos_t;
-
-
-/*遥控器值，使用CMD_RcLogic函数传入CMD_t结构体*/
-typedef struct {
-	
-#ifdef dr16_t	
-	
-  float ch_l_x; /* 遥控器左侧摇杆横轴值，上为正 */
-  float ch_l_y; /* 遥控器左侧摇杆纵轴值，右为正 */
-  float ch_r_x; /* 遥控器右侧摇杆横轴值，上为正 */
-  float ch_r_y; /* 遥控器右侧摇杆纵轴值，右为正 */
-
-  float ch_res; /* 第五通道值 */
-
-  CMD_SwitchPos_t sw_r; /* 右侧拨杆位置 */
-  CMD_SwitchPos_t sw_l; /* 左侧拨杆位置 */
-
-
-  uint16_t key; /* 按键值 */
-
-  uint16_t res; /* 保留，未启用 */
-
-	
-#else	
-    int16_t ch_x;     // X轴（Channel 1）
-    int16_t mul;      // MUL（Channel 2）
-    int16_t ch_y;     // Y轴（Channel 3）
-    int16_t ch_w;     // W轴（Channel 4）
-    int16_t key[8];   // 按键通道（Channel 5-12）
-		
-	  int16_t offline;
-#endif		
-} __attribute__((packed))CMD_RC_t;
-
-
-
-
+   Dribbl_transfer
+  }CMD_mode_t;
 typedef struct {
  uint8_t  status_fromnuc;
  uint8_t  ctrl_status;  //取其中每一个二进制位用作通信
@@ -133,56 +49,95 @@ typedef struct {
 	{
 		fp32 angle;
 	}sick_cali;
-	
 } CMD_NUC_t;
+/* 拨杆位置 */
+typedef enum {
+  CMD_SW_ERR = 0,
+  CMD_SW_UP = 1,
+  CMD_SW_MID = 3,
+  CMD_SW_DOWN = 2,
+} CMD_SwitchPos_t;
 
-typedef struct{
-	 fp32 posy;
-   fp32 posx;
-   fp32 posw;	
-}CMD_FOR_PICK;
+/*遥控器值，使用CMD_RcLogic函数传入CMD_t结构体*/
+typedef struct {
+  float ch_l_x; /* 遥控器左侧摇杆横轴值，上为正 */
+  float ch_l_y; /* 遥控器左侧摇杆纵轴值，右为正 */
+  float ch_r_x; /* 遥控器右侧摇杆横轴值，上为正 */
+  float ch_r_y; /* 遥控器右侧摇杆纵轴值，右为正 */
+
+  float ch_res; /* 第五通道值 */
+
+  CMD_SwitchPos_t sw_r; /* 右侧拨杆位置 */
+  CMD_SwitchPos_t sw_l; /* 左侧拨杆位置 */
 
 
-typedef  struct {
-  fp32 vx;
-  fp32 vy;
-  fp32 wz;
-	
-	
-}CMD_FOR_NAVI;
+  uint16_t key; /* 按键值 */
+
+  uint16_t res; /* 保留，未启用 */
+
+} __attribute__((packed))CMD_RC_t;
 
 typedef struct {
-   uint8_t cmd_status;
-   uint8_t raw_status;
-	 
-   uint8_t status[7];
-	
-   fp32 Vx;
-   fp32 Vy;
-   fp32 Vw;
 
-   fp32 poscamear;
-	  
+   uint8_t cmd_status;
+   uint8_t raw_status;	 
+   uint8_t status[7];	
+	
    fp32 key_ctrl_l;
    fp32 key_ctrl_r;
-	 
-	 fp32 forsick_wz;
 	
-	CMD_Chassis_Ctrl_t C_cmd;
-	CMD_FOR_NAVI C_navi;
-	CMD_FOR_PICK C_pick;
+	 fp32 Vx;
+   fp32 Vy;
+   fp32 Vw;
+   fp32 poscamear;
+
+		struct 
+		{
+			 fp32 posx;
+			 fp32 posy;
+			 fp32 posw;
+			}cmd_pick;
+				struct 
+		{
+			 fp32 posx;
+			 fp32 posy;
+			 fp32 posw;
+			}cmd_MID360;
+		 
+
+ 
+   CMD_mode_t CMD_mode;
+   CMD_CtrlType_t CMD_CtrlType;
+	
 } CMD_t;
 
 
+/*nuc数据统一到cmd*/
+/*
+该任务用来处理来自不同控制方式下传回的期望的控制指令在此处统一为通用格式的控制指令发送给其他任务
+
+*/
+
+
+
+/* Includes ----------------------------------------------------------------- */
+#include "cmd.h"
+#include "gpio.h"
+#include "user_math.h"
+#include <string.h>
+
+/* Private function  -------------------------------------------------------- */
+/*Export function --------------------------------------------------------------*/
+
 int8_t CMD_Init(CMD_t *cmd);
 
+
+static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd) ;
+static void CMD_RcLostLogic(CMD_t *cmd);
 int8_t CMD_ParseRc(CMD_t *cmd,CMD_RC_t *rc);
 
 int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n);
-
-int8_t CMD_SwitchStatus(CMD_t *cmd);
-
-int8_t CMD_CtrlSet(CMD_t *cmd);
+int8_t CMD_ParseRC(CMD_t *cmd,const CMD_RC_t *rc) ;
 
 #endif 
 
diff --git a/User/device/dr16.c b/User/device/dr16.c
index 8c8dc81..04454d2 100644
--- a/User/device/dr16.c
+++ b/User/device/dr16.c
@@ -12,8 +12,8 @@
 #include "error_detect.h"
 
 
-#ifdef dr16_t
-//#define 
+
+
 /* Private define ----------------------------------------------------------- */
 #define DR16_CH_VALUE_MIN (364u)
 #define DR16_CH_VALUE_MID (1024u)
@@ -132,5 +132,5 @@ int8_t DR16_HandleOffline(const DR16_t *dr16, CMD_RC_t *rc) {
 }
 
 
-#endif
+
 
diff --git a/User/device/dr16.h b/User/device/dr16.h
index 0eca50c..2b99679 100644
--- a/User/device/dr16.h
+++ b/User/device/dr16.h
@@ -12,10 +12,10 @@
 /* Exported macro ----------------------------------------------------------- */
 /* Exported types ----------------------------------------------------------- */
 
-#ifdef dr16_t
 
 
-#define dr16_t
+
+
 typedef  __packed struct  
 {
   uint16_t ch_r_x : 11;
@@ -49,5 +49,5 @@ int8_t DR16_HandleOffline(const DR16_t *dr16, CMD_RC_t *rc);
 
 #endif
 
-#endif
+
 
diff --git a/User/device/motor_control.c b/User/device/motor_control.c
index 5bcf876..560da9f 100644
--- a/User/device/motor_control.c
+++ b/User/device/motor_control.c
@@ -1,249 +1,7 @@
-/**
-  ****************************(C) COPYRIGHT 2021 QUT****************************
-  * @file       motor_control.c/h
-  * @brief      3508/2006电机初始化、速度环、位置环；6020电机初始化、位置环（相对角度）
-  * @note       3508位置环使用的是增量式编码器，没有绝对位置，注意它所对应的角度是M3508的输出轴所对应的角度，由减速比经过换算得来，
-	*             若要修改对于的真实角度，应该在CAN_RECEIVE中修改相对应的解码函数。
-	              同时3508的控制函数也可对应相同ID的2006配套使用（但注意相对应的角度和速度值可能会出现问题）需要修改相应的减速比
-  * @history
-  *  Version    Date            Author          Modification        remark
-  *  V1.0.0     Dec-23-2021     QUT              1. 完成
-  *  V1.0.1     Dec-23-2023     QUT hks          1. 完成            仅仅对原先函数进行了少量修改，增添了部分注释，未对部分参数进行适配
-	
-  @verbatim
-  ==============================================================================
-
-  ==============================================================================
-  @endverbatim
-  ****************************(C) COPYRIGHT 2021 QUT****************************
-  */
 #include "motor_control.h"
-#include "CAN_receive.h"
 #include "pid.h"
 #include "user_math.h"
 #include "device\device.h"
-
-//3508motor init ID1~4
-//mode	1:velocity
-//			2:pos
-void M3508_motor_init(int mode);
-//3508velocity_loop ID1~4
-void M3508_velocity_loop(int16_t speed1,int16_t speed2,int16_t speed3,int16_t speed4);
-//3508position_loop ID1~4
-void M3508_pos_loop(fp32 angle1,fp32 angle2,fp32 angle3,fp32 angle4);
-//6020 init ID1
-void GM6020_position_loop_init(void);
-//6020 pos_loop ID1 relative angle ,this is used for 6020 angel range from 0 to 180
-void GM6020_pos_loop(int32_t ecd);
+#include "can_use.h"
 
 
-
-const motor_measure_t *motor_data[4];//3508 motor _data
-pid_type_def motor_pid[4];//3508velocity_pid
-pid_type_def angle_pid[4];//3508position_pid 
-//position_pid time interval 10ms
-const fp32 speed_PID[3]={5.0,0.3,0.0};	//P,I,D(velocity)
-const fp32 angle_PID[3]={25.0,0.0,1.5};	//P,I,D(position)
-//velocity_pid
-const fp32 PID[3]={5.0,0.01,0.0};
-
-const motor_measure_t *GM6020_data;//6020 data_motor
-gimbal_PID_t gimbal_motor_relative_angle_pid;//6020 relative angle position_pid
-pid_type_def gimbal_motor_gyro_pid;//6020 relative angle velocity_pid
-gimbal_pid_control_t GM6020_pos_loop_control;//6020 position_control
-const fp32 GM6020_PID[3]={3600.0,20.0,0};//6020 velocity_PID
-
-//*****3508*****//
-//3508 motor init ID1~4
-//mode	1:velocity
-//			2:position
-void M3508_motor_init(int mode)
-{
-	//get 3508 motor_data from CAN_receive
-	motor_data[0] = get_chassis_motor_measure_point(0);
-	motor_data[1] = get_chassis_motor_measure_point(1);
-	motor_data[2] = get_chassis_motor_measure_point(2);
-	motor_data[3] = get_chassis_motor_measure_point(3);
-	
-	switch(mode)
-	{
-		//velocity_init
-		case 1:
-						for(int i=0;i<4;i++)
-						{
-							PID_init(&motor_pid[i],PID_POSITION,PID,16000,2000);		//velocity_pid init
-						}break;
-		//pos_init
-		case 2:
-						for(int i=0;i<4;i++)
-						{
-							PID_init(&motor_pid[i],PID_POSITION,speed_PID,16000,2000);		//velocity_pid init
-							PID_init(&angle_pid[i],PID_POSITION,angle_PID,1000,200);		// pos_pid init
-						}break;
-	}
-
-}
-
-//3508 velocity
-/**
-  * @brief          send control current of motor (0x201, 0x202, 0x203, 0x204)
-  * the param is compatable with the fab data of 3508
-  * @param[in]      speed1: (0x201) 3508 motor control current, range [-16384,16384] 
-  * @param[in]      speed2: (0x202) 3508 motor control current, range [-16384,16384] 
-  * @param[in]      speed3: (0x203) 3508 motor control current, range [-16384,16384] 
-  * @param[in]      speed4: (0x204) 3508 motor control current, range [-16384,16384] 
-  * @retval         none
-  */
-void M3508_velocity_loop(int16_t speed1,int16_t speed2,int16_t speed3,int16_t speed4)
-{
-	int32_t speed[4];
-	int32_t delta_speed[4];
-	int i;
-	
-	speed[0] = speed1;
-	speed[1] = speed2;
-	speed[2] = speed3;
-	speed[3] = speed4;
-	
-	for(i=0;i<4;i++)
-	{
-		delta_speed[i] = PID_calc(&motor_pid[i],motor_data[i]->speed_rpm,speed[i]);
-	}
-	
-	CAN_cmd_G3508(delta_speed[0],delta_speed[1],delta_speed[2],delta_speed[3]);
-}
-
-//3508 pos
-/**
-  * @brief          send control current of motor (0x201, 0x202, 0x203, 0x204)
-  * this also can used to control the same ID of M2006  
-  * the param angle  is  correspond to the output shaft of M3508 
-
-  * @Attention!!!!!  the kind fo DJI ecd is incremental. 
-
-  * @param[in]      angle1: (0x201) 3508 motor control current, range [reserved] 
-  * @param[in]      angle2: (0x202) 3508 motor control current, range [reserved] 
-  * @param[in]      angle3: (0x203) 3508 motor control current, range [reserved] 
-  * @param[in]      angle4: (0x204) 3508 motor control current, range [reserved] 
-  * @retval         none
-  */
-void M3508_pos_loop(fp32 angle1,fp32 angle2,fp32 angle3,fp32 angle4)
-{
-	fp32 angle[4];
-	fp32 delta_angle[4];
-	int32_t delta_speed[4];
-	int i;
-	
-	angle[0] = angle1;
-	angle[1] = angle2;
-	angle[2] = angle3;
-	angle[3] = angle4;
-	
-	//outer ring angle_loop
-	for(i=0;i<4;i++)
-	{
-		delta_angle[i] = PID_calc(&angle_pid[i],motor_data[i]->total_angle,angle[i]);
-	}
-	//inner ring velocity_loop
-	for(i=0;i<4;i++)
-	{
-		delta_speed[i] = PID_calc(&motor_pid[i],motor_data[i]->speed_rpm,delta_angle[i]);
-	}
-	
-	CAN_cmd_G3508(delta_speed[0],delta_speed[1],delta_speed[2],delta_speed[3]);
-}
-
-
-//*****6020*****//
-//6020motor_init ID1
-void GM6020_position_loop_init(void)
-{
-	gimbal_PID_init(&GM6020_pos_loop_control.gimbal_motor_relative_angle_pid,10.0f,0.0f,8.0f,5.0f,0.0f);		//angle_loop_pid init
-	PID_init(&GM6020_pos_loop_control.gimbal_motor_gyro_pid,PID_POSITION,GM6020_PID,30000.0,5000.0);		//velocity_loop init
-	
-	GM6020_data=get_G6020_motor_measure_point();
-}
-
-//6020Pos ID1(relative angle)
-//this 6020is uesd only for angle_maximum(180)
-//ecd encoder  valueֵ(0-8191)
-void GM6020_pos_loop(int32_t ecd)
-{
-	//set positon
-	GM6020_pos_loop_control.angel_set=rad(ecd);
-	//calculate_relative_angle(-PI,PI)
-	GM6020_pos_loop_control.angel_get=motor_ecd_to_angle_change(GM6020_data->ecd,GM6020_pos_loop_control.angel_set);
-	//calculate_gyro (set-get)
-	GM6020_pos_loop_control.gyro_get=rad(GM6020_pos_loop_control.angel_set-GM6020_pos_loop_control.angel_get);
-	//angle_loop, pid
-  GM6020_pos_loop_control.gyro_set = gimbal_PID_calc(&GM6020_pos_loop_control.gimbal_motor_relative_angle_pid,GM6020_pos_loop_control.angel_get, GM6020_pos_loop_control.angel_set,GM6020_pos_loop_control.gyro_get);
-  GM6020_pos_loop_control.current_set = PID_calc(&GM6020_pos_loop_control.gimbal_motor_gyro_pid,GM6020_pos_loop_control.gyro_get, GM6020_pos_loop_control.gyro_set);
-	
-  //control value
-  GM6020_pos_loop_control.current_given = (int16_t)(GM6020_pos_loop_control.current_set);
-	CAN_cmd_gimbal(GM6020_pos_loop_control.current_given,0,0,0);
-}
-
-
-
-
-
-void gimbal_PID_init(gimbal_PID_t *pid, fp32 maxout, fp32 max_iout, fp32 kp, fp32 ki, fp32 kd)
-{
-//    if (pid == NULL)
-//    {
-//        return;
-//    }
-    pid->kp = kp;
-    pid->ki = ki;
-    pid->kd = kd;
-
-    pid->err = 0.0f;
-    pid->get = 0.0f;
-
-    pid->max_iout = max_iout;
-    pid->max_out = maxout;
-}
-
-fp32 gimbal_PID_calc(gimbal_PID_t *pid, fp32 get, fp32 set, fp32 error_delta)
-{
-    fp32 err;
-//    if (pid == NULL)
-//    {
-//        return 0.0f;
-//    }
-    pid->get = get;
-    pid->set = set;
-
-    err = set - get;
-    pid->err = rad_format(err);
-    pid->Pout = pid->kp * pid->err;
-    pid->Iout += pid->ki * pid->err;
-    pid->Dout = pid->kd * error_delta;
-    abs_limit_fp(&pid->Iout, pid->max_iout);
-    pid->out = pid->Pout + pid->Iout + pid->Dout;
-    abs_limit_fp(&pid->out, pid->max_out);
-    return pid->out;
-}
-
-//calculate relative_angle (-4096,4095)
-fp32 motor_ecd_to_angle_change(uint32_t ecd, uint16_t offset_ecd)
-{
-    int32_t relative_ecd = ecd - offset_ecd;
-    if (relative_ecd > 4095)
-    {
-      while(relative_ecd>4095)
-			{
-				relative_ecd -= 8191;
-			}
-    }
-    else if (relative_ecd < -4096)
-    { 
-			while(relative_ecd<-4096)
-			{
-				relative_ecd += 8191;
-			}
-    }
-		return relative_ecd * MOTOR_ECD_TO_RAD;
-}
-
diff --git a/User/device/motor_control.h b/User/device/motor_control.h
index c2158e3..2bf0589 100644
--- a/User/device/motor_control.h
+++ b/User/device/motor_control.h
@@ -4,58 +4,20 @@
 #include "struct_typedef.h"
 #include "pid.h"
 
-
-
+/*�󽮵����������*/
 typedef struct
 {
-    fp32 kp;
-    fp32 ki;
-    fp32 kd;
-
-    fp32 set;
-    fp32 get;
-    fp32 err;
-
-    fp32 max_out;
-    fp32 max_iout;
-
-    fp32 Pout;
-    fp32 Iout;
-    fp32 Dout;
-
-    fp32 out;
-} gimbal_PID_t;
-
-typedef struct
-{
-	gimbal_PID_t gimbal_motor_relative_angle_pid;
-	pid_type_def gimbal_motor_gyro_pid;
-	fp32 angel_set;
-	fp32 angel_get;
-	fp32 gyro_set;
-	fp32 gyro_get;
-	fp32 current_set;
-	fp32 current_given;
-}gimbal_pid_control_t;
+    uint16_t ecd;
+    int16_t speed_rpm;
+    int16_t given_current;
+    uint16_t temperate;
+    int16_t last_ecd;
+		int16_t round_cnt;
+		int16_t total_angle;
+		uint16_t offset_ecd;
+		uint32_t msg_cnt;
+} DJmotor_measure_t;
 
 
-#define FILTER_BUF_LEN		5
-
-//#define MOTOR_ECD_TO_ANGLE 0.00231311936
-#define MOTOR_ECD_TO_ANGLE  (360.0 / 8191.0 / (3591.0/187.0))
-#define MOTOR_ECD_TO_RAD 0.000766990394f
-#define rad_format(Ang) loop_fp32_constrain((Ang), (-4096*MOTOR_ECD_TO_RAD), (4095*MOTOR_ECD_TO_RAD))
-#define rad(code) ((code)*MOTOR_ECD_TO_RAD)
-
-
-void M3508_motor_init(int mode);
-void M3508_pos_loop(fp32 angle1,fp32 angle2,fp32 angle3,fp32 angle4);
-void GM6020_position_loop_init(void);
-void GM6020_pos_loop(int32_t ecd);
-void M3508_velocity_loop(int16_t speed1,int16_t speed2,int16_t speed3,int16_t speed4);
-fp32 gimbal_PID_calc(gimbal_PID_t *pid, fp32 get, fp32 set, fp32 error_delta);
-fp32 motor_ecd_to_angle_change(uint32_t ecd, uint16_t offset_ecd);
-void gimbal_PID_init(gimbal_PID_t *pid, fp32 maxout, fp32 max_iout, fp32 kp, fp32 ki, fp32 kd);
-void motor_speed_change_low_to_high(int rag);
-#endif 
 
+#endif
\ No newline at end of file
diff --git a/User/device/vofa.c b/User/device/vofa.c
index 0d584cc..248e161 100644
--- a/User/device/vofa.c
+++ b/User/device/vofa.c
@@ -30,10 +30,10 @@ void vofa_tx_main(float *data)
 	/*在下面使用对应的串口发送函数*/
 //	HAL_UART_Transmit_DMA(&huart1, ( uint8_t *)fdata, sizeof(fdata));
 //	osDelay(1);
-	HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_VOFA), tail, 4);
-  osDelay(1);
-//	CDC_Transmit_FS( ( uint8_t *)fdata, sizeof(fdata));
-//	osDelay(1);
+//	HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_VOFA), tail, 4);
+//  osDelay(1);
+	CDC_Transmit_FS( ( uint8_t *)fdata, sizeof(fdata));
+	osDelay(1);
 //	CDC_Transmit_FS( tail, 4);
 
 }
diff --git a/User/task/Calc_task.c b/User/task/Calc_task.c
deleted file mode 100644
index 0631191..0000000
--- a/User/task/Calc_task.c
+++ /dev/null
@@ -1,64 +0,0 @@
-#include "Calc_task.h"
-#include "cmsis_os.h"
-#include "user_task.h"
-#include "chassis.h"
-#include "remote_control.h"
-#include "map.h"
-#include "usart.h"
-
-
-
-////·����ѡ��
-//void path_select(void)
-//{
-
-//  current_path = rc_ctrl.sw[2];
-//	if(current_path!=last_path)
-//	{
-//		switch (current_path)
-//	  {
-//		//��ǰ��
-//	  case POINT_CURRENT:
-////			chassis_pos->IS_SICK = 0;//������sick��λ
-//			path_state.path = &path_current;//ѡ��·��
-//	    path_state.points_num = 1;//ȷ��·������
-//		  break;
-//	  //���趨��
-//	  case POINT_LEFT:
-////			chassis_pos->IS_SICK = 0;//������sick��λ
-//		  path_state.path = &path_left;//ѡ��·��
-//	    path_state.points_num = 1;//ȷ��·������
-////	    chassis_pos->sick.dis_set[2] = SICK_PATH_LEFT_Y;//sick�趨ֵX�ḳֵ
-////	    chassis_pos->sick.dis_set[3] = SICK_PATH_LEFT_X;//sick�趨ֵY�ḳֵ
-//		  break;
-//		//ǰ�趨��
-//	  case POINT_FRONT:
-////			chassis_pos->IS_SICK = 1;//����sick��λ
-//			path_state.path = &path_front;//ѡ��·��
-//	    path_state.points_num = 1;//ȷ��·������
-////      chassis_pos->sick.dis_set[2] = SICK_PATH_FRONT_Y;//sick�趨ֵX�ḳֵ
-////	    chassis_pos->sick.dis_set[3] = SICK_PATH_FRONT_X;//sick�趨ֵY�ḳֵ
-//		  break;
-//	  }
-//		chassis_pos->POS_IS_CPT = NO;
-////		chassis_pos->SICK_IS_CPT = NO;		
-//	}
-//   last_path = current_path;
-//}
-//��������
-void Task_Calc(void *argument)
-{
-	(void)argument; /* δʹ��argument���������� */
-	const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CALCULATE;
-	uint32_t tick = osKernelGetTickCount(); /* ������������Ƶ�ʵļ�ʱ */
- while(1)
-  {
-		
-   tick += delay_tick; /* ������һ������ʱ�� */
-   osDelayUntil(tick); /* ���н������ȴ���һ�λ��� */	
-	}
-	
-
-	
-}
-
diff --git a/User/task/Calc_task.h b/User/task/Calc_task.h
deleted file mode 100644
index 2f16765..0000000
--- a/User/task/Calc_task.h
+++ /dev/null
@@ -1,10 +0,0 @@
-#ifndef CALC_TASK_H
-#define CALC_TASK_H
-
-void path_select(void);
-void cal_task(void);
-void calc_init(void);
-
-
-
-#endif
diff --git a/User/task/can_task.c b/User/task/can_task.c
index a6081a8..d557f43 100644
--- a/User/task/can_task.c
+++ b/User/task/can_task.c
@@ -30,7 +30,7 @@ void Task_can(void *argument)
 const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CAN;
 
   /* Device Setup */
-  CAN_Init(&can, &(task_runtime.config.chassis_config->can));
+  CAN_Init(&can, &(task_runtime.config.config->can));
 
   uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计算 */
 	
@@ -57,15 +57,14 @@ const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CAN;
 			CAN_DJIMotor_Control(CAN_MOTOR_PITCH6020,&can_out,&can);				
 	  }									
 		if (osMessageQueueGet(task_runtime.msgq.can.output.chassis3508,
-                          &(can_out.motor3508), 0, 0) == osOK) {
-               CAN_DJIMotor_Control(CAN_MOTOR_3508,&can_out,&can);
+                          &(can_out.motor_CHASSIS3508), 0, 0) == osOK) {
+               CAN_DJIMotor_Control(CAN_MOTOR_CHASSIS3508,&can_out,&can);
+       }
+		if (osMessageQueueGet(task_runtime.msgq.can.output.chassis3508,
+                          &(can_out.motor_UP3508), 0, 0) == osOK) {
+               CAN_DJIMotor_Control(CAN_MOTOR_UP3508,&can_out,&can);
        }
 
-		if (osMessageQueueGet(task_runtime.msgq.can.output.chassis6020,
-		                      &(can_out.chassis6020),0,0) == osOK)  {
-			CAN_DJIMotor_Control(CAN_MOTOR_CHASSIS6020,&can_out,&can);				
-	  }	
-	
 		tick += delay_tick; /* 计算下一个唤醒时刻 */
           osDelayUntil(tick); /* 运行结束，等待下一个周期唤醒 */
       }
diff --git a/User/task/chassis_task.c b/User/task/chassis_task.c
index f3b6f7c..d9f8c87 100644
--- a/User/task/chassis_task.c
+++ b/User/task/chassis_task.c
@@ -47,7 +47,7 @@ void Task_Chassis(void *argument)
 	 (void)argument; /* 未使用argument，消除警告*/
 	const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CHASSIS;
 	
-	Chassis_init(&chassis,&(task_runtime.config.chassis_config->chassis),TASK_FREQ_CHASSIS);
+	Chassis_init(&chassis,&(task_runtime.config.config->chassis),TASK_FREQ_CHASSIS);
 
   uint32_t tick = osKernelGetTickCount();
 	
@@ -70,7 +70,7 @@ void Task_Chassis(void *argument)
 	osMessageQueueGet(task_runtime.msgq.can.feedback.CAN_feedback, &can, NULL, 0);
 						
 	/*底盘控制信息获取,目前dji遥控器*/		
-     osMessageQueueGet(task_runtime.msgq.cmd.chassis,&ctrl, NULL, 0);
+     osMessageQueueGet(task_runtime.msgq.cmd.cmd_ctrl_t,&ctrl, NULL, 0);
 			
 	/*锁定RTOS（实时操作系统）内核，防止任务切换，直到 osKernelUnlock() 被调用*/		
 	osKernelLock();
@@ -85,13 +85,13 @@ void Task_Chassis(void *argument)
       osKernelUnlock();
 	
 		osMessageQueueReset(task_runtime.msgq.can.output.chassis3508);//清空队列
-		osMessageQueuePut(task_runtime.msgq.can.output.chassis3508, &out.motor3508, 0, 0);  //发送数据
+		osMessageQueuePut(task_runtime.msgq.can.output.chassis3508, &out.motor_CHASSIS3508, 0, 0);  //发送数据
 		
 		osMessageQueueReset(task_runtime.msgq.can.output.pitch6020);
 		osMessageQueuePut(task_runtime.msgq.can.output.pitch6020, &out.pitch6020, 0, 0);
 
-		osMessageQueueReset(task_runtime.msgq.can.output.chassis6020);
-		osMessageQueuePut(task_runtime.msgq.can.output.chassis6020, &out.chassis6020, 0, 0);
+		osMessageQueueReset(task_runtime.msgq.can.output.pitch6020 );
+		osMessageQueuePut(task_runtime.msgq.can.output.pitch6020 , &out.pitch6020, 0, 0);
 		
 	vofa_send[0] = chassis.vofa_send[0];
    	vofa_send[1] = chassis.vofa_send[1];
diff --git a/User/task/cmd_task.c b/User/task/cmd_task.c
index d155116..23df6f0 100644
--- a/User/task/cmd_task.c
+++ b/User/task/cmd_task.c
@@ -46,7 +46,6 @@ void Task_cmd(void *argument){
 		if(osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc, &Nuc,0 ,0) ==osOK) //nuc
       CMD_ParseNuc(&cmd,&Nuc);
 		
-		CMD_CtrlSet(&cmd);
 		
 		if(osMessageQueueGet(task_runtime.msgq.cmd.raw.rc, &rc_ctrl, 0, 0) == osOK)//遥控器
       CMD_ParseRc(&cmd, &rc_ctrl);
@@ -54,8 +53,8 @@ void Task_cmd(void *argument){
     osKernelUnlock(); /* 同上 解锁RTOS内核 */	
 		
     /*将需要与其他任务共享的数据放到消息队列里 此处主要分享给底盘 后续会添加和上层机构的通信 */
-     osMessageQueueReset(task_runtime.msgq.cmd.chassis);
-     osMessageQueuePut(task_runtime.msgq.cmd.chassis,&cmd,0,0);
+     osMessageQueueReset(task_runtime.msgq.cmd.cmd_ctrl_t);
+     osMessageQueuePut(task_runtime.msgq.cmd.cmd_ctrl_t,&cmd,0,0);
 
     tick += delay_tick; /*计算下一个唤醒时刻*/
     osDelayUntil(tick); /*绝对延时 等待下一个唤醒时刻 */
diff --git a/User/task/dr16_task.c b/User/task/dr16_task.c
index 58d1662..d465e15 100644
--- a/User/task/dr16_task.c
+++ b/User/task/dr16_task.c
@@ -13,7 +13,7 @@
 /* Private macro ------------------------------------------------------------ */
 /* Private variables -------------------------------------------------------- */
 
-#ifdef dr16_t
+
 
 #ifdef DEBUG
 DR16_t dr16;
@@ -23,7 +23,7 @@ static DR16_t dr16;
 static CMD_RC_t cmd_rc;
 #endif
 
-#endif
+
 
 /* Private function --------------------------------------------------------- */
 /* Exported functions ------------------------------------------------------- */
@@ -38,13 +38,13 @@ void Task_dr16(void *argument) {
 	  const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_DR16;
       uint32_t tick = osKernelGetTickCount();
 
-#ifdef dr16_t
+
 
   DR16_Init(&dr16); /* 初始化dr16 */
-#endif
+
 
   while (1) {
-#ifdef dr16_t
+
 		
 #ifdef DEBUG
     /*  */
@@ -65,7 +65,7 @@ void Task_dr16(void *argument) {
     osMessageQueueReset(task_runtime.msgq.cmd.raw.rc);
     osMessageQueuePut(task_runtime.msgq.cmd.raw.rc, &cmd_rc, 0, 0);
   }
-#endif	
+
 
 		 	tick += delay_tick; /* 计算下一个唤醒时*/
 		
diff --git a/User/task/init.c b/User/task/init.c
index 3c90daa..e06a809 100644
--- a/User/task/init.c
+++ b/User/task/init.c
@@ -46,7 +46,9 @@ void Task_Init(void *argument) {
       osThreadNew(Task_cmd,NULL,&attr_cmd);
   task_runtime.thread.nuc =
       osThreadNew(Task_nuc,NULL,&attr_nuc);
-	
+	task_runtime.thread.up=
+      osThreadNew(Task_up,NULL,&attr_up);
+		
 	task_runtime.thread.error_detect =
 	    osThreadNew(Task_error_detect,NULL,&attr_error_detect);
 
@@ -62,9 +64,7 @@ void Task_Init(void *argument) {
 	task_runtime.msgq.can.output.pitch6020 = 
 	    osMessageQueueNew(2u, sizeof(CAN_DJIOutput_t), NULL);
 
-	task_runtime.msgq.can.output.chassis6020 = 
-	    osMessageQueueNew(2u, sizeof(CAN_DJIOutput_t), NULL);
-			
+
 
 	/* imu */
    task_runtime.msgq.imu.accl =
@@ -77,7 +77,7 @@ void Task_Init(void *argument) {
 		/*cmd */
    task_runtime.msgq.cmd.raw.rc =
       osMessageQueueNew(3u, sizeof(CMD_RC_t), NULL);
-	 task_runtime.msgq.cmd.chassis =
+	 task_runtime.msgq.cmd.cmd_ctrl_t =
 	    osMessageQueueNew(3u, sizeof(CMD_t), NULL);
    task_runtime.msgq.cmd.raw.nuc =
       osMessageQueueNew(3u,sizeof(CMD_NUC_t), NULL);	 
diff --git a/User/task/up_task.c b/User/task/up_task.c
new file mode 100644
index 0000000..7769574
--- /dev/null
+++ b/User/task/up_task.c
@@ -0,0 +1,118 @@
+/*
+上层控制任务
+*/
+/* Includes ----------------------------------------------------------------- */
+
+#include "up.h"
+#include "user_task.h"
+#include "can_use.h"
+#include <cmsis_os2.h>
+
+#include "vofa.h"
+
+static  CAN_t can;
+
+
+#ifdef DEBUG
+
+CAN_Output_t UP_CAN_out;
+
+UP_t UP  ;
+
+CMD_t up_cmd;
+
+
+#else 
+
+static CAN_Output_t up_can_out;
+
+static UP_t UP;
+
+static CMD_t up_cmd;
+
+#endif
+float  aaa=0;
+float bbb=0;
+float CCC=0;
+float ddd=0;
+
+
+/**
+ * \brief 
+ *
+ * \param argument 
+ */
+void Task_up(void *argument)
+{
+	
+	 (void)argument; /* 未使用argument，消除警告*/
+	const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_UP;
+	
+  uint32_t tick = osKernelGetTickCount();
+	up_init(&UP,&(task_runtime.config.config->up ),TASK_FREQ_UP);
+	
+	while(1)
+		{
+#ifdef DEBUG
+    task_runtime.stack_water_mark.up = osThreadGetStackSpace(osThreadGetId());
+#endif
+		UP_UpdateFeedback(&UP, &can,&up_cmd) ;
+//    GM6020_control(&UP, 100) ;
+//			UP_M3508_speed(&UP, 500); 
+			
+//				UP_angle_control(&UP,0,M2006);
+//			
+//			
+//			VESC_M5065_Control(&UP,  20000);
+			
+			
+//			
+
+      UP_control(&UP,&UP_CAN_out,&up_cmd);
+//			UP.motor_target .go_shoot =aaa;
+//			UP.motor_target .M2006_angle =bbb ;
+
+//			UP.motor_target .M3508_angle =CCC;
+//			UP.motor_target .go_shoot =aaa;
+//			UP.motor_target .M2006_angle =bbb;
+//			UP.motor_target .go_spin  =ddd;
+//			
+			ALL_Motor_Control(&UP,&UP_CAN_out);
+
+		osDelay(1);
+			
+
+	/*imu数据获取*/					
+	osMessageQueueGet(task_runtime.msgq.imu.eulr, &UP.pos088.imu_eulr, NULL, 0);
+			
+	osMessageQueueGet(task_runtime.msgq.imu.gyro, &UP.pos088.bmi088.gyro,NULL, 0);
+			
+	osMessageQueueGet(task_runtime.msgq.imu.accl, &UP.pos088.bmi088.accl,NULL, 0);
+	/*can上设备数据获取*/	
+	osMessageQueueGet(task_runtime.msgq.can.feedback.CAN_feedback, &can, NULL, 0);
+					
+	osMessageQueueGet(task_runtime.msgq.cmd .cmd_ctrl_t , &up_cmd, NULL, 0);
+		
+	/*锁定RTOS（实时操作系统）内核，防止任务切换，直到 osKernelUnlock() 被调用*/		
+	osKernelLock();
+	
+	 /*解锁*/
+      osKernelUnlock();
+
+
+
+		osMessageQueueReset(task_runtime.msgq.can.output.up3508 );//清空队列
+		osMessageQueuePut(task_runtime.msgq.can.output.up3508, &UP_CAN_out.motor_UP3508 , 0, 0);  //发送数据
+		osMessageQueuePut(task_runtime.msgq.can.output.shoot5065 ,&UP_CAN_out.chassis5065, 0, 0); 
+	
+
+   vofa_send [0]=UP.vofa_send [0];
+   vofa_send [1]=UP.vofa_send [1];
+	 vofa_tx_main (vofa_send);
+	 
+	tick += delay_tick;
+        osDelayUntil(tick);
+		 
+	  }
+	
+}
diff --git a/User/task/user_task.c b/User/task/user_task.c
index cbfad5d..bbf54ea 100644
--- a/User/task/user_task.c
+++ b/User/task/user_task.c
@@ -66,4 +66,8 @@ const osThreadAttr_t attr_dr16 = {
 	   .stack_size = 128 *4,
 };
 
-
+const osThreadAttr_t attr_up = {
+	   .name = "up",
+	   .priority = osPriorityRealtime,
+	   .stack_size = 512 * 4,
+};
diff --git a/User/task/user_task.h b/User/task/user_task.h
index 8ed568d..db66cb3 100644
--- a/User/task/user_task.h
+++ b/User/task/user_task.h
@@ -15,6 +15,8 @@
 
 // 分配的频率该如何给定?
 #define TASK_FREQ_CHASSIS (900u)
+#define TASK_FREQ_UP (900u)
+
 #define TASK_FREQ_CTRL_CMD (500u)
 #define TASK_FREQ_NUC (500u)
 #define TASK_FREQ_CAN (1000u)
@@ -36,6 +38,8 @@ typedef struct {
   struct {
     osThreadId_t atti_esti;
 		osThreadId_t chassis;
+		osThreadId_t up;
+
 		osThreadId_t dr16;		
 	  osThreadId_t r12ds;  
     osThreadId_t can;
@@ -60,16 +64,18 @@ typedef struct {
 				
 		 
       }raw;
-      osMessageQueueId_t chassis;
+      osMessageQueueId_t cmd_ctrl_t;
       osMessageQueueId_t status;
     } cmd;
         /* can任务放入、读取，电机或电容的输入输出 */
     struct {
       struct {
-        osMessageQueueId_t chassis6020;
-        osMessageQueueId_t chassis5065;
+
+        osMessageQueueId_t shoot5065;
 				osMessageQueueId_t chassis3508;
 				osMessageQueueId_t pitch6020;
+				osMessageQueueId_t up3508;
+
       } output;
       struct {
 		     osMessageQueueId_t CAN_feedback;//包括底盘，云台6020,3508,5065,sick,can上设备数据
@@ -91,6 +97,8 @@ typedef struct {
 #ifdef DEBUG
   struct {
     UBaseType_t chassis;
+		UBaseType_t up;
+
     UBaseType_t can;
     UBaseType_t atti_esti;
 		UBaseType_t dr16;
@@ -108,6 +116,7 @@ typedef struct {
     float atti_esti;
     float r12ds;
 		float dr16;
+		float up;
 
     float cmd;
     float nuc;
@@ -120,6 +129,7 @@ typedef struct {
     float atti_esti;
     float r12ds;
 		float dr16;
+		float up;
 
     float cmd;
     float nuc;
@@ -134,6 +144,8 @@ extern const osThreadAttr_t attr_init;
 
 extern const osThreadAttr_t attr_atti_esti;
 
+extern const osThreadAttr_t attr_up;
+
 extern const osThreadAttr_t attr_chassis;
 
 extern const osThreadAttr_t attr_can;
@@ -155,6 +167,9 @@ void Task_AttiEsti(void *argument);
 
 void Task_Chassis(void *argument);
 
+void Task_up(void *argument);
+
+
 void Task_can(void *argument);
 
 void Task_cmd(void *argument);