遥控器按键控制上层，重新分配控制

2025-03-27 00:54:24 +08:00 · 2025-03-27 00:54:24 +08:00 · b2be72e1b2
commit b2be72e1b2
parent 9cd4a4b161
52 changed files with 644 additions and 10160 deletions
--- a/AUTO_CHASSIS.ioc
+++ b/AUTO_CHASSIS.ioc
@ -145,23 +145,24 @@ Mcu.Pin24=PA4
 Mcu.Pin25=PC4
 Mcu.Pin26=PC5
 Mcu.Pin27=PE9
-Mcu.Pin28=PA7
+Mcu.Pin28=PE11
-Mcu.Pin29=PB0
+Mcu.Pin29=PA7
 Mcu.Pin3=PB3
-Mcu.Pin30=VP_CRC_VS_CRC
+Mcu.Pin30=PB0
-Mcu.Pin31=VP_FREERTOS_VS_CMSIS_V2
+Mcu.Pin31=VP_CRC_VS_CRC
-Mcu.Pin32=VP_SYS_VS_Systick
+Mcu.Pin32=VP_FREERTOS_VS_CMSIS_V2
-Mcu.Pin33=VP_TIM4_VS_ClockSourceINT
+Mcu.Pin33=VP_SYS_VS_Systick
-Mcu.Pin34=VP_TIM7_VS_ClockSourceINT
+Mcu.Pin34=VP_TIM4_VS_ClockSourceINT
-Mcu.Pin35=VP_TIM10_VS_ClockSourceINT
+Mcu.Pin35=VP_TIM7_VS_ClockSourceINT
-Mcu.Pin36=VP_USB_DEVICE_VS_USB_DEVICE_CDC_FS
+Mcu.Pin36=VP_TIM10_VS_ClockSourceINT
 Mcu.Pin37=VP_USB_DEVICE_VS_USB_DEVICE_CDC_FS
 Mcu.Pin4=PA14
 Mcu.Pin5=PA13
 Mcu.Pin6=PB7
 Mcu.Pin7=PB6
 Mcu.Pin8=PD0
 Mcu.Pin9=PC11
-Mcu.PinsNb=37
+Mcu.PinsNb=38
 Mcu.ThirdPartyNb=0
 Mcu.UserConstants=
 Mcu.UserName=STM32F407IGHx
@ -272,6 +273,8 @@ PD14.GPIOParameters=GPIO_Label
 PD14.GPIO_Label=Buzzer
 PD14.Locked=true
 PD14.Signal=S_TIM4_CH3
 PE11.Locked=true
 PE11.Signal=GPIO_Output
 PE9.GPIOParameters=PinState,GPIO_PuPd,GPIO_Label
 PE9.GPIO_Label=FlagForUpper
 PE9.GPIO_PuPd=GPIO_PULLUP
--- a/Core/Inc/stm32f4xx_hal_conf.h
+++ b/Core/Inc/stm32f4xx_hal_conf.h
@ -190,7 +190,7 @@
 #define  USE_HAL_SMBUS_REGISTER_CALLBACKS       0U /* SMBUS register callback disabled     */
 #define  USE_HAL_SPI_REGISTER_CALLBACKS         0U /* SPI register callback disabled       */
 #define  USE_HAL_TIM_REGISTER_CALLBACKS         0U /* TIM register callback disabled       */
-#define  USE_HAL_UART_REGISTER_CALLBACKS        1U /* UART register callback disabled      */
+#define  USE_HAL_UART_REGISTER_CALLBACKS        0U /* UART register callback disabled      */
 #define  USE_HAL_USART_REGISTER_CALLBACKS       0U /* USART register callback disabled     */
 #define  USE_HAL_WWDG_REGISTER_CALLBACKS        0U /* WWDG register callback disabled      */
--- a/Core/Src/gpio.c
+++ b/Core/Src/gpio.c
@ -63,6 +63,9 @@ void MX_GPIO_Init(void)
  /*Configure GPIO pin Output Level */
  HAL_GPIO_WritePin(FlagForUpper_GPIO_Port, FlagForUpper_Pin, GPIO_PIN_SET);
  /*Configure GPIO pin Output Level */
  HAL_GPIO_WritePin(GPIOE, GPIO_PIN_11, GPIO_PIN_RESET);
  /*Configure GPIO pin Output Level */
  HAL_GPIO_WritePin(GYRO_CS_GPIO_Port, GYRO_CS_Pin, GPIO_PIN_SET);
@ -99,6 +102,13 @@ void MX_GPIO_Init(void)
  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
  HAL_GPIO_Init(FlagForUpper_GPIO_Port, &GPIO_InitStruct);
  /*Configure GPIO pin : PE11 */
  GPIO_InitStruct.Pin = GPIO_PIN_11;
  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
  HAL_GPIO_Init(GPIOE, &GPIO_InitStruct);
  /*Configure GPIO pin : PtPin */
  GPIO_InitStruct.Pin = GYRO_CS_Pin;
  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
--- a/Core/Src/main.c
+++ b/Core/Src/main.c
@ -32,7 +32,6 @@
 /* USER CODE BEGIN Includes */
 #include "bsp_delay.h"
 #include "bsp_can.h"
 #include "Action.h"
 //#include "bsp_dwt.h"
 /* USER CODE END Includes */
@ -80,7 +79,9 @@ int main(void)
  /* MCU Configuration--------------------------------------------------------*/
-  /* Reset of all peripherals, Initializes the Flash interface and the Systick. *   HAL_Init();
+  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();
  /* USER CODE BEGIN Init */
@ -95,7 +96,6 @@ int main(void)
  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_DMA_Init();
  MX_SPI1_Init();
  MX_TIM4_Init();
--- a/MDK-ARM/ELE_CHASSIS/AUTO_CHASSIS.hex
+++ b/MDK-ARM/ELE_CHASSIS/AUTO_CHASSIS.hex
--- a/MDK-ARM/ELE_CHASSIS/ELE_CHASSIS.hex
+++ b/MDK-ARM/ELE_CHASSIS/ELE_CHASSIS.hex
--- a/MDK-ARM/AUTO_CHASSIS.uvoptx
+++ b/MDK-ARM/AUTO_CHASSIS.uvoptx
@ -12,7 +12,7 @@
    <lExt>*.lib</lExt>
    <tExt>*.txt; *.h; *.inc; *.md</tExt>
    <pExt>*.plm</pExt>
-    <CppX>*.cpp</CppX>
+    <CppX>*.cpp; *.cc; *.cxx</CppX>
    <nMigrate>0</nMigrate>
  </Extensions>
@ -22,11 +22,11 @@
  </DaveTm>
  <Target>
-    <TargetName>AUTO_CHASSIS</TargetName>
+    <TargetName>R2_UP</TargetName>
    <ToolsetNumber>0x4</ToolsetNumber>
    <ToolsetName>ARM-ADS</ToolsetName>
    <TargetOption>
-      <CLKADS>12000000</CLKADS>
+      <CLKADS>25000000</CLKADS>
      <OPTTT>
        <gFlags>1</gFlags>
        <BeepAtEnd>1</BeepAtEnd>
@ -117,11 +117,6 @@
        <pMon>STLink\ST-LINKIII-KEIL_SWO.dll</pMon>
      </DebugOpt>
      <TargetDriverDllRegistry>
        <SetRegEntry>
          <Number>0</Number>
          <Key>ST-LINKIII-KEIL_SWO</Key>
          <Name>-U00160029510000164E574E32 -O206 -SF5000 -C0 -A0 -I0 -HNlocalhost -HP7184 -P1 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO131091 -TC12000000 -TT12000000 -TP21 -TDS8005 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)</Name>
        </SetRegEntry>
        <SetRegEntry>
          <Number>0</Number>
          <Key>ARMRTXEVENTFLAGS</Key>
@ -139,13 +134,13 @@
        </SetRegEntry>
        <SetRegEntry>
          <Number>0</Number>
-          <Key>CMSIS_AGDI</Key>
+          <Key>ST-LINKIII-KEIL_SWO</Key>
-          <Name>-X"Horco CMSIS-DAP" -U8626380832 -O206 -S0 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO65554 -TC168000000 -TT168000000 -TP20 -TDS8010 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)</Name>
+          <Name>-U-O206 -O206 -SF10000 -C0 -A0 -I0 -HNlocalhost -HP7184 -P1 -N00("ARM CoreSight SW-DP (ARM Core") -D00(2BA01477) -L00(0) -TO131090 -TC10000000 -TT10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO7 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)</Name>
        </SetRegEntry>
        <SetRegEntry>
          <Number>0</Number>
          <Key>DLGUARM</Key>
-          <Name></Name>
+          <Name>(105=-1,-1,-1,-1,0)</Name>
        </SetRegEntry>
        <SetRegEntry>
          <Number>0</Number>
@ -158,57 +153,20 @@
        <Ww>
          <count>0</count>
          <WinNumber>1</WinNumber>
-          <ItemText>raw,0x0A</ItemText>
+          <ItemText>aaa,0x0A</ItemText>
        </Ww>
        <Ww>
          <count>1</count>
          <WinNumber>1</WinNumber>
          <ItemText>UP,0x0A</ItemText>
        </Ww>
        <Ww>
          <count>2</count>
          <WinNumber>1</WinNumber>
          <ItemText>can,0x0A</ItemText>
        </Ww>
        <Ww>
          <count>3</count>
          <WinNumber>1</WinNumber>
          <ItemText>can_out,0x0A</ItemText>
        </Ww>
        <Ww>
          <count>4</count>
          <WinNumber>1</WinNumber>
          <ItemText>bbb,0x0A</ItemText>
        </Ww>
        <Ww>
          <count>5</count>
          <WinNumber>1</WinNumber>
          <ItemText>aaa,0x0A</ItemText>
        </Ww>
        <Ww>
          <count>6</count>
          <WinNumber>1</WinNumber>
          <ItemText>CCC</ItemText>
        </Ww>
        <Ww>
          <count>7</count>
          <WinNumber>1</WinNumber>
          <ItemText>GO_motor_info</ItemText>
        </Ww>
      </WatchWindow1>
      <WatchWindow2>
        <Ww>
          <count>0</count>
          <WinNumber>2</WinNumber>
          <ItemText>a</ItemText>
        </Ww>
      </WatchWindow2>
      <Tracepoint>
        <THDelay>0</THDelay>
      </Tracepoint>
      <DebugFlag>
        <trace>0</trace>
-        <periodic>1</periodic>
+        <periodic>0</periodic>
        <aLwin>1</aLwin>
        <aCover>0</aCover>
        <aSer1>0</aSer1>
@ -246,18 +204,18 @@
      <pSingCmdsp></pSingCmdsp>
      <pMultCmdsp></pMultCmdsp>
      <DebugDescription>
-        <Enable>0</Enable>
+        <Enable>1</Enable>
        <EnableFlashSeq>0</EnableFlashSeq>
        <EnableLog>0</EnableLog>
        <Protocol>2</Protocol>
-        <DbgClock>5000000</DbgClock>
+        <DbgClock>10000000</DbgClock>
      </DebugDescription>
    </TargetOption>
  </Target>
  <Group>
    <GroupName>Application/MDK-ARM</GroupName>
-    <tvExp>1</tvExp>
+    <tvExp>0</tvExp>
    <tvExpOptDlg>0</tvExpOptDlg>
    <cbSel>0</cbSel>
    <RteFlg>0</RteFlg>
@ -289,7 +247,7 @@
  <Group>
    <GroupName>Application/User/Core</GroupName>
-    <tvExp>1</tvExp>
+    <tvExp>0</tvExp>
    <tvExpOptDlg>0</tvExpOptDlg>
    <cbSel>0</cbSel>
    <RteFlg>0</RteFlg>
@ -721,7 +679,7 @@
  <Group>
    <GroupName>Middlewares/FreeRTOS</GroupName>
-    <tvExp>1</tvExp>
+    <tvExp>0</tvExp>
    <tvExpOptDlg>0</tvExpOptDlg>
    <cbSel>0</cbSel>
    <RteFlg>0</RteFlg>
@ -928,30 +886,6 @@
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
      <bDave2>0</bDave2>
      <PathWithFileName>..\User\bsp\gpio.c</PathWithFileName>
      <FilenameWithoutPath>gpio.c</FilenameWithoutPath>
      <RteFlg>0</RteFlg>
      <bShared>0</bShared>
    </File>
    <File>
      <GroupNumber>7</GroupNumber>
      <FileNumber>53</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
      <bDave2>0</bDave2>
      <PathWithFileName>..\User\bsp\spi.c</PathWithFileName>
      <FilenameWithoutPath>spi.c</FilenameWithoutPath>
      <RteFlg>0</RteFlg>
      <bShared>0</bShared>
    </File>
    <File>
      <GroupNumber>7</GroupNumber>
      <FileNumber>54</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
      <bDave2>0</bDave2>
      <PathWithFileName>..\User\bsp\pwm.c</PathWithFileName>
      <FilenameWithoutPath>pwm.c</FilenameWithoutPath>
      <RteFlg>0</RteFlg>
@ -959,7 +893,7 @@
    </File>
    <File>
      <GroupNumber>7</GroupNumber>
-      <FileNumber>55</FileNumber>
+      <FileNumber>53</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -971,7 +905,7 @@
    </File>
    <File>
      <GroupNumber>7</GroupNumber>
-      <FileNumber>56</FileNumber>
+      <FileNumber>54</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -983,7 +917,7 @@
    </File>
    <File>
      <GroupNumber>7</GroupNumber>
-      <FileNumber>57</FileNumber>
+      <FileNumber>55</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -995,7 +929,7 @@
    </File>
    <File>
      <GroupNumber>7</GroupNumber>
-      <FileNumber>58</FileNumber>
+      <FileNumber>56</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1007,7 +941,7 @@
    </File>
    <File>
      <GroupNumber>7</GroupNumber>
-      <FileNumber>59</FileNumber>
+      <FileNumber>57</FileNumber>
      <FileType>5</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1019,7 +953,7 @@
    </File>
    <File>
      <GroupNumber>7</GroupNumber>
-      <FileNumber>60</FileNumber>
+      <FileNumber>58</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1031,7 +965,7 @@
    </File>
    <File>
      <GroupNumber>7</GroupNumber>
-      <FileNumber>61</FileNumber>
+      <FileNumber>59</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1043,7 +977,7 @@
    </File>
    <File>
      <GroupNumber>7</GroupNumber>
-      <FileNumber>62</FileNumber>
+      <FileNumber>60</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1053,6 +987,30 @@
      <RteFlg>0</RteFlg>
      <bShared>0</bShared>
    </File>
    <File>
      <GroupNumber>7</GroupNumber>
      <FileNumber>61</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
      <bDave2>0</bDave2>
      <PathWithFileName>..\User\bsp\bsp_spi.c</PathWithFileName>
      <FilenameWithoutPath>bsp_spi.c</FilenameWithoutPath>
      <RteFlg>0</RteFlg>
      <bShared>0</bShared>
    </File>
    <File>
      <GroupNumber>7</GroupNumber>
      <FileNumber>62</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
      <bDave2>0</bDave2>
      <PathWithFileName>..\User\bsp\bsp_gpio.c</PathWithFileName>
      <FilenameWithoutPath>bsp_gpio.c</FilenameWithoutPath>
      <RteFlg>0</RteFlg>
      <bShared>0</bShared>
    </File>
  </Group>
  <Group>
@ -1140,8 +1098,8 @@
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
      <bDave2>0</bDave2>
-      <PathWithFileName>..\User\task\dr16_task.c</PathWithFileName>
+      <PathWithFileName>..\User\task\up_task.c</PathWithFileName>
-      <FilenameWithoutPath>dr16_task.c</FilenameWithoutPath>
+      <FilenameWithoutPath>up_task.c</FilenameWithoutPath>
      <RteFlg>0</RteFlg>
      <bShared>0</bShared>
    </File>
@ -1152,8 +1110,20 @@
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
      <bDave2>0</bDave2>
-      <PathWithFileName>..\User\task\up_task.c</PathWithFileName>
+      <PathWithFileName>..\User\task\cmd_task.c</PathWithFileName>
-      <FilenameWithoutPath>up_task.c</FilenameWithoutPath>
+      <FilenameWithoutPath>cmd_task.c</FilenameWithoutPath>
      <RteFlg>0</RteFlg>
      <bShared>0</bShared>
    </File>
    <File>
      <GroupNumber>8</GroupNumber>
      <FileNumber>71</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
      <bDave2>0</bDave2>
      <PathWithFileName>..\User\task\dr16_task.c</PathWithFileName>
      <FilenameWithoutPath>dr16_task.c</FilenameWithoutPath>
      <RteFlg>0</RteFlg>
      <bShared>0</bShared>
    </File>
@ -1167,7 +1137,7 @@
    <RteFlg>0</RteFlg>
    <File>
      <GroupNumber>9</GroupNumber>
-      <FileNumber>71</FileNumber>
+      <FileNumber>72</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1177,18 +1147,6 @@
      <RteFlg>0</RteFlg>
      <bShared>0</bShared>
    </File>
    <File>
      <GroupNumber>9</GroupNumber>
      <FileNumber>72</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
      <bDave2>0</bDave2>
      <PathWithFileName>..\User\task\cmd_task.c</PathWithFileName>
      <FilenameWithoutPath>cmd_task.c</FilenameWithoutPath>
      <RteFlg>0</RteFlg>
      <bShared>0</bShared>
    </File>
    <File>
      <GroupNumber>9</GroupNumber>
      <FileNumber>73</FileNumber>
@ -1249,18 +1207,6 @@
      <RteFlg>0</RteFlg>
      <bShared>0</bShared>
    </File>
    <File>
      <GroupNumber>9</GroupNumber>
      <FileNumber>78</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
      <bDave2>0</bDave2>
      <PathWithFileName>..\User\Algorithm\navi.c</PathWithFileName>
      <FilenameWithoutPath>navi.c</FilenameWithoutPath>
      <RteFlg>0</RteFlg>
      <bShared>0</bShared>
    </File>
  </Group>
  <Group>
@ -1271,7 +1217,7 @@
    <RteFlg>0</RteFlg>
    <File>
      <GroupNumber>10</GroupNumber>
-      <FileNumber>79</FileNumber>
+      <FileNumber>78</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1283,7 +1229,7 @@
    </File>
    <File>
      <GroupNumber>10</GroupNumber>
-      <FileNumber>80</FileNumber>
+      <FileNumber>79</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1295,7 +1241,7 @@
    </File>
    <File>
      <GroupNumber>10</GroupNumber>
-      <FileNumber>81</FileNumber>
+      <FileNumber>80</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1307,7 +1253,7 @@
    </File>
    <File>
      <GroupNumber>10</GroupNumber>
-      <FileNumber>82</FileNumber>
+      <FileNumber>81</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1319,7 +1265,7 @@
    </File>
    <File>
      <GroupNumber>10</GroupNumber>
-      <FileNumber>83</FileNumber>
+      <FileNumber>82</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1329,6 +1275,18 @@
      <RteFlg>0</RteFlg>
      <bShared>0</bShared>
    </File>
    <File>
      <GroupNumber>10</GroupNumber>
      <FileNumber>83</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>
    <File>
      <GroupNumber>10</GroupNumber>
      <FileNumber>84</FileNumber>
@ -1341,18 +1299,6 @@
      <RteFlg>0</RteFlg>
      <bShared>0</bShared>
    </File>
    <File>
      <GroupNumber>10</GroupNumber>
      <FileNumber>85</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>
@ -1363,7 +1309,7 @@
    <RteFlg>0</RteFlg>
    <File>
      <GroupNumber>11</GroupNumber>
-      <FileNumber>86</FileNumber>
+      <FileNumber>85</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1375,7 +1321,7 @@
    </File>
    <File>
      <GroupNumber>11</GroupNumber>
-      <FileNumber>87</FileNumber>
+      <FileNumber>86</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1387,7 +1333,7 @@
    </File>
    <File>
      <GroupNumber>11</GroupNumber>
-      <FileNumber>88</FileNumber>
+      <FileNumber>87</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1401,13 +1347,13 @@
  <Group>
    <GroupName>Application/User/USB_DEVICE/Target</GroupName>
-    <tvExp>1</tvExp>
+    <tvExp>0</tvExp>
    <tvExpOptDlg>0</tvExpOptDlg>
    <cbSel>0</cbSel>
    <RteFlg>0</RteFlg>
    <File>
      <GroupNumber>12</GroupNumber>
-      <FileNumber>89</FileNumber>
+      <FileNumber>88</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1421,13 +1367,13 @@
  <Group>
    <GroupName>Middlewares/USB_Device_Library</GroupName>
-    <tvExp>1</tvExp>
+    <tvExp>0</tvExp>
    <tvExpOptDlg>0</tvExpOptDlg>
    <cbSel>0</cbSel>
    <RteFlg>0</RteFlg>
    <File>
      <GroupNumber>13</GroupNumber>
-      <FileNumber>90</FileNumber>
+      <FileNumber>89</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1439,7 +1385,7 @@
    </File>
    <File>
      <GroupNumber>13</GroupNumber>
-      <FileNumber>91</FileNumber>
+      <FileNumber>90</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1451,7 +1397,7 @@
    </File>
    <File>
      <GroupNumber>13</GroupNumber>
-      <FileNumber>92</FileNumber>
+      <FileNumber>91</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1463,7 +1409,7 @@
    </File>
    <File>
      <GroupNumber>13</GroupNumber>
-      <FileNumber>93</FileNumber>
+      <FileNumber>92</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
--- a/MDK-ARM/AUTO_CHASSIS.uvprojx
+++ b/MDK-ARM/AUTO_CHASSIS.uvprojx
@ -7,7 +7,7 @@
  <Targets>
    <Target>
-      <TargetName>AUTO_CHASSIS</TargetName>
+      <TargetName>R2_UP</TargetName>
      <ToolsetNumber>0x4</ToolsetNumber>
      <ToolsetName>ARM-ADS</ToolsetName>
      <pArmCC>5060960::V5.06 update 7 (build 960)::.\ARMCC</pArmCC>
@ -49,8 +49,8 @@
            <NotGenerated>0</NotGenerated>
            <InvalidFlash>1</InvalidFlash>
          </TargetStatus>
-          <OutputDirectory>ELE_CHASSIS\</OutputDirectory>
+          <OutputDirectory>.\R2_UP\</OutputDirectory>
-          <OutputName>AUTO_CHASSIS</OutputName>
+          <OutputName>R2_UP</OutputName>
          <CreateExecutable>1</CreateExecutable>
          <CreateLib>0</CreateLib>
          <CreateHexFile>0</CreateHexFile>
@ -82,7 +82,7 @@
          </BeforeMake>
          <AfterMake>
            <RunUserProg1>0</RunUserProg1>
-            <RunUserProg2>1</RunUserProg2>
+            <RunUserProg2>0</RunUserProg2>
            <UserProg1Name></UserProg1Name>
            <UserProg2Name></UserProg2Name>
            <UserProg1Dos16Mode>0</UserProg1Dos16Mode>
@ -1128,16 +1128,6 @@
              <FileType>1</FileType>
              <FilePath>..\User\bsp\delay.c</FilePath>
            </File>
            <File>
              <FileName>gpio.c</FileName>
              <FileType>1</FileType>
              <FilePath>..\User\bsp\gpio.c</FilePath>
            </File>
            <File>
              <FileName>spi.c</FileName>
              <FileType>1</FileType>
              <FilePath>..\User\bsp\spi.c</FilePath>
            </File>
            <File>
              <FileName>pwm.c</FileName>
              <FileType>1</FileType>
@ -1183,6 +1173,16 @@
              <FileType>1</FileType>
              <FilePath>..\User\bsp\bsp_buzzer.c</FilePath>
            </File>
            <File>
              <FileName>bsp_spi.c</FileName>
              <FileType>1</FileType>
              <FilePath>..\User\bsp\bsp_spi.c</FilePath>
            </File>
            <File>
              <FileName>bsp_gpio.c</FileName>
              <FileType>1</FileType>
              <FilePath>..\User\bsp\bsp_gpio.c</FilePath>
            </File>
          </Files>
        </Group>
        <Group>
@ -1218,16 +1218,72 @@
              <FileType>1</FileType>
              <FilePath>..\User\task\error_detect_task.c</FilePath>
            </File>
            <File>
              <FileName>dr16_task.c</FileName>
              <FileType>1</FileType>
              <FilePath>..\User\task\dr16_task.c</FilePath>
            </File>
            <File>
              <FileName>up_task.c</FileName>
              <FileType>1</FileType>
              <FilePath>..\User\task\up_task.c</FilePath>
            </File>
            <File>
              <FileName>cmd_task.c</FileName>
              <FileType>1</FileType>
              <FilePath>..\User\task\cmd_task.c</FilePath>
              <FileOption>
                <CommonProperty>
                  <UseCPPCompiler>2</UseCPPCompiler>
                  <RVCTCodeConst>0</RVCTCodeConst>
                  <RVCTZI>0</RVCTZI>
                  <RVCTOtherData>0</RVCTOtherData>
                  <ModuleSelection>0</ModuleSelection>
                  <IncludeInBuild>1</IncludeInBuild>
                  <AlwaysBuild>2</AlwaysBuild>
                  <GenerateAssemblyFile>2</GenerateAssemblyFile>
                  <AssembleAssemblyFile>2</AssembleAssemblyFile>
                  <PublicsOnly>2</PublicsOnly>
                  <StopOnExitCode>11</StopOnExitCode>
                  <CustomArgument></CustomArgument>
                  <IncludeLibraryModules></IncludeLibraryModules>
                  <ComprImg>1</ComprImg>
                </CommonProperty>
                <FileArmAds>
                  <Cads>
                    <interw>2</interw>
                    <Optim>0</Optim>
                    <oTime>2</oTime>
                    <SplitLS>2</SplitLS>
                    <OneElfS>2</OneElfS>
                    <Strict>2</Strict>
                    <EnumInt>2</EnumInt>
                    <PlainCh>2</PlainCh>
                    <Ropi>2</Ropi>
                    <Rwpi>2</Rwpi>
                    <wLevel>0</wLevel>
                    <uThumb>2</uThumb>
                    <uSurpInc>2</uSurpInc>
                    <uC99>2</uC99>
                    <uGnu>2</uGnu>
                    <useXO>2</useXO>
                    <v6Lang>0</v6Lang>
                    <v6LangP>0</v6LangP>
                    <vShortEn>2</vShortEn>
                    <vShortWch>2</vShortWch>
                    <v6Lto>2</v6Lto>
                    <v6WtE>2</v6WtE>
                    <v6Rtti>2</v6Rtti>
                    <VariousControls>
                      <MiscControls></MiscControls>
                      <Define></Define>
                      <Undefine></Undefine>
                      <IncludePath></IncludePath>
                    </VariousControls>
                  </Cads>
                </FileArmAds>
              </FileOption>
            </File>
            <File>
              <FileName>dr16_task.c</FileName>
              <FileType>1</FileType>
              <FilePath>..\User\task\dr16_task.c</FilePath>
            </File>
          </Files>
        </Group>
        <Group>
@ -1238,11 +1294,6 @@
              <FileType>1</FileType>
              <FilePath>..\User\Algorithm\filter.c</FilePath>
            </File>
            <File>
              <FileName>cmd_task.c</FileName>
              <FileType>1</FileType>
              <FilePath>..\User\task\cmd_task.c</FilePath>
            </File>
            <File>
              <FileName>ahrs.c</FileName>
              <FileType>1</FileType>
@ -1268,11 +1319,6 @@
              <FileType>1</FileType>
              <FilePath>..\User\Algorithm\pid.c</FilePath>
            </File>
            <File>
              <FileName>navi.c</FileName>
              <FileType>1</FileType>
              <FilePath>..\User\Algorithm\navi.c</FilePath>
            </File>
          </Files>
        </Group>
        <Group>
@ -1303,16 +1349,16 @@
              <FileType>1</FileType>
              <FilePath>..\User\device\vofa.c</FilePath>
            </File>
            <File>
              <FileName>dr16.c</FileName>
              <FileType>1</FileType>
              <FilePath>..\User\device\dr16.c</FilePath>
            </File>
            <File>
              <FileName>GO_M8010_6_Driver.c</FileName>
              <FileType>1</FileType>
              <FilePath>..\User\device\GO_M8010_6_Driver.c</FilePath>
            </File>
            <File>
              <FileName>dr16.c</FileName>
              <FileType>1</FileType>
              <FilePath>..\User\device\dr16.c</FilePath>
            </File>
          </Files>
        </Group>
        <Group>
@ -1998,7 +2044,7 @@
      <component Cclass="CMSIS" Cgroup="CORE" Cvendor="ARM" Cversion="5.3.0" condition="ARMv6_7_8-M Device">
        <package name="CMSIS" schemaVersion="1.3" url="http://www.keil.com/pack/" vendor="ARM" version="5.6.0"/>
        <targetInfos>
-          <targetInfo name="AUTO_CHASSIS"/>
+          <targetInfo name="R2_UP"/>
        </targetInfos>
      </component>
    </components>
--- a/MDK-ARM/ELE_CHASSIS/AUTO_CHASSIS.axf
+++ b/MDK-ARM/ELE_CHASSIS/AUTO_CHASSIS.axf
--- a/User/Algorithm/navi.c
+++ b/User/Algorithm/navi.c
@ -1,216 +0,0 @@
 #include "navi.h"
 //导航初始化一系列参数
 int8_t Action_init(ops_t *o , const ops_param_t *param , Action_POS_t *pos)
 {
 	if(o ==NULL)  return DEVICE_ERR;
 	if(param == NULL) return DEVICE_ERR;
 //	while(!Action_ready)
 //	{		
 //     TIM4->CCR3 = 9999;//等待全场定位初始化
 //	}
 //    TIM4->CCR3 = 0;
 //	HAL_GPIO_WritePin(LED_G_GPIO_Port,LED_G_Pin,GPIO_PIN_SET);//接收到码盘数据后亮灯指示
 //	
 //			osDelay(5);
 	o->param = param;
 	/*全场定位pid初始化*/ 
 	//全场定位跑路径的xy方向速度环pid
 	PID_init(&o->pid_PosSpeed_x, PID_POSITION,(&o->param->ops_pid.pid_PosSpeed_x_param));
 	PID_init(&o->pid_PosSpeed_y, PID_POSITION,(&o->param->ops_pid.pid_PosSpeed_y_param));
 	 /* 初始化角度纠正PID */
 	PID_init(&o->pid_OutAngle,PID_POSITION,(&o->param->ops_pid.pid_OutAngle_param));//外环
 	PID_init(&o->pid_InnerAngle,PID_POSITION,(&o->param->ops_pid.pid_InnerAngle_param));//内环
 	//底盘xy方向位置环pid初始化
 	//内环
 	PID_init(&o->pid_pos_x_inner,PID_POSITION,(&o->param->ops_pid.pid_pos_x_inner_param));
 	PID_init(&o->pid_pos_y_inner,PID_POSITION,(&o->param->ops_pid.pid_pos_y_inner_param));
 	//外环
 	PID_init(&o->pid_pos_x_out,PID_POSITION,(&o->param->ops_pid.pid_pos_x_out_param));
 	PID_init(&o->pid_pos_y_out,PID_POSITION,(&o->param->ops_pid.pid_pos_y_out_param));
 	//底盘路径速度pid初始化
 	PID_init(&o->path_speed_pid,PID_POSITION,(&o->param->ops_pid.path_speed_pid_param));
 	//全场定位底盘坐标初始化
 	o->chassis_pos->pos_x=0;
 	o->chassis_pos->pos_y=0;	
 	o->chassis_pos->pos_lastX=0;
 	o->chassis_pos->pos_lastY=0;
 	o->chassis_map =param->path ;	
 	o->chassis_pos = pos;
 	//设置全场定位允许的误差范围
 	o->state.mistake = POS_ALLOW_MISTAKE;
 	o->state .angle_mistake =POS_ALLOW_ANGLE_MISTAKE ;
 	o->state.moveState = START;
 	o->state.points_num = param->path_num;
 	//标志位初始化
 	o->POS_IS_CPT = NO;
 	return DEVICE_OK;
 }
 //利用C板imu纠正角度
 fp32 ops9_AngleCorr(ops_t *o,fp32 hope_angle)
 {
 	fp32 delta_angle,delta_w;
 	//外环角度纠正
 	delta_angle = PID_calc(&o->pid_OutAngle,o->ops_imu_pos.yaw,hope_angle);
 	//内环速度纠正
 	delta_w = -PID_calc(&o->pid_InnerAngle,o->ops_gyro.z,delta_angle);
 	return delta_w;
 }
 //底盘xy速度计算(全场定位)
 void POS_chassis_set(ops_t *o,fp32 vx_set, fp32 vy_set, fp32 yaw_angle_set){
 		o->final_out.vx = (PID_calc(&o->pid_PosSpeed_x, o->chassis_pos->pos_Vx,vx_set));//根据遥控器的方向选择正负
 		o->final_out.vy = PID_calc(&o->pid_PosSpeed_y, o->chassis_pos->pos_Vy,vy_set);
 	  o->final_out.yaw_angle = ops9_AngleCorr(o,yaw_angle_set);
 }
 //定点控制
 void hold_point(ops_t *o,point_t p,fp32 yaw_angle_set)
 {
 	fp32 delta_x,delta_y;
 	fp32 delta_vx,delta_vy;
 	fp32 vx_set,vy_set;//x,y,w目标速度
 	//x
 	delta_x = PID_calc(&o->pid_pos_x_out,o->chassis_pos->pos_x,p.x);
 	delta_vx = PID_calc(&o->pid_pos_x_inner,o->chassis_pos->pos_Vx,delta_x);
 	//y
 	delta_y = PID_calc(&o->pid_pos_y_out,o->chassis_pos->pos_y,p.y);
 	delta_vy = PID_calc(&o->pid_pos_y_inner,o->chassis_pos->pos_Vy,delta_y);
 	vx_set = delta_vx;
 	vy_set = delta_vy;
 	POS_chassis_set(o,vx_set,vy_set,yaw_angle_set);
 }
 //判断是否到达函数(全场定位)，最最后判断
 bool isArrive(point_t p,ops_t *o)
 {
 		uint16_t xArrive = 0, yArrive = 0, zArrive=0;
 	xArrive = abs_float_double(p.x,o->chassis_pos->pos_x) < o->state.mistake ? 1:0;
 	yArrive = abs_float_double(p.y,o->chassis_pos->pos_y) < o->state.mistake ? 1:0;
 	if(xArrive && yArrive) return true;
 	else return false;
 }
 //寻迹，跑点函数
 int8_t go_path(ops_t *o,CMD_ACTION_t *ops_out)
 {
   static  fp32 distance;//两点之间的距离差
   static fp32 chassis_speed_set;//底盘速度设置
   static fp32 SIN,COS;//方向解算
   static fp32 world_vx,world_vy;//x,y方向分解速度
   static fp32 chassis_vx,chassis_vy;//底盘xy分速度
   static int cnt =0 ;//计数标志位
 	 static int flag_update_num;//记录flag更新，防止cnt一直变化
 	/*数据同步*/
 	o->current_x =o->chassis_pos ->pos_x ;
 	o->current_y =o->chassis_pos ->pos_y ;	
 	o->next_mapx = o->chassis_map[cnt].x;
 	o->next_mapy =o->chassis_map[cnt].y;	
  o->Navi_Mode .Trig_flag = ops_out ->flag ;	
 	if(o == NULL) return DEVICE_ERR;
 	if (ops_out == NULL) return DEVICE_ERR;
 // static fp32 yaw_angle_set = 0;	
 	if(o->POS_IS_CPT == NO)//路径未跑完
 	{
 		if(o->state.moveState ==START)
 		{
 			//更新路径状态
 			o->state.moveState =MOVING;			
 		}
 		distance = sqrt((o->chassis_map[cnt].x - o->chassis_pos->pos_x)*(o->chassis_map[cnt].x - o->chassis_pos->pos_x)
 										+(o->chassis_map[cnt].y - o->chassis_pos->pos_y)*(o->chassis_map[cnt].y - o->chassis_pos->pos_y));
 		chassis_speed_set = PID_calc(&o->path_speed_pid,-distance,0);//此处由pid计算距离得出相应的速度
 		//速度分解
 		SIN = (o->chassis_map[cnt].y - o->chassis_pos->pos_y)/distance;
 		COS = (o->chassis_map[cnt].x - o->chassis_pos->pos_x)/distance;
 		world_vx = chassis_speed_set * COS;
 		world_vy = chassis_speed_set * SIN;
 //		chassis_vx = world_vx;
 //		chassis_vy = world_vy;
 		//世界坐标转机器坐标系转换
 		chassis_vx = -world_vy*sin(o->ops_imu_pos.yaw) + world_vx*cos(o->ops_imu_pos.yaw);
 		chassis_vy = world_vy*cos(o->ops_imu_pos.yaw) + world_vx*sin(o->ops_imu_pos.yaw);
 	}
 /*下面的逻辑按需更改，根据cnt计数器自动跑点，遥控器触发跑点等等*/	
 	  if(isArrive(o->chassis_map[cnt],o))			//判断是否到达(全场定位)，只涉及xy的判断
 	{ 	
         /*到达点后，这里应该增加cnt增加或减小的触发方式*/
 	         if (o->Navi_Mode .Trig_flag != flag_update_num) { // 只有flag发生变化时才更新cnt
              if (o->Navi_Mode .Trig_flag==1){
 				cnt++;	
 		       }           
 		    }        
            else if (o->Navi_Mode .Trig_flag == -1) {
 			cnt--;
 		     }          
           flag_update_num = o->Navi_Mode .Trig_flag; // 更新prev_flag为当前的flag状态
 	}	
    else
 	{
 		 o->POS_IS_CPT = NO;
        hold_point(o,o->chassis_map[cnt],o->chassis_map[cnt].angle);	//调用该函数使临近误差值快速响应			
 	}					
 		if(cnt>3)//根据点数更改
 		{
 		  o->POS_IS_CPT =YES;
 			  //到达目的地
 	      o->final_out .vx =0;
 	      o->final_out .vy =0;
 	      o->final_out.yaw_angle=0;	
 		}	
 	ops_out->out.Vx= o->final_out .vx ;
 	ops_out->out.Vy = o->final_out .vy ;
 	ops_out->out.Vw = o->final_out.yaw_angle;
 	return DEVICE_OK;
 }
--- a/User/Algorithm/navi.h
+++ b/User/Algorithm/navi.h
@ -1,108 +0,0 @@
 #ifndef NAVI_H
 #define NAVI_H
 #include "Action.h"
 #include "pid.h"
 #include "ahrs.h"
 #include "bmi088.h"
 #include "map.h"
 typedef enum {
    AUTO_MODE, // 自动跑点模式
    MANUAL_MODE ,  // 控制模式
 	  NO_MODE
 } Navi_Mode_t;
 typedef struct {
   Navi_Mode_t mode; 
 	 int Trig_flag;//触发沿，初始值为零，两种控制模式下，这个值变为1，会使cnt+1，为-1，使cnt-1
 } Navi_COT_MODE_t;
 typedef struct{
 	float pos_x;
 	float pos_y;
 	float last_pos_x;
  float	last_pos_y;
 }chassis_position_t;
 typedef enum{
 	YES,
 	NO,
 	NEXT1,
 	NEXT2
 }is_cpt_e;
 typedef struct{
   pid_param_t pid_PosSpeed_x_param;                 
   pid_param_t pid_PosSpeed_y_param;                
   pid_param_t pid_pos_x_out_param;                   
   pid_param_t pid_pos_x_inner_param;                 
   pid_param_t pid_pos_y_out_param;                   
   pid_param_t pid_pos_y_inner_param;                 
   pid_param_t pid_OutAngle_param;                    
   pid_param_t pid_InnerAngle_param;                   
   pid_param_t path_speed_pid_param;                  
 }ops_pid_param_t;
 typedef struct {
 	ops_pid_param_t ops_pid;
 	const  point_t *path;
 	int8_t path_num;
 }ops_param_t;
 /**
 * @brief 
 *
 */
 typedef struct {
 	Action_POS_t *chassis_pos;          
 	fp32 current_x;														 	
 	fp32 current_y;														
 	fp32 next_mapx;
 	fp32 next_mapy;
  const point_t *chassis_map;		
  const ops_param_t *param;	
 	PathState_t state;
 	Navi_COT_MODE_t Navi_Mode; 
 	 AHRS_Eulr_t ops_imu_pos;
   AHRS_Gyro_t ops_gyro;
 	pid_type_def pid_PosSpeed_x;              
 	pid_type_def pid_PosSpeed_y;	         
 	pid_type_def pid_pos_x_out;
 	pid_type_def pid_pos_x_inner;
 	pid_type_def pid_pos_y_out;
 	pid_type_def pid_pos_y_inner;
 	pid_type_def pid_OutAngle;
 	pid_type_def pid_InnerAngle;
 	pid_type_def path_speed_pid;
 	is_cpt_e POS_IS_CPT;                     
 		 struct 
 	 {
 		fp32 vx;
 		fp32 vy;
 		fp32 yaw_angle;
 	 }final_out;
 }ops_t;
 int8_t Action_init(ops_t *o,const ops_param_t *param,Action_POS_t *pos);
 int8_t go_path(ops_t *o,CMD_ACTION_t *ops_out);
 #endif
--- a/User/Module/Chassis.c
+++ b/User/Module/Chassis.c
@ -1,231 +0,0 @@
 #include "Chassis.h"
 #include "gpio.h"
 #include "Action.h"
 #include "user_math.h"
 #include "bsp_buzzer.h"
 #include "bsp_delay.h"
 /*机器人坐标系，向前x，右y，上yaw
   不同于nuc (前x，左y，上yaw)                          */
 /*          
               x
               |
                 --y
 */
 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; /*模式未改变直接返回*/
  //此处源代码处做了pid的reset 待添加
   c->ctrl =ctrl->C_cmd.type;
   c->mode =ctrl->C_cmd.mode;
   return CHASSIS_OK;
 }    //设置控制模式
 /*该函数用来更新can任务获得的电机反馈值*/
 int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can) {
  if (c == NULL) return CHASSIS_ERR_NULL;
  if (can == NULL) return CHASSIS_ERR_NULL;
  for (uint8_t i = 0; i < 4; i++) 
 {
 	c->motorfeedback.rotor_rpm3508[i] = can->motor.motor3508.as_array[i].rotor_speed;
 	c->motorfeedback.rotor_current3508[i] = can->motor.motor3508.as_array[i].torque_current;
 }
   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_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_ecd;
   c->motorfeedback.rotor_gimbal_pitchrpm = can->motor.chassis6020.as_array[1].rotor_speed;
   c->sick_dis[0] = can->sickfed.raw_dis[0];
   c->sick_dis[1] = can->sickfed.raw_dis[1];
   c->sick_dis[2] = can->sickfed.raw_dis[2];
   c->sick_dis[3] = can->sickfed.raw_dis[3];
  return CHASSIS_OK;
 }
 int8_t Chassis_init(Chassis_t *c,const Chassis_Param_t *param,float target_freq)
 {
   if (c == NULL) return CHASSIS_ERR_NULL;
   c->param = param; 	/*初始化参数 */
 		 for(int i =0 ; i < 4 ; i++)
 	{
 		   PID_init(&(c->pid.chassis_3508VecPID[i]),PID_POSITION_D,&(c->param->M3508_param));  //带D项滤波
 	}
 	    PID_init((&c->pid.chassis_pitAngle6020),PID_POSITION,&(c->param->C6020pitAngle_param));//尝试位置控制角度
 	    PID_init((&c->pid.chassis_pitOmega6020),PID_POSITION,&(c->param->C6020pitOmega_param));
 	    PID_init((&c->pid.chassis_gimbal_yawAnglePID),PID_POSITION,&(c->param->Gimbal_yawAngle_param));//尝试位置控制角度
 	    PID_init((&c->pid.chassis_gimbal_yawOmegaPID),PID_POSITION,&(c->param->Gimbal_yawOmega_param));
 	    PID_init((&c->pid.chassis_gimbal_pitchAnglePID),PID_POSITION,&(c->param->Gimbal_pitchAngle_param));//尝试位置控制角度
 	    PID_init((&c->pid.chassis_gimbal_pitchOmegaPID),PID_POSITION,&(c->param->Gimbal_pitchOmega_param));
 	    PID_init(&(c->pid.chassis_NaviVxPID),PID_POSITION,&(c->param->NaviVx_param));
 	    PID_init(&(c->pid.chassis_NaviVyPID),PID_POSITION,&(c->param->NaviVy_param));
 	    PID_init(&(c->pid.chassis_NaviWzPID),PID_POSITION,&(c->param->NaviVw_param));
 	    PID_init(&(c->pid.sick_CaliforYPID),PID_POSITION,&(c->param->Sick_CaliYparam));
 	    PID_init(&(c->pid.sick_CaliforXPID),PID_POSITION,&(c->param->Sick_CaliXparam));
 	    LowPassFilter2p_Init(&(c->filled[0]),target_freq,80.0f);  //给角加速度做滤波
 	    LowPassFilter2p_Init(&(c->filled[1]),target_freq,80.0f); //给w 做滤波
 	    LowPassFilter2p_Init(&(c->filled[2]),target_freq,80.0f);  //给y做滤波
 	    LowPassFilter2p_Init(&(c->filled[3]),target_freq,80.0f); //给x 做滤波
 		//
 	 return CHASSIS_OK;
 }
 void Chassis_speed_calculate(Chassis_t *c,fp32 Vx,fp32 Vy,fp32 Vw)  //底盘逆运动学的解算
 {		
 		c->hopemotorout.OmniSpeedOut[3] = -Vx+Vy+Vw;//右前
 		c->hopemotorout.OmniSpeedOut[0] = -Vx-Vy+Vw;//右后
 		c->hopemotorout.OmniSpeedOut[1] = Vx-Vy+Vw;//左后
 		c->hopemotorout.OmniSpeedOut[2] = Vx+Vy+Vw;//左前
 }
 //bool isArrive(fp32 real_pos,fp32 target_pos,fp32 mistake)
 //{
 //	uint16_t xArrive = 0, yArrive = 0;
 //	xArrive = abs_float_double(real_pos,target_pos) < mistake ? 1:0;
 //	yArrive = abs_float_double(real_pos,target_pos) < mistake ? 1:0;
 //	if(xArrive && yArrive) return true;
 //	else return false;
 //}
 int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out)
 {
 	if(c ==NULL)  return CHASSIS_ERR_NULL;
 	if(ctrl ==NULL)  return CHASSIS_ERR_NULL;
 	  Chassis_SetCtrl(c,ctrl);
 	 //此处对imu加滤波做修正
 	  c->pos088.bmi088.filtered_gyro.z =LowPassFilter2p_Apply(&(c->filled[0]),c->pos088.bmi088.gyro.z);
 	switch (c->ctrl)
 	{
 	case RC:
 /*     
 	在cmd里对数据进行处理 包括方向和油门	  
 	  6000为全向轮的倍率,遥控器坐标系和机器人坐标系不同*/
 	    c->move_vec.Vw = ctrl->Vw*6000;
 		  c->move_vec.Vx = -ctrl->Vy*6000;
 		  c->move_vec.Vy = ctrl->Vx*6000;
 		if(c->mode == RC_MODE1 ){
 		}
 	    break;
 	case MID_NAVI:
 //		  //这套是全向轮的方向,一定要注意这里的xy方向
 	    c->move_vec.Vw =ctrl->C_navi.wz ;
 	    c->move_vec.Vx =ctrl->C_navi.vy ;
 	    c->move_vec.Vy =ctrl->C_navi.vx ;
 	    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);
 	    c->move_vec.Vw =PID_calc(&(c->pid.chassis_NaviWzPID),c->pos088.bmi088.gyro.z,c->move_vec.Vw);
 	    c->move_vec.Vx =PID_calc(&(c->pid.chassis_NaviVxPID),c->pos088.bmi088.accl.y,c->move_vec.Vx);
 	    c->move_vec.Vy =PID_calc(&(c->pid.chassis_NaviVyPID),c->pos088.bmi088.accl.x,c->move_vec.Vy);
 	   if(ctrl->status[5] ==1)
 		 {
 			c->move_vec.Vw = c->move_vec.Vw * 0.8f;
 			c->move_vec.Vx = c->move_vec.Vx * 0.5f; 
 			c->move_vec.Vy = c->move_vec.Vy * 0.5f; 	 
 		 }
 	    break;
 	}
 /*怎么用*/
 	switch (c->mode)
 	{
 	case RELAXED:
 				c->move_vec.Vx =0;
 				c->move_vec.Vy =0;
 				c->move_vec.Vw =0;
 		break;
 	case NORMAL:
 		 break;
 	case GYRO_STAY://陀螺仪yaw修正
 		   c->move_vec.Vw = c->move_vec.Vw +c->pos088.bmi088.gyro.z *2000;
 	   break;
 }
   //电机速度限幅
 //  abs_limit_fp(&c->move_vec.Vx,2000.0f);
 //  abs_limit_fp(&c->move_vec.Vy,2000.0f);
 //  abs_limit_fp(&c->move_vec.Vw,2000.0f);
  Chassis_speed_calculate(c,c->move_vec.Vx,c->move_vec.Vy,c->move_vec.Vw);
 		for (uint8_t i = 0 ; i <4 ; i++)
 	{
 		 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];
 	}
 //	c->vofa_send[0]=c->pos088.bmi088.gyro.x;
 //	c->vofa_send[1]=c->pos088.bmi088.gyro.y;
 //	c->vofa_send[2]=c->pos088.bmi088.gyro.z;
 //	c->vofa_send[3]=c->pos088.bmi088.accl.x;
 //	c->vofa_send[4]=c->pos088.bmi088.accl.y;
 //	c->vofa_send[5]=c->pos088.bmi088.accl.z;
 	return CHASSIS_OK;
 }
--- a/User/Module/Chassis.h
+++ b/User/Module/Chassis.h
@ -1,232 +0,0 @@
 #ifndef CHASSIS_H
 /**
 *  @brief  底盘控制类头文件
 * 
 *  本头文件定义了底盘控制类的接口和数据结构，用于控制车辆的底盘。
 * 
 *  @param  ChassisImu_t  底盘的 IMU 数据结构体
 *  @param  ops_t  操作数据结构体
 *  @param  Chassis_t  底盘数据结构体
 * 
 *  @brief  底盘控制类的功能包括：
 *          速度控制
 *          方向控制
 *          PID 控制
 *          速度和方向的综合控制
 * 
 *  @attention  PID 控制器的配置需要在 config 文件中设置
 * 
 */
 #define CHASSIS_H
 #include "struct_typedef.h"
 #include "pid.h"
 #include "bmi088.h"
 #include "map.h"
 #include "user_math.h"
 #include "ahrs.h"
 #include "can_use.h"
 #include "cmd.h"
 #include "filter.h"
 #include "Action.h"
 #define CHASSIS_OK (0)      
 #define CHASSIS_ERR (-1)     
 #define CHASSIS_ERR_NULL (-2) 
 #define CHASSIS_ERR_MODE (-3) /*CMD_ChassisMode_t */
 #define CHASSIS_ERR_TYPE (-4) /*Chassis_Type_t */
 //m3508的电机转速转换为底盘的实际速度
 #define M3508_MOTOR_RPM_TO_VECTOR 0.0008809748903494517209f  
 #define M6020_MOTOR_RPM_TO_VECTOR 0.003664f 
 #define PI 3.1415926535f
 typedef struct {
 BMI088_t bmi088;
 /*可通过该枚举类型来决定Imu的数据量纲*/
 enum {
  IMU_DEGREE,//角度值（0-360）
  IMU_RADIAN//弧度制（0-2pi)
 }angle_mode;
 AHRS_Eulr_t imu_eulr;//解算后存放欧拉角（弧度制）
 }ChassisImu_t;
 /*底盘的类型*/
 typedef enum {
  CHASSIS_TYPE_MECANUM,    /* 麦轮 */
  CHASSIS_TYPE_OMNI_CROSS, /* 全向轮*/
  CHASSIS_TYPE_AGV,		/* AGV舵轮 */
 } Chassis_Type_e;
 /*底盘的电机轮组*/
 typedef enum {
     DJI_M3508,
     DJI_G6020,
 	   AGV_Group,
 }Chassis_Motortype_e;
 /* 该结构体用于存取固定的一些参数 在config.c中更改后不再变化 */
 typedef struct
 {
  Chassis_Type_e chassis_type; /*  */
  Chassis_Motortype_e motor_type; /**/
 	/*该部分决定PID的参数整定在config中修改*/
  pid_param_t M3508_param;	
  pid_param_t AngleCor_param;
  pid_param_t OmegaCor_param; 
  pid_param_t DisCamera_param;    
  pid_param_t ImuCor_param;
  pid_param_t C6020pitAngle_param;
  pid_param_t C6020pitOmega_param;	
  pid_param_t Gimbal_yawAngle_param;
  pid_param_t Gimbal_yawOmega_param;	
  pid_param_t Gimbal_pitchAngle_param;
  pid_param_t Gimbal_pitchOmega_param;	
  pid_param_t NaviVx_param;
  pid_param_t NaviVy_param;
  pid_param_t NaviVw_param;	
 	pid_param_t Sick_CaliYparam;
 	pid_param_t Sick_CaliXparam;
 }Chassis_Param_t;
 /*该结构体用于底盘的期望运动向量*/
 typedef struct 
 {
   fp32 Vx;
   fp32 Vy;
   fp32 Vw;
 	fp32 mul;//油门倍率
 }ChassisMove_Vec;
 /**
 *  @brief  
 * 
 */
 typedef struct{
   uint8_t chassis_task_run; //线程的运行
   const	Chassis_Param_t *param;  //一些固定的参数
    ChassisImu_t pos088; //088的实时姿态
    Action_POS_t Action_pos;
    CMD_Chassis_CtrlType_e ctrl;            
    CMD_Chassis_mode_e mode;                 
    ChassisMove_Vec move_vec;         //由控制任务决定
    struct{
 	  fp32 rotor_rpm3508[4];
 	  fp32 rotor_current3508[4];
 	  fp32 rotor_pit6020angle;
 	  fp32 rotor_pit6020rpm;
 	  fp32 rotor_gimbal_yawangle;
 	  fp32 rotor_gimbal_yawrpm;
 	  fp32 rotor_gimbal_pitchangle;
 	  fp32 rotor_gimbal_pitchrpm;
 	}motorfeedback;
 	/*期望的底盘输出值*/
 	struct{
 	fp32 OmniSpeedOut[4];
 	}hopemotorout;
 	/*经PID计算后的实际发送给电机的实时输出值*/
    struct 
 	{
 		fp32 final_3508out[4];
 		fp32 final_pitchout;
 		fp32 final_gimbal_yawout;
 		fp32 final_gimbal_pitchout;
 	}final_out;
 	struct{                
 	pid_type_def chassis_3508VecPID[4];
 	pid_type_def chassis_pitAngle6020;
 	pid_type_def chassis_pitOmega6020;
 	pid_type_def chassis_gimbal_yawAnglePID;
 	pid_type_def chassis_gimbal_yawOmegaPID;
 	pid_type_def chassis_gimbal_pitchAnglePID;
 	pid_type_def chassis_gimbal_pitchOmegaPID;
 	pid_type_def chassis_NaviVxPID;
 	pid_type_def chassis_NaviVyPID;
 	pid_type_def chassis_NaviWzPID;
 	pid_type_def sick_CaliforYPID;
 	pid_type_def sick_CaliforXPID;
 		pid_type_def Action_VxPID;
 		pid_type_def Action_VyPID;
 		pid_type_def Action_WzPID;
 	}pid;
   fp32 vofa_send[8];
 	 LowPassFilter2p_t filled[6]; /* 输出滤波器滤波器数组 */
 	int32_t sick_dis[4];   //获取到的sick激光值
 }Chassis_t;
 /**
 * @brief 
 *
 * @param c 
 * @param param 
 * @param mech_zero 
 * @param wheelPolar 
 * @return 
 */
 int8_t Chassis_init(Chassis_t *c,const Chassis_Param_t *param,float target_freq);
 /**
 * \brief 
 */
 int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can);
 /**
 * \brief 
 */
 int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out);
 /// @brief 
 /// @param c 
 void vesc_current_detection(Chassis_t *c);
 #endif
--- a/User/Module/config.c
+++ b/User/Module/config.c
@ -46,6 +46,20 @@ static const ConfigParam_t param_chassis ={
    .i_limit = 2000.0f,
    .out_limit = 3000.0f,
 },
 .M3508_angle_param = {  
    .p = 30.0f,        
    .i = 0.0f,         
    .d = 1.5f,         
    .i_limit = 1000.0f,
    .out_limit = 3000.0f,
 },
 .M3508_speed_param = {  
    .p = 5.0f,         
    .i = 0.3f,         
    .d = 0.0f,         
    .i_limit = 2000.0f,
    .out_limit = 3000.0f,
 },
 .UP_GM6020_angle_param={
 			.p = 30.0f,
 			.i = 20.0f,
@ -71,102 +85,13 @@ static const ConfigParam_t param_chassis ={
    .rev = 0,
 	  .T=0.1,
 	  .W=0.1,
-	  .K_P=0.1,
+	  .K_P=0.2,
-	  .K_W=0.1,
+	  .K_W=0.05,
 }
 	},
 //   .chassis = {/**/
 //    .C6020pitAngle_param = {
 //			.p = 15.0f,
 //			.i = 0.3f,
 //			.d =0.0f,
 //			.i_limit = 200.0f,
 //			.out_limit = 3000.0f,
 //		},
 //		.C6020pitOmega_param = {
 //			.p =30.0f,
 //			.i =0.3f,
 //			.d =0.0f,
 //			.i_limit = 200.0f,
 //			.out_limit = 3000.0f				
 //		},
 //		
 //		.Gimbal_yawAngle_param = {
 //			.p =8.0f,
 //			.i =0.0f,
 //			.d =0.0f,
 //			.i_limit = 200.0f,
 //			.out_limit = 3000.0f
 //		},
 //		
 //		.Gimbal_yawOmega_param = {
 //			.p =18.0f,
 //			.i =0.0f,
 //			.d =0.0f,
 //			.i_limit = 200.0f,
 //			.out_limit = 3000.0f
 //		},
 //		
 //		.Gimbal_pitchAngle_param = {
 //			.p =8.0f,
 //			.i =0.0f,
 //			.d =0.0f,
 //			.i_limit = 200.0f,
 //			.out_limit = 3000.0f
 //		},
 //		
 //		.Gimbal_pitchOmega_param = {
 //			.p =18.0f,
 //			.i =0.0f,
 //			.d =0.0f,
 //			.i_limit = 200.0f,
 //			.out_limit = 3000.0f
 //		},
 //     .AngleCor_param = {
 //			.p =0.8f,
 //			.i =0.0f,
 //			.d =1.0f,
 //			.i_limit = 0.0f,
 //			.out_limit =5000.0f,
 //     },
 //		 
 //     .OmegaCor_param = {
 //			.p =23.5f,
 //			.i =0.0f,
 //			.d =0.05f,
 //			.i_limit = 0.0f,
 //			.out_limit =5000.0f,
 //     },
 //		 
 //   .ImuCor_param = {
 //	    .p =95.0f,
 //	 		.i =0.0f,
 //			.d =0.0f,
 //			.i_limit = 0.0f,
 //			.out_limit =200.0f,
 //    }, 
 //	 
 //	 .DisCamera_param = {
 //       .p =80.0f,
 //		   .i =0.1f,
 //		   .d =0.0f,
 //		   .i_limit = 0.0f,
 //		   .out_limit =5000.0f,
 //	 },
 //	 	 .M3508_param = {
 //		 .p = 15.1f,
 //		 .i = 0.02f,
 //		 .d = 3.2f,
 //		 .i_limit = 200.0f,
 //		 .out_limit =6000.0f,
 //	 }
 //		 
 //   },
   .can = {
--- a/User/Module/config.h
+++ b/User/Module/config.h
@ -1,14 +1,12 @@
 #ifndef _CONFIG_H
 #define _CONFIG_H
 #include "Chassis.h"
 #include "can_use.h"
 #include "ahrs.h"
 #include "map.h"
 #include "up.h"
 typedef struct{
 Chassis_Param_t chassis; /**/
 UP_Param_t up;
 CAN_Params_t can;
 AHRS_Eulr_t mech_zero[4];
@ -27,8 +25,6 @@ void Config_Get(Config_t *cfg);
 void Config_Set(Config_t *cfg);
 //void set_ops_path(ConfigParam_t *config, const point_t *path, int8_t path_num) ;
 extern const ConfigParam_t *Config_ChassisGet(void);
--- a/User/Module/up.c
+++ b/User/Module/up.c
@ -4,102 +4,150 @@
 #include "bsp_buzzer.h"
 #include "bsp_delay.h"
-#define GEAR_RATIO         (36)   // 2006减速比
+#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编码值转轴角度
+#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编码值转轴角度
 int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
 {
-	 u->param = param; 	/*初始化参数 */
+	u->param = param; 	/*初始化参数 */
-   
+	/*go电机初始化*/
 	GO_M8010_init();
 	/*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 .M2006_angle ,PID_POSITION ,&(u->param->M2006_angle_param  ));
 	PID_init (&u->pid .M2006_speed ,PID_POSITION ,&(u->param->M2006_speed_param ));
 	PID_init (&u->pid .M3508_angle ,PID_POSITION ,&(u->param->M3508_angle_param  ));
 	PID_init (&u->pid .M3508_speed ,PID_POSITION ,&(u->param->M3508_speed_param ));
 	PID_init (&u->pid .GM6020_speed,PID_POSITION ,&(u->param ->UP_GM6020_speed_param ));
 	PID_init (&u->pid .GM6020_angle,PID_POSITION ,&(u->param ->UP_GM6020_angle_param ));
-	
+		
-	u->M2006 .motor =M2006 ;
+
-	u->M3508 .motor =M3508 ;
+	for(int i=0;i<2;i++){		 //go初始位置设置为0 
 	for(int i=0;i<3;i++){
 	PID_init (&u->pid .M3508_speed[i] ,PID_POSITION ,&(u->param ->M3508_speed_param ));
 	    }
 			for(int i=0;i<2;i++){		 //go初始位置设置为0 
 			u->GO_motor_info[i] = getGoPoint(i);
 	    GO_M8010_send_data(BSP_UART_GetHandle(BSP_UART_RS485), i,u->param->go_param .rev ,u->param->go_param .T ,u->param->go_param .W ,0,u->param->go_param .K_P ,u->param->go_param .K_W );		
 	}
 	/**/
 	u->state .Dribble_flag =Not_started_dri;
 	u->state. Pitch_flag=Not_started_Pit;
 	u->state .last_state = Not_started_dri;
 }
-
+/*can，上层状态更新*/
-int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can) {
+int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *c) {
-
+		for(int i=0;i<2;i++){		     //go初始位置设置为0 
-	u->motorfeedback .M2006_angle=can ->motor .motor3508 .as_array [0].rotor_ecd ;
+	  u->motorfeedback .GO_motor_info [i] = getGoPoint(i);
-	u->motorfeedback .M2006_rpm =can ->motor .motor3508 . as_array [0].rotor_speed  ;
+	}
 	u->motorfeedback .M2006 .motor =M2006;
 	u->motorfeedback .M3508 .motor =M3508;
 	u->motorfeedback .VESC_5065_M1_rpm =can ->motor .chassis5065 .as_array [0].rotor_speed ;
 	u->motorfeedback .VESC_5065_M2_rpm =can ->motor .chassis5065 .as_array [1].rotor_speed ;
 	u->motorfeedback .rotor_pit6020ecd =can ->motor .chassis6020.as_array [2].rotor_ecd ;
 	u->motorfeedback .rotor_pit6020rpm =can ->motor .chassis6020.as_array [2].rotor_speed  ;
-	for(int i=0;i<3;i++){
+	
-	u->motorfeedback .M3508_speed[i] =can ->motor .motor3508 .as_array [i+1].rotor_speed ;
+	for(int i=0;i<4;i++){
 	u->motorfeedback .M3508_rpm[i] =can ->motor .motor3508 .as_array [i].rotor_speed ;
 	u->motorfeedback .M3508_angle [i]=can ->motor .motor3508 .as_array [i].rotor_ecd ;
 	}
 	u->cmd =c;
 	return 0;
 }
 	 int8_t cnt=0;
-/*上层电机控制*/
+/*上层电机控制,使用can1的id1和2*/
-int8_t UP_angle(UP_t *u, fp32 target_angle) {
+int8_t UP_angle_control(UP_t *u, fp32 target_angle,MotorType_t motor) {
    // 获取当前编码器角度
-	
+	fp32 angle ,delta;
-//	switch (u->)
+	switch(motor)
-    float angle = u->motorfeedback.M2006_angle;
+	{
-    // 初始化阶段：前50次循环记录初始值
+		case M2006 :  			
-    if (u->M2006.init_cnt < 50) {
+    angle = u->motorfeedback.M3508_angle[0]; 
-        u->M2006.orig_angle = angle;    // 记录初始编码器值
+    if (u->motorfeedback .M2006 .init_cnt < 50) {
-        u->M2006.last_angle = angle;    
+        u->motorfeedback .M2006.orig_angle = angle;    // 记录初始编码器值
        u->motorfeedback .M2006.last_angle = angle;    
-        u->M2006.init_cnt++;            // 初始化计数器递增
+        u->motorfeedback .M2006.init_cnt++;            // 初始化计数器递增
        return 0;                       
    }
-
+		
-    
+		    delta = angle - u->motorfeedback .M2006.last_angle;
-    float delta = angle - u->M2006.last_angle;
+	    	delta = angle - u->motorfeedback .M2006.last_angle;
    if (delta > 4096) {
-        u->M2006.round_cnt--;           // 逆时针跨圈
+        u->motorfeedback .M2006.round_cnt--;           // 逆时针跨圈
    } else if (delta < -4096) {
-        u->M2006.round_cnt++;           // 顺时针跨圈
+        u->motorfeedback .M2006.round_cnt++;           // 顺时针跨圈
    }
-
+        u->motorfeedback.M2006.last_angle = angle;
-    u->M2006.last_angle = angle;
+		    // 计算总角度
-
+    float total_angle = (u->motorfeedback.M2006 .round_cnt * 8191 + (angle - u->motorfeedback.M2006.orig_angle)) * MOTOR2006_ECD_TO_ANGLE;
-    // 计算总角度
+    u->motorfeedback.M2006.total_angle =total_angle;
    float total_angle = (u->M2006.round_cnt * 8191 + (angle - u->M2006.orig_angle)) * MOTOR2006_ECD_TO_ANGLE;
    u->M2006 .total_angle =total_angle;
    float delta_angle = PID_calc(&u->pid.M2006_angle, total_angle, target_angle);
-    float delta_speed = PID_calc(&u->pid.M2006_speed, u->motorfeedback.M2006_rpm, delta_angle);
+    float delta_speed = PID_calc(&u->pid.M2006_speed, u->motorfeedback.M3508_rpm [0], delta_angle);
    u->motor_target.M2006_angle = target_angle;
-    u->final_out .final_2006out =delta_speed;
+    u->final_out .final_3508out [0] =delta_speed;
 		break ;
 		case M3508 : 
    angle = u->motorfeedback.M3508_angle[1]; 
    if (u->motorfeedback .M3508 .init_cnt < 50) {
        u->motorfeedback .M3508.orig_angle = angle;    // 记录初始编码器值
        u->motorfeedback .M3508.last_angle = angle;    
        u->motorfeedback .M3508.init_cnt++;            // 初始化计数器递增
        return 0;                       
    }
 		    delta = angle - u->motorfeedback .M3508.last_angle;
 	    	delta = angle - u->motorfeedback .M3508.last_angle;
    if (delta > 4096) {
        u->motorfeedback .M3508.round_cnt--;           // 逆时针跨圈
    } else if (delta < -4096) {
        u->motorfeedback .M3508.round_cnt++;           // 顺时针跨圈
    }
        u->motorfeedback.M3508.last_angle = angle;
 		    // 计算总角度
     total_angle = (u->motorfeedback.M3508 .round_cnt * 8191 + (angle - u->motorfeedback.M3508.orig_angle)) * MOTOR2006_ECD_TO_ANGLE; 
    u->motorfeedback.M3508.total_angle =total_angle;
     delta_angle = PID_calc(&u->pid.M3508_angle , total_angle, target_angle);
     delta_speed = PID_calc(&u->pid.M3508_speed , u->motorfeedback.M3508_rpm [1], delta_angle);
    u->motor_target.M3508_angle  = target_angle;
    u->final_out .final_3508out[1] =delta_speed;
 		break ;  
 	}
    return 0;
 }
-int8_t UP_M3508_speed(UP_t *u,fp32 speed)
+//int8_t UP_M3508_speed(UP_t *u,fp32 speed)
-{
+//{
-	u->motor_target .M3508_speed =speed;
+//	u->motor_target .M3508_speed [] =speed;
-	for(int i=0;i<3;i++){
+//	for(int i=0;i<3;i++){
-	    u->final_out .final_3508out [i] =
+//	    u->final_out .final_3508out [i] =
-	        PID_calc (&(u->pid .M3508_speed[i] ),u->motorfeedback .M3508_speed [i],speed );	
+//	        PID_calc (&(u->pid .M3508_speed[i] ),u->motorfeedback .M3508_speed [i],speed );	
-	}
+//	}
-	
+//	
-}
+//}
 int8_t VESC_M5065_Control(UP_t *u,fp32 speed)
@ -107,16 +155,17 @@ 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_M1out =-u->motor_target .VESC_5065_M1_rpm;
-  u->final_out .final_VESC_5065_M2out =-u->motor_target .VESC_5065_M2_rpm;
+  u->final_out .final_VESC_5065_M2out =u->motor_target .VESC_5065_M2_rpm;
 }
 int8_t GM6020_control(UP_t *u,fp32 angle)
 {	
 	fp32 delat_speed;
-	Clip(&angle,90,270);
+//	Clip(&angle,90,270);
 	delat_speed =
 	PID_calc (&(u->pid .GM6020_angle ),u->motorfeedback .rotor_pit6020ecd ,(angle /360*8191));
@ -124,6 +173,7 @@ int8_t GM6020_control(UP_t *u,fp32 angle)
 	PID_calc (&(u->pid .GM6020_speed  ),u->motorfeedback.rotor_pit6020rpm ,delat_speed);
 	u->motor_target .rotor_pit6020angle =angle ;
 }
 /*go电机控制*/
 int8_t GO_SendData(UP_t *u,int id,float pos)
 {
@ -133,35 +183,71 @@ int8_t GO_SendData(UP_t *u,int id,float pos)
 }
 int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
 {
 	//电机控制	，传进can
 	out ->motor3508 .as_array [0]=u->final_out .final_2006out ;
-	for(int i=1;i<4;i++){
+	for(int i=0;i<4;i++){
-	out ->motor3508 .as_array[i]=u->final_out.final_3508out [i-1] ;
+	out ->motor3508 .as_array[i]=u->final_out.final_3508out [i] ;
 	}
-	out ->chassis5065 .erpm [0]=u->final_out .final_VESC_5065_M1out ;
+	out ->chassis5065 .erpm [0]=  u->final_out .final_VESC_5065_M1out ;
-	out ->chassis5065 .erpm [1]=u->final_out .final_VESC_5065_M2out ;
+	out ->chassis5065 .erpm [1]=  -u->final_out .final_VESC_5065_M2out ;
 	out ->chassis6020 .as_array [2]=u->final_out .final_pitchout ;
 }
 int8_t UP_control(UP_t *u,CAN_Output_t *out)
 {
-//	if(u ==NULL)  return 0;
+	
-//	if(u ==NULL)  return 0;
+	if(u ==NULL)  return 0;
-//   UP_M2006_angle(u,2500);
+	if(u ==NULL)  return 0;
-	switch (u->ctrl)
+	
-	{
+ 
-		case STATE_IDLE         :			     break ;
+			
-    case STATE_PRE_DRIBBLE  :	         break ;
+//  switch(u->flag ){
-    case STATE_POST_DRIBBLE :	         break ;
+//		case 0 :   
-    case STATE_PRE_LAUNCH   : 	       break ;
+//			GO_SendData(u, 0,0);
-    case STATE_POST_LAUNCH  :  	       break ;		
+//			UP_angle_control(u, 0,M2006 );
-	}
+
 //		
 //		/*发射过程*/
 //		case 1 : 
 //			UP_angle_control(u, -130,M2006 );
 //			
 //			GO_SendData(u, 0,-1950);
 //		break ;  
 //			case 2 : 
 //			UP_angle_control(u, 0,M2006 );  
 //			
 //			if((u->motorfeedback .M2006 .total_angle )<10)
 //			
 //			GO_SendData(u, 0,0);
 //		break ;  
 //	
 //	
 //	}
 }
--- a/User/Module/up.h
+++ b/User/Module/up.h
@ -5,26 +5,47 @@
 #include "struct_typedef.h"
 #include "pid.h"
 #include "bmi088.h"
 #include "map.h"
 #include "user_math.h"
 #include "ahrs.h"
 #include "can_use.h"
 #include "cmd.h"
 #include "filter.h"
 #include "Action.h"
 #include "chassis.h" 
 #include "vofa.h"	
 #include "GO_M8010_6_Driver.h"
-	
+#include "bsp_usart.h"
-typedef enum {
+
    STATE_IDLE,          // 空闲状态
    STATE_PRE_DRIBBLE,   // 运球前
    STATE_POST_DRIBBLE,  // 运球后
    STATE_PRE_LAUNCH,    // 发射前
    STATE_POST_LAUNCH    // 发射后
 } OperationState;
 typedef enum
 {
 	 	  Not_started_Pit=0,//未开始
 	    Launch_Ready=1, //准备发射
 	    Launch_complete=2,//发射完成
 		  Done_Pit=3       //已完成
 }Pitch_flag_t;
 typedef	enum{
 	  Not_started_dri=0,//未开始
 	  No_ball =1,  //抓上无球    
    Have_ball_F=2,  //刚开始有球
    Have_ball_S=3,  //中途有球
 	  Done_dri =4         //已完成
 }Dribble_flag_t;
 /*运行控制中的控制*/
 typedef struct{
 	/*投球过程*/	
 	Pitch_flag_t Pitch_flag;
 	/*运球过程*/
 	Dribble_flag_t Dribble_flag;
 	int last_state;
 } Oper_control_state_t;
 typedef struct {
@ -52,17 +73,18 @@ typedef struct {
 	float K_W;
 }GO_param_t;
-
+/*角度环控制电机类型*/
 typedef enum {
    M2006 = 1,
    M3508
 } MotorType_t;
 typedef struct
 {	
-	/*该部分决定PID的参数整定在config中修改*/
+
 	pid_param_t VESC_5065_M1_param;
 	pid_param_t VESC_5065_M2_param;
@ -73,6 +95,8 @@ typedef struct
 	pid_param_t M2006_angle_param;	
 	pid_param_t M3508_speed_param;	
 	pid_param_t M3508_angle_param;	
 	GO_param_t go_param;
 }UP_Param_t;
@ -80,6 +104,7 @@ typedef struct
 typedef struct
 {		
        MotorType_t motor;
        float orig_angle;     
        float last_angle;     
        int32_t round_cnt;    
@ -93,26 +118,30 @@ typedef struct{
   uint8_t up_task_run; 
   const	UP_Param_t *param;  
 	 UP_Imu_t pos088; 
-	OperationState ctrl;
+	 UP_Imu_t pos088; 
- motor_angle_data M2006;
+	  
- motor_angle_data M3508;
+	/*控制及状态*/
-	GO_Motorfield *GO_motor_info[GO_NUM];//存放go电机数据
+	 CMD_t *cmd;	
-		
+	 Oper_control_state_t state;//上层机构的运行状态
-	 struct{
+
-					
+	 struct{					
 	  fp32 rotor_pit6020ecd;
 	  fp32 rotor_pit6020rpm;
 	  fp32 VESC_5065_M1_rpm;
 	  fp32 VESC_5065_M2_rpm;
-	  fp32 M2006_rpm;
+    motor_angle_data M2006;
-	  fp32 M2006_angle;
+		motor_angle_data M3508;
-		fp32 M3508_speed[3];
+		GO_Motorfield *GO_motor_info[GO_NUM];//存放go电机数据
 	  fp32 M3508_angle[4];
 	  fp32 M3508_rpm  [4];
 		 int flag;
 	}motorfeedback;
@ -125,7 +154,10 @@ typedef struct{
 	  fp32 VESC_5065_M2_rpm;
 	  fp32 M2006_angle;
-		fp32 M3508_speed;
+		fp32 M3508_angle;
 		fp32 go_shoot;
 		fp32 go_spin;
 	}motor_target;
@ -133,33 +165,35 @@ typedef struct{
 		 pid_type_def GM6020_speed;
 		 pid_type_def GM6020_angle;
 		 pid_type_def VESC_5065_M1;
 		 pid_type_def VESC_5065_M2;
 		 pid_type_def M2006_angle;
 		 pid_type_def M2006_speed;
-		 pid_type_def M3508_speed[3];
+		 
 		  pid_type_def M3508_angle;
 		  pid_type_def M3508_speed;
 	}pid;
 		/*经PID计算后的实际发送给电机的实时输出值*/
    struct 
 	{
-		fp32 final_2006out;
+		fp32 final_3508out[4];
 		fp32 final_3508out[3];
 		fp32 final_pitchout;
 		fp32 final_VESC_5065_M1out;
 		fp32 final_VESC_5065_M2out;
 	}final_out;
 	 LowPassFilter2p_t filled[6]; /* 输出滤波器滤波器数组 */
 	   fp32 vofa_send[8];
 }	UP_t;
@ -167,11 +201,11 @@ typedef struct{
 int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq);
-int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can) ;
+int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *c) ;
 int8_t GM6020_control(UP_t *u,fp32 angle);
 int8_t VESC_M5065_Control(UP_t *u,fp32 speed);
 int8_t GO_SendData(UP_t *u,int id,float pos);
-int8_t UP_angle(UP_t *u, fp32 target_angle) ;
+int8_t UP_angle_control(UP_t *u, fp32 target_angle,MotorType_t motor);
 int8_t UP_control(UP_t *u,CAN_Output_t *out);
 int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out);
 int8_t UP_M2006_angle(UP_t *u,fp32 target_angle);
--- a/User/bsp/bsp_gpio.c
+++ b/User/bsp/bsp_gpio.c
@ -1,5 +1,5 @@
 /* Includes ----------------------------------------------------------------- */
-#include "bsp\gpio.h"
+#include "bsp\bsp_gpio.h"
 #include <gpio.h>
 #include <main.h>
--- a/User/bsp/bsp_gpio.h
+++ b/User/bsp/bsp_gpio.h
--- a/User/bsp/bsp_spi.c
+++ b/User/bsp/bsp_spi.c
@ -1,5 +1,5 @@
 /* Includes ----------------------------------------------------------------- */
-#include "bsp\spi.h"
+#include "bsp\bsp_spi.h"
 /* Private define ----------------------------------------------------------- */
 /* Private macro ------------------------------------------------------------ */
--- a/User/bsp/bsp_spi.h
+++ b/User/bsp/bsp_spi.h
--- a/User/bsp/bsp_usart.c
+++ b/User/bsp/bsp_usart.c
@ -8,8 +8,8 @@ static void (*UART_Callback[BSP_UART_NUM][BSP_UART_CB_NUM])(void);
 static BSP_UART_t UART_Get(UART_HandleTypeDef *huart) {
  if (huart->Instance == USART3)
    return BSP_UART_REMOTE;
-  else if (huart->Instance == USART1)
+//  else if (huart->Instance == USART1)
-    return BSP_UART_NUC;
+//    return BSP_UART_RS485;
  else if (huart->Instance == USART6)
    return BSP_UART_RS485;
  /*
--- a/User/bsp/protocol.h
+++ b/User/bsp/protocol.h
@ -28,7 +28,7 @@ typedef struct __attribute__((packed)) {
 	int remain;
 } Protocol_UpDataMCU_t;
-/* 视觉 -> 电控  底盘数据结构体*/
+/* 视觉 -> 电控  数据结构体*/
 typedef struct __attribute__((packed)) {
  Protocol_ID_t recv_id;//作为帧头使用确认通信ID正确
--- a/User/device/Action.c
+++ b/User/device/Action.c
@ -1,203 +0,0 @@
 /**
 * 东北大学ACTION码盘驱动
 *
 * 本驱动采用ops_9定位系统，负责收发码盘的原始数据，并在任务中解析出相应的位置坐标。
 * 解析后的位置坐标将被交给导航处理模块，生成期望的运动向量。
 *
 * 使用手册：
 * 请参阅相关文档了解如何使用本驱动。
 *
 * 注意：
 * 本驱动仅适用于东北大学ACTION码盘。
 *删去Action_HandleOffline错误处理函数中对结构体清0的函数
 */
 /* Includes ----------------------------------------------------------------- */
 #include "tim.h"
 #include "Action.h"
 #include <string.h>
 static osThreadId_t thread_alert;
 uint8_t rxbuf[RS232_FRAME_LENGTH];
 static void Ops10msTimerCallback(void *arg){
  (void)arg;
  osThreadFlagsSet(thread_alert,SIGNAL_OPSTIMER_REDY);
 }
 /* Private function  -------------------------------------------------------- */
 static void ACTION_IdleCallback(void) {	
 	osThreadFlagsSet(thread_alert,SIGNAL_ACTION_RAW_REDY);
 }
 /* Exported functions ------------------------------------------------------- */
 int8_t ACTIONRECV_Init(Action_POS_t *pos){
 	if(pos == NULL) return DEVICE_ERR_NULL;
 	 pos->Action_ready =0;//码盘校准标志位初始化
   if((thread_alert = osThreadGetId()) == NULL ) return DEVICE_ERR_NULL;
   pos->action_timer_Id =
      osTimerNew(Ops10msTimerCallback, osTimerPeriodic,NULL,NULL);
 	osTimerStart(pos->action_timer_Id,10);//此处ticks 理解为1个tick为1ms 此处为每10ms触发一次回调函数   
   BSP_UART_RegisterCallback(BSP_UART_ACTION,BSP_UART_IDLE_LINE_CB,
                            ACTION_IdleCallback);
   return DEVICE_OK;
 }
 int8_t ACTION_StartReceiving() {
  if (HAL_UARTEx_ReceiveToIdle_DMA(BSP_UART_GetHandle(BSP_UART_ACTION),
                           (uint8_t *)rxbuf,
                           sizeof(rxbuf)) == HAL_OK)
    return DEVICE_OK;
  return DEVICE_ERR;
 }
 bool_t ACTION_WaitDmaCplt(void) {
  return(osThreadFlagsWait(SIGNAL_ACTION_RAW_REDY, osFlagsWaitAll,400) ==
          SIGNAL_ACTION_RAW_REDY);   
 }
 /* *
 */
 int8_t ACTION_Parse(Action_POS_t *pos)
 {
  if (pos == NULL) return DEVICE_ERR_NULL;
  static union
  {
    fp32 pos_data[3];
    char rxbuff[12];
  } pos_rxbuf;
  if (rxbuf[0] == 0x0D && rxbuf[1] == 0x0A)
  {
    if (rxbuf[26] == 0x0A && rxbuf[27] == 0x0D)
    {
      pos_rxbuf.rxbuff[0]=rxbuf[2];
      pos_rxbuf.rxbuff[1]=rxbuf[3];
      pos_rxbuf.rxbuff[2]=rxbuf[4];
      pos_rxbuf.rxbuff[3]=rxbuf[5];
      pos_rxbuf.rxbuff[4]=rxbuf[14];
      pos_rxbuf.rxbuff[5]=rxbuf[15];
      pos_rxbuf.rxbuff[6]=rxbuf[16];
      pos_rxbuf.rxbuff[7]=rxbuf[17];
      pos_rxbuf.rxbuff[8]=rxbuf[18];
      pos_rxbuf.rxbuff[9]=rxbuf[19];
      pos_rxbuf.rxbuff[10]=rxbuf[20];
      pos_rxbuf.rxbuff[11]=rxbuf[21];
      // 数据解析
      pos->pos_yaw = pos_rxbuf.pos_data[0];
     // 按照安装方向决定正负号
      pos->pos_x = pos_rxbuf.pos_data[1];
      pos->pos_y = pos_rxbuf.pos_data[2];
      pos ->Action_ready =1;//码盘校准完成
    }
  }
  else
  {
    return -1;
  }
  return DEVICE_OK;
 }
 //该函数用来计算速度(利用获取的位置来计算相应的速度)
 //此处获取的是真实的位置(mm)后每10ms做一次微分处理
 int8_t ACTION_DataRefresh(Action_POS_t *pos){
 	if (pos == NULL) return DEVICE_ERR_NULL;
  if (osThreadFlagsGet() & SIGNAL_OPSTIMER_REDY){
 	   osThreadFlagsClear(SIGNAL_OPSTIMER_REDY);
 	   		//计算每毫秒的速度 该函数每10ms调用
 		pos->pos_Vx = (pos->pos_x - pos->pos_lastX) / 10;
 		pos->pos_Vy = (pos->pos_y - pos->pos_lastY) / 10;
 		pos->pos_lastX = pos->pos_x;
 		pos->pos_lastY = pos->pos_y;
  } 
 	return DEVICE_OK;
 }
 int8_t Action_HandleOffline(Action_POS_t *pos) {
  if (pos == NULL) return DEVICE_ERR_NULL;
  (void)pos;
 //  memset(pos, 0, sizeof(*pos));
  return 0;
 }
 /* 将字符串拼接 */
 void Strcat(char str1[], char str2[], uint8_t num)
 {
    int i = 0, j = 0;
    while (str1[i] != '\0')
        i++;
    for (j = 0; j < num; j++)
    {
        str1[i++] = str2[j];
    }
 }
 /* 该部分函数用于码盘的重定位系统 */
 /* ----------------------------- */
 /* 手动标定，用于校正 */
 void ACT_Calibration(void)
 {
    HAL_UART_Transmit(&huart1, (uint8_t *)"ACTR", 4, 100);
 }
 /* 清零 */
 /* 将当前位置设置为 (0, 0) */
 void ACT_ZeroClear(void)
 {
    HAL_UART_Transmit(&huart1, (uint8_t *)"ACT0", 4, 100);
 }
 /* 更新 XY 坐标 */
 /* 将当前位置更新为指定的 X 和 Y */
 void ACT_UpdateXY(float pos_x, float pos_y)
 {
    char update_xy[12] = "ACTD"; // 命令前缀为 "ACTD"
    static union
    {
        float XY[2];    // 两个浮点数表示 X 和 Y 坐标
        char data[8];   // 将浮点数视为字节数据
    } set;
    set.XY[0] = pos_x;
    set.XY[1] = pos_y;
    Strcat(update_xy, set.data, 8);
    HAL_UART_Transmit(&huart1, (uint8_t *)update_xy, sizeof(update_xy), 100);
 }
--- a/User/device/Action.h
+++ b/User/device/Action.h
@ -1,59 +0,0 @@
 #ifndef _ACTION_H_
 #define _ACTION_H_
 #define RS232_FRAME_LENGTH 28u
 #define REF_LEN_RX_BUFF  0xFF
 #include <cmsis_os2.h>
 #include "device.h"
 #include "bsp_usart.h"
 #include "cmd.h"
 typedef struct 
 {
  osTimerId_t action_timer_Id;
  /*原始数据*/
  fp32 pos_x;
  fp32 pos_y;
  fp32 pos_yaw;  // 航向角速度
  /*处理数据*/
  fp32 pos_Vx;
  fp32 pos_Vy;
  fp32 pos_lastX;
  fp32 pos_lastY;
 	int Action_ready;//码盘校准标志位
 }Action_POS_t;
 int8_t ACTIONRECV_Init(Action_POS_t*pos);
 int8_t ACTION_StartReceiving(void);
 bool_t ACTION_WaitDmaCplt(void);
 int8_t ACTION_Parse(Action_POS_t*pos);
 int8_t ACTION_DataRefresh(Action_POS_t*pos);
 int8_t Action_HandleOffline(Action_POS_t *pos);
 void ACT_UpdateXY(float pos_x,float pos_y);
 void ACT_ZeroClear(void);
 #endif
--- a/User/device/GO_M8010_6_Driver.c
+++ b/User/device/GO_M8010_6_Driver.c
@ -38,21 +38,13 @@ void GO_M8010_init (void){
 //暂存接收数据
-uint8_t Temp_buffer[16];
+static uint8_t Temp_buffer[16];
 void USART6_RxCompleteCallback(void ) 
 {
 	 UART_HandleTypeDef *huart6 = BSP_UART_GetHandle(BSP_UART_RS485);
    uint16_t crc = CRC16_Calc(Temp_buffer,sizeof(Temp_buffer)-2,0x0000);
    if ((Temp_buffer[14] != (crc&0xFF)) || (Temp_buffer[15] != ((crc>>8) & 0xFF)))
    { 
 //        HAL_GPIO_WritePin(LED_B_GPIO_Port, LED_B_Pin, GPIO_PIN_SET);    //蓝色灯亮      
 //        HAL_GPIO_WritePin(LED_R_GPIO_Port, LED_R_Pin, GPIO_PIN_RESET);      //红色灯灭
        return;
    }
 //    HAL_GPIO_WritePin(GPIOH, GPIO_PIN_11, GPIO_PIN_SET);                 // indicate CRC correct
 //    HAL_GPIO_WritePin(LED_R_GPIO_Port, LED_R_Pin, GPIO_PIN_RESET);      //红色灯灭
    GO_M8010_recv_data(Temp_buffer);
 }
--- a/User/device/LD_remote.c
+++ b/User/device/LD_remote.c
@ -1,176 +0,0 @@
 /*
 乐迪遥控器
 */
 /* Includes ----------------------------------------------------------------- */
 #include "LD_remote.h"
 #include <string.h>
 #include "bsp_usart.h"
 #include "error_detect.h"
 #include "LD_remote.h"
 //static void sbus_to_rc(volatile const uint8_t *sbus_buf, RC_ctrl_t *rc_ctrl);
 //RC_ctrl_t rc_ctrl;
 //static uint8_t sbus_rx_buf[2][RC_FRAME_LENGTH];
 //uint8_t remote_ready = 0;//Ò£¿ØÆ÷×¼±¸Íê³É 
 //void remote_control_init(void)
 //{
 //	RC_init(sbus_rx_buf[0], sbus_rx_buf[1], RC_FRAME_LENGTH);
 //}
 // int map_int(int x, int in_min, int in_max, int out_min, int out_max)		
 //{
 //	return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
 //}
 ////´®¿ÚÖÐ¶Ï
 //void USART3_IRQHandler(void)
 //{
 //	//have received data  ???
 //	if(huart3.Instance->SR & UART_FLAG_RXNE)
 //	{
 //		__HAL_UART_CLEAR_FEFLAG(&huart3);
 //	}
 //	else if(USART3->SR & UART_FLAG_IDLE)
 //	{
 //	
 //    static uint16_t this_time_rx_len = 0;
 //		__HAL_UART_CLEAR_PEFLAG(&huart3);
 //		
 //		if( (hdma_usart3_rx.Instance->CR & DMA_SxCR_CT) == RESET)
 //		{
 //			//current memory buffer used is memory0
 //			
 //			//disable dma to change dma register
 //			__HAL_DMA_DISABLE(&hdma_usart3_rx);
 //			
 //			//get received data length, length = set_data_length - remain_length 
 //			this_time_rx_len = SBUS_RX_BUF_NUM - hdma_usart3_rx.Instance->NDTR;
 //			//reset set_data_length
 //			hdma_usart3_rx.Instance->NDTR = SBUS_RX_BUF_NUM;
 //			
 //			//change memory0 to memory1
 //			hdma_usart3_rx.Instance->CR |= DMA_SxCR_CT;
 //			
 //			//enable dma
 //			__HAL_DMA_ENABLE(&hdma_usart3_rx);
 //			
 //			//1 frame length is correct data
 //			if(this_time_rx_len == RC_FRAME_LENGTH)
 //			{
 //				sbus_to_rc(sbus_rx_buf[0], &rc_ctrl);
 //			}
 //		}
 //			else
 //			{
 //				__HAL_DMA_DISABLE(&hdma_usart3_rx);
 //				
 //				this_time_rx_len = SBUS_RX_BUF_NUM - hdma_usart3_rx.Instance->NDTR;
 //				
 //				hdma_usart3_rx.Instance->NDTR = SBUS_RX_BUF_NUM;
 //				
 //				//change memory1 to memory0
 //				DMA1_Stream1->CR &= ~(DMA_SxCR_CT);
 //				
 //				__HAL_DMA_ENABLE(&hdma_usart3_rx);
 //				
 //				if(this_time_rx_len == RC_FRAME_LENGTH)
 //				{
 //					sbus_to_rc(sbus_rx_buf[1], &rc_ctrl);
 //				}
 //			}
 //	}
 //}
 //static void sbus_to_rc(volatile const uint8_t *sbus_buf, RC_ctrl_t *rc_ctrl)
 //{
 //    if (sbus_buf == NULL || rc_ctrl == NULL)
 //    {
 //        return;
 //    }
 //		
 //		rc_ctrl->ch[0] = (sbus_buf[1] | (sbus_buf[2] << 8)) & 0x07ff;        //!< Channel 0
 //    rc_ctrl->ch[1] = ((sbus_buf[2] >> 3) | (sbus_buf[3] << 5)) & 0x07ff; //!< Channel 1
 //    rc_ctrl->ch[2] = ((sbus_buf[3] >> 6) | (sbus_buf[4] << 2) |          //!< Channel 2
 //                         (sbus_buf[5] << 10)) &0x07ff;
 //    rc_ctrl->ch[3] = ((sbus_buf[5] >> 1) | (sbus_buf[6] << 7)) & 0x07ff; //!< Channel 3
 //    
 //		rc_ctrl->sw[0] = ((int16_t)sbus_buf[6] >> 4 | ((int16_t)sbus_buf[7] << 4 )) & 0x07FF;                  //!< Switch left
 //    rc_ctrl->sw[1] = ((int16_t)sbus_buf[7] >> 7 | ((int16_t)sbus_buf[8] << 1 ) | (int16_t)sbus_buf[9] << 9 ) & 0x07FF;                       //!< Switch right
 //    rc_ctrl->sw[2] = ((int16_t)sbus_buf[9] >> 2 | ((int16_t)sbus_buf[10] << 6 )) & 0x07FF;;                    //!< Mouse X axis
 //    rc_ctrl->sw[3] = ((int16_t)sbus_buf[10] >> 5 | ((int16_t)sbus_buf[11] << 3 )) & 0x07FF;                    //!< Mouse Y axis
 //    rc_ctrl->sw[4] = ((int16_t)sbus_buf[12] << 0 | ((int16_t)sbus_buf[13] << 8 )) & 0x07FF;                  //!< Mouse Z axis
 //    rc_ctrl->sw[5] = ((int16_t)sbus_buf[13] >> 3 | ((int16_t)sbus_buf[14] << 5 )) & 0x07FF;                                  //!< Mouse Left Is Press ?
 //    rc_ctrl->sw[6] = ((int16_t)sbus_buf[14] >> 6 | ((int16_t)sbus_buf[15] << 2 ) | (int16_t)sbus_buf[16] << 10 ) & 0x07FF;                                  //!< Mouse Right Is Press ?
 //    rc_ctrl->sw[7] = ((int16_t)sbus_buf[16] >> 1 | ((int16_t)sbus_buf[17] << 7 )) & 0x07FF;                    //!< KeyBoard value
 //		rc_ctrl->ch[0] -= RC_CH_VALUE_OFFSET;
 //    rc_ctrl->ch[1] -= RC_CH_VALUE_OFFSET;
 //    rc_ctrl->ch[2] -= RC_CH_VALUE_OFFSET;
 //    rc_ctrl->ch[3] -= RC_CH_VALUE_OFFSET;
 //    
 //		//µ÷ÕûÒ¡¸ËÖÐÖµÊ¹Ò£¿ØÆ÷Î¢µ÷Á¿Îª0Ê±Ò¡¸ËÖÐÖµ½Ó½ü0£¬ÓÍÃÅ³ýÍâ
 //		//µÚÒ»´Îµ÷³µÇ°ÏÈÈ·ÈÏÍ¨µÀÖµÊÇ·ñÕýÈ·£¡£¡Èç²»·ûºÏ¸ù¾ÝÊµ¼ÊÍ¨µÀÖµ½øÐÐÐÞ¸Ä
 //		//ÐÞ¸Ä´Ë´¦¼°omni_chassis.cÖÐÍ¨µÀÖµ²¿·Ö
 //		rc_ctrl->ch[0] += 24;	//y(-694,693)×óÓÒ
 //		rc_ctrl->ch[1] = -rc_ctrl->ch[1]-24;	//x(-693,694)Ç°ºó
 //		rc_ctrl->ch[2] = -rc_ctrl->ch[2]+764;		//m(95,1482)ÓÍÃÅ
 //		rc_ctrl->ch[3] += 24;	//w(-694,693)Ðý×ª
 //		
 //		rc_ctrl->ch[1] = map_int(rc_ctrl->ch[1],-693,694,-700,700);		//xÓ³Éäµ½(-700,700)
 //		rc_ctrl->ch[0] = map_int(rc_ctrl->ch[0],-694,693,-700,700);		//yÓ³Éäµ½(-700,700)
 //		rc_ctrl->ch[3] = 0.5*(rc_ctrl->ch[3]);		//w
 //		
 //		//ËÀÇø(-30,30)
 //		if(rc_ctrl->ch[0]>-30&&rc_ctrl->ch[0]<30) rc_ctrl->ch[0]=0;
 //		if(rc_ctrl->ch[1]>-30&&rc_ctrl->ch[1]<30) rc_ctrl->ch[1]=0;
 //		if(rc_ctrl->ch[2]>-30&&rc_ctrl->ch[2]<30) rc_ctrl->ch[2]=0;
 //		if(rc_ctrl->ch[3]>-30&&rc_ctrl->ch[3]<30) rc_ctrl->ch[3]=0;
 //    remote_ready = 1;
 //}                                                                            																																					
 //int map(int x, int in_min, int in_max, int out_min, int out_max)		//Ó³Éäº¯Êý
 //{
 //	return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
 //}
 ///*
 //  																																	
 //306																							  					  	  306
 // sw[]																						 	 					  		sw[7]
 //1694																							 					  		1694
 // 
 //306																														    	306
 //sw[6]                                                              sw[4]
 //1694                                                               1694
 //306         306                                        306        306   
 //sw[0]      sw[2]    sw[1]:306-1694   sw[5]:306-1694  sw[]1000   sw[3] 
 //1694       1694                                         1694       1694
 //                                      710
 //               688                   1425
 //                |                      |
 //                |                      |
 //   54    -616------ch[3]770   -354---------ch[0] 339  0
 //                |                      |
 //                |                      |
 //	             ch[2]                 ch[1]
 //               _699                   38   
 //*/
--- a/User/device/LD_remote.h
+++ b/User/device/LD_remote.h
@ -1,31 +0,0 @@
 #ifndef LD_H
 #define LD_H
 #include "user_math.h"
 #define SBUS_RX_BUF_NUM 50u
 #define RC_FRAME_LENGTH 25u
 #define RC_CH_VALUE_OFFSET  ((uint16_t)1024)
 typedef  struct
 {
 	int16_t ch[4];
  int16_t sw[8]; 
 }__attribute__((packed)) RC_ctrl_t;
 int map_int(int x, int in_min, int in_max, int out_min, int out_max)	;	
 void remote_control_init(void);
 static void sbus_to_rc(volatile const uint8_t *sbus_buf, RC_ctrl_t *rc_ctrl);
 int map(int x, int in_min, int in_max, int out_min, int out_max);		
 #endif
--- a/User/device/bmi088.c
+++ b/User/device/bmi088.c
@ -12,8 +12,8 @@
 #include <string.h>
 #include "user_math.h"
 #include "bsp\delay.h"
-#include "bsp\gpio.h"
+#include "bsp\bsp_gpio.h"
-#include "bsp\spi.h"
+#include "bsp\bsp_spi.h"
 #include "device\device.h"
 #include "bsp\pwm.h"
 #include "pid.h"
--- a/User/device/can_use.c
+++ b/User/device/can_use.c
@ -378,7 +378,8 @@ int8_t CAN_StoreMsg(CAN_t *can, CAN_RawRx_t *can_rx) {
 //    case CAN_VSEC5065_M4_MSG1:
 //      // 存储消息到对应的电机结构体中
 //      CAN_VescMotor_Decode_1(&(can->motor.chassis5065.as_array[3]), can_rx->rx_data);
-      break;
+
 																											break;
--- a/User/device/cmd.c
+++ b/User/device/cmd.c
@ -8,79 +8,32 @@
 /* Includes ----------------------------------------------------------------- */
 #include "cmd.h"
 #include "gpio.h"
-
+ 
 #include <string.h>
 /* Private function  -------------------------------------------------------- */
 /*Export function --------------------------------------------------------------*/
 int8_t CMD_Init(CMD_t *cmd){
   /*若主结构体为空 自动返回错误 */
  if(cmd == NULL) return-1;
 /**/
  cmd->C_cmd.type =RC;
 	cmd->C_cmd.mode =NORMAL;
 return 0;
 }
 static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd) {
  /* 将操纵杆的对应值转换为底盘的控制向量和云台变化的欧拉角 */
  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 ;
 }
 /**
 * @brief rc失控时机器人恢复放松模式
 *
 * @param cmd 主结构体
 */
 static void CMD_RcLostLogic(CMD_t *cmd){
  /* 机器人底盘运行模式恢复至放松模式 */
  cmd->C_cmd.mode = RELAXED;
 }
 int8_t CMD_ParseRc(CMD_t *cmd,CMD_RC_t *rc){   
   if (cmd == NULL) return -1;
   if (rc == NULL) return -1;
  /*c当rc丢控时，恢复机器人至默认状态 */
  if ((rc->sw_l == CMD_SW_ERR) || (rc->sw_r == CMD_SW_ERR)) {
    CMD_RcLostLogic(cmd);
  } else {
      CMD_RcLogic(rc, cmd);
  }
 return 0;
 }
 /*nuc数据统一到cmd*/
 int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n){
  if(cmd == NULL) return -1;
  if(n == NULL) return -1;
-  cmd->cmd_status = n->status_fromnuc;
+  cmd->NAVI_t .cmd_status  = n->status_fromnuc;
-  cmd->raw_status = n->ctrl_status;
+  cmd->NAVI_t .raw_status = n->ctrl_status;
 	 for (int i = 0; i < 7; ++i) 
 	  { // 从最低位到最高位遍历
-        cmd->status[i] = ((cmd->raw_status) & (1 << i)) ? 1 : 0;
+        cmd->NAVI_t .status[i] = ((cmd->NAVI_t .raw_status) & (1 << i)) ? 1 : 0;
    }
-    switch(cmd->cmd_status){
+    switch(cmd->NAVI_t .cmd_status){
 			case PICK :
 				cmd ->NAVI_t .pick .posx =n->pick .posx ;
 				cmd ->NAVI_t .pick .posy =n->pick .posy ;
 				cmd ->NAVI_t .pick .posw =n->pick .posw ;
 			case MID:
 			cmd->C_navi.vx = n->navi.vx;
 			cmd->C_navi.vy = n->navi.vy;
 			cmd->C_navi.wz = n->navi.wz;
 			break;
@ -88,108 +41,37 @@ int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n){
        return 0;
 }
-
+/*遥控器，上下层通用，按键控制，统一到cmd*/
-
+int8_t CMD_ParseRC(CMD_t *cmd,const CMD_RC_t *rc) {
 /*
             遥控器模式重新分配
  这一部分没有设置具体的模式名，后期根据需要修改
 		遥控器模式(RC):
 		左按键  ---  右按键
 		             mode1
          up     	 no_mode	
                     mode2
 		             mode3
          down     	 no_mode	
                     mode4
          mid        auto_navi(0x09)雷达导航
 */
 int8_t CMD_CtrlSet(CMD_t *cmd) {
 if(cmd == NULL) return -1;
  if(cmd->key_ctrl_l == CMD_SW_UP)//当左拨杆打到最上面时 强制使用遥控器控制
 	{
       /*遥控器模式下，右按键三种状态分配*/	
-		if(cmd->key_ctrl_r==CMD_SW_UP)
+  if ((rc->sw_l == CMD_SW_ERR) || (rc->sw_r == CMD_SW_ERR)) {
-		{
+     cmd->CMD_CtrlType =RELAXED;
-		     cmd->C_cmd.type = RC;
+  }	
-		     cmd->C_cmd.mode = RC_MODE1;			
+	else if(rc->sw_l==CMD_SW_UP)
 		}
 		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_MODE2;			
 		}
 	}
 	else if(cmd->key_ctrl_l ==CMD_SW_DOWN)
 	{
-		if(cmd->key_ctrl_r==CMD_SW_UP)
+		cmd ->CMD_CtrlType =UP_RCcontrol;
-		{
+		if(rc->sw_r ==CMD_SW_UP) cmd ->CMD_UP_mode =Pitch; //左上，右上，投篮
-		     cmd->C_cmd.type = RC;
+		if(rc->sw_r ==CMD_SW_MID) cmd ->CMD_UP_mode =Normal; //左上，右中，无模式
-		     cmd->C_cmd.mode = RC_MODE3;			
+	  if(rc->sw_r ==CMD_SW_DOWN) cmd ->CMD_UP_mode =Dribble; //左上，右上，投篮
 		}
 		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;			
 		}
 	}
 	else //左按键打到中间，自动模式
 	{	
      if(	cmd->key_ctrl_l==CMD_SW_MID	){
 		 cmd->C_cmd.type = MID_NAVI; 
 		    cmd->C_cmd.mode = RC_NO_MODE;			
      switch(cmd->cmd_status)
 {
 	  case MID://雷达，视觉导航
 		 cmd->C_cmd.type = MID_NAVI; 
 	   break;
  }
 }
  }
 return 0;
 }
 //接收码盘导航的返回数据，传入cmd_t结构体
 int8_t CMD_ParseAction(CMD_t *cmd,CMD_ACTION_t *act)
 {
 	 if(cmd == NULL) return -1;
   if(act == NULL) return -1;
 	if(cmd->key_ctrl_l ==CMD_SW_MID	)
 	{
 		if(cmd->key_ctrl_r == CMD_SW_UP) 
 			act->flag =1;
 		if(cmd->key_ctrl_r == CMD_SW_DOWN ) 
 			act->flag =-1;
 	}
-	 cmd->C_navi .vx  =act ->out .Vx ;
+	else if(rc->sw_l==CMD_SW_MID)
-	 cmd ->C_navi .vy  =act ->out .Vy ;
+	{
-	 cmd ->C_navi .wz  =act ->out .Vw ;
+				cmd ->CMD_CtrlType =MID_NAVI;
-	 
+	}
-	 return 0;
+	else if(rc->sw_l==CMD_SW_DOWN)
 	{
 		cmd ->CMD_CtrlType =PICK_t;
 		if(rc->sw_r ==CMD_SW_UP) cmd ->CMD_UP_mode =Pitch; //左下，右上，投篮
 		if(rc->sw_r ==CMD_SW_MID) cmd ->CMD_UP_mode =Normal; //左下，右中，无模式
 	  if(rc->sw_r ==CMD_SW_DOWN) cmd ->CMD_UP_mode =Dribble; //左下，右上，投篮
 	}	
 	return 0;
 }
--- a/User/device/cmd.h
+++ b/User/device/cmd.h
@ -1,19 +1,3 @@
 /*        
 该任务用于接收来自各个不同的控制方式所期望的控制指令 将其集中统一化后分发给各个模块
 */			
 /*
     按键控制逻辑
 	RC模式，左按键打到最上，右按键启用，中间无状态，上下各代表模式1、模式2
 	雷达导航，左按键打到中间，右按键禁用
 	左按键打到下面，保留，未启用
 */
 #ifndef _CMD_H
@ -23,38 +7,44 @@
 #define MID (0x09)
 #define PICK (0x06)
 typedef enum{
-	RC,//遥控器控制，左按键上
+	UP_RCcontrol,//遥控器控制，左按键上，控制上层
 	MID_NAVI,//雷达导航
- }CMD_Chassis_CtrlType_e;
+	PICK_t,
 	RELAXED,//异常模式
 }CMD_CtrlType_t;
 typedef  enum{
-	 RELAXED,//异常模式
+			
 	 NORMAL,
 	 GYRO_STAY,
 	 RC_MODE1,
 	 RC_NO_MODE,
 	 RC_MODE2,
 	 RC_MODE3,
 	 RC_MODE4,
 	 AUTO_NAVI
  }CMD_Chassis_mode_e;
 /*该结构体负责接收和解析地盘模块所需要的控制指令*/
 typedef struct{
 CMD_Chassis_CtrlType_e type;
 CMD_Chassis_mode_e mode;
 }CMD_Chassis_Ctrl_t;
 		Normal,    //无模式
    Dribble ,  //运球
 	  Pitch  ,    //投篮
  }CMD_UP_mode_t;
 typedef struct {
 uint8_t  status_fromnuc;
 uint8_t  ctrl_status;  //取其中每一个二进制位用作通信
  struct 
  {
  fp32 vx;
  fp32 vy;
  fp32 wz;
  }navi;
 struct 
 {
 	 fp32 posx;
   fp32 posy;
   fp32 posw;
 }pick;
  struct
 	{
 		fp32 angle;
 	}sick_cali;
 } CMD_NUC_t;
 /* 拨杆位置 */
 typedef enum {
  CMD_SW_ERR = 0,
@ -82,102 +72,38 @@ typedef struct {
 } __attribute__((packed))CMD_RC_t;
 typedef	struct
 	{
 		fp32 Vx;
 		fp32 Vy;
 		fp32 Vw;
 	}OpsHopeVector_t;
 typedef	struct
 	{
 		fp32 x;
 		fp32 y;
 		fp32 z;
 		int cnt_point;//计数点
 	}ops_point;
 typedef struct {
  OpsHopeVector_t out;
 	int flag;//传递flag触发标志位
 } CMD_ACTION_t;
 typedef struct {
 uint8_t  status_fromnuc;
 uint8_t  ctrl_status;  //取其中每一个二进制位用作通信
  struct 
  {
  fp32 vx;
  fp32 vy;
  fp32 wz;
  }navi;
 struct 
 {
 	 fp32 posx;
   fp32 posy;
   fp32 posw;
 }pick;
  struct
 	{
 		fp32 angle;
 	}sick_cali;
 } CMD_NUC_t;
 typedef struct{
 	 fp32 posy;
   fp32 posx;
   fp32 posw;	
 }CMD_FOR_PICK;
 typedef  struct {
  fp32 vx;
  fp32 vy;
  fp32 wz;
 }CMD_FOR_NAVI;
 typedef struct {
   uint8_t cmd_status;
   uint8_t raw_status;
   uint8_t status[7];
   fp32 Vx;
   fp32 Vy;
   fp32 Vw;
   fp32 poscamear;
   fp32 key_ctrl_l;
   fp32 key_ctrl_r;
 	 fp32 forsick_wz;
-	CMD_Chassis_Ctrl_t C_cmd;
+ /*视觉*/		
-	CMD_FOR_NAVI C_navi;
+	struct {
   uint8_t cmd_status;
   uint8_t raw_status;	 
   uint8_t status[7];	
 		struct 
 		{
 			 fp32 posx;
 			 fp32 posy;
 			 fp32 posw;
 			}pick;
 }NAVI_t;
   CMD_UP_mode_t CMD_UP_mode;
   CMD_CtrlType_t CMD_CtrlType;
 } CMD_t;
-int8_t CMD_Init(CMD_t *cmd);
+/*nuc数据统一到cmd*/
 int8_t CMD_ParseRc(CMD_t *cmd,CMD_RC_t *rc);
 int8_t CMD_ParseAction(CMD_t *cmd,CMD_ACTION_t *act);
 int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n);
-
+int8_t CMD_ParseRC(CMD_t *cmd,const CMD_RC_t *rc) ;
 int8_t CMD_SwitchStatus(CMD_t *cmd);
 int8_t CMD_CtrlSet(CMD_t *cmd);
 #endif 
--- a/User/device/nuc.c
+++ b/User/device/nuc.c
@ -45,13 +45,13 @@ bool_t NUC_WaitDmaCplt(void) {
 int8_t NUC_RawParse(CMD_NUC_t *n){
 	if(n ==NULL) return DEVICE_ERR_NULL;
-//  if(nucbuf[0]!=HEAD) goto error;  
+  if(nucbuf[0]!=HEAD) goto error;  
-//  else{
+  else{
    n->status_fromnuc =nucbuf[1];
 		n->ctrl_status =nucbuf[2];
-//    switch (n->status_fromnuc)
+    switch (n->status_fromnuc)
-//    {
+    {
-//		case MID://控制帧0x09
+		case MID://控制帧0x09
 				/* 协议格式 
 		   0xFF  HEAD
 		   0x0X  控制帧
@ -62,7 +62,7 @@ int8_t NUC_RawParse(CMD_NUC_t *n){
 		   0xFE  TAIL
 	使用的是串口1
 		  */
-//		if(nucbuf[15]!=TAIL)goto error;
+		if(nucbuf[15]!=TAIL)goto error;
       instance.data[3] = nucbuf[6];
       instance.data[2] = nucbuf[5];
       instance.data[1] = nucbuf[4];
@ -78,59 +78,42 @@ int8_t NUC_RawParse(CMD_NUC_t *n){
       instance.data[9] = nucbuf[12];
       instance.data[8] = nucbuf[11];
      n->navi.wz = instance.x[2];//
-//      break;
+      break;
-//    case PICK:
+    case PICK:
-//			/* 协议格式 
+			/* 协议格式 
-//		   0xFF  HEAD
+		   0xFF  HEAD
-//		   0x0X  控制帧
+		   0x06  控制帧
-//		   0x01  相机帧
+		   0x01  相机帧
-//		   cmd   8位
+		   cmd   8位
-//		   dis   相机深度值
+		   dis   相机深度值
-//		   posx  相机yaw轴值
+		   posx  相机yaw轴值
-//		   posy  相机pitch轴值
+		   posy  相机pitch轴值
-//		   0xFE  TAIL
+		   0xFE  TAIL
-//		  */
+		  */
-//		  if(nucbuf[15]!=TAIL)goto error;
+		  if(nucbuf[15]!=TAIL)goto error;
-//       instance.data[3] = nucbuf[6];
+       instance.data[3] = nucbuf[6];
-//       instance.data[2] = nucbuf[5];
+       instance.data[2] = nucbuf[5];
-//       instance.data[1] = nucbuf[4];
+       instance.data[1] = nucbuf[4];
-//       instance.data[0] = nucbuf[3];
+       instance.data[0] = nucbuf[3];
-//      n->pick.posx = instance.x[0]; //距离球中心的角度值
+      n->pick.posx = instance.x[0]; //距离球中心的角度值
-//       instance.data[7] = nucbuf[10];
+       instance.data[7] = nucbuf[10];
-//       instance.data[6] = nucbuf[9];
+       instance.data[6] = nucbuf[9];
-//       instance.data[5] = nucbuf[8];
+       instance.data[5] = nucbuf[8];
-//       instance.data[4] = nucbuf[7];
+       instance.data[4] = nucbuf[7];
-//		 n->pick.posy = instance.x[1];// 相机yaw轴
+		 n->pick.posy = instance.x[1];// 相机yaw轴
-//		   instance.data[11]  = nucbuf[14];
+		   instance.data[11]  = nucbuf[14];
-//		   instance.data[10]  = nucbuf[13];
+		   instance.data[10]  = nucbuf[13];
-//		   instance.data[9]  = nucbuf[12];
+		   instance.data[9]  = nucbuf[12];
-//		   instance.data[8]  = nucbuf[11];
+		   instance.data[8]  = nucbuf[11];
-//		 n->pick.posw= instance.x[2];// 暂未用到
+		 n->pick.posw= instance.x[2];// 暂未用到
-//       break;
+       break;
-//		case SICK_CAIL:
+
-//			if(nucbuf[15]!=TAIL)goto error;
+    }
-//       instance.data[3] = nucbuf[14];
+     return DEVICE_OK;
-//       instance.data[2] = nucbuf[13];
+  }
-//       instance.data[1] = nucbuf[12];
+    error:
-//       instance.data[0] = nucbuf[11];
+    drop_message++;
-//      n->sick_cali.angle = instance.x[0]; //
+    return DEVICE_ERR;
 //       instance.data[7] = nucbuf[10];
 //       instance.data[6] = nucbuf[9];
 //       instance.data[5] = nucbuf[8];
 //       instance.data[4] = nucbuf[7];
 //		 n->sick_cali.isleft = instance.x[1];// 
 //		   instance.data[11]  = nucbuf[14];
 //		   instance.data[10]  = nucbuf[13];
 //		   instance.data[9]  = nucbuf[12];
 //		   instance.data[8]  = nucbuf[11];
 //		 n->pick.posw= instance.x[2];// 暂未用到
 //		   break;
 //    }
 //     return DEVICE_OK;
 //  }
 //    error:
 //    drop_message++;
 //    return DEVICE_ERR;
 }
 int8_t NUC_HandleOffline(CMD_NUC_t *cmd)
--- a/User/device/r12ds.c
+++ b/User/device/r12ds.c
@ -1,216 +0,0 @@
 /*
  乐迪r12ds遥控器。
  需要根据实际的遥控器通道修改解析的函数 该遥控器采用sbus  用的uart3
 */
 ///* Includes ----------------------------------------------------------------- */
 //#include "r12ds.h"
 //#include "main.h"
 //#include "error_detect.h"
 //#include <string.h>
 //extern UART_HandleTypeDef huart3;
 //extern DMA_HandleTypeDef hdma_usart3_rx;
 //osThreadId_t thread_alert;
 //int buf0ready =0;
 //int buf1ready =0;
 ///* Private function  -------------------------------------------------------- */
 //static void R12DS_IdleCallback(void) {
 //	static uint16_t this_time_rx_len = 0;
 //	if( (hdma_usart3_rx.Instance->CR & DMA_SxCR_CT) == RESET)
 //		{
 //			//current memory buffer used is memory0
 //			
 //			//disable dma to change dma register
 //			__HAL_DMA_DISABLE(&hdma_usart3_rx);
 //			
 //			//get received data length, length = set_data_length - remain_length 
 //			this_time_rx_len = SBUS_RX_BUF_NUM - hdma_usart3_rx.Instance->NDTR;
 //			//reset set_data_length
 //			hdma_usart3_rx.Instance->NDTR = SBUS_RX_BUF_NUM;
 //			
 //			//change memory0 to memory1
 //			hdma_usart3_rx.Instance->CR |= DMA_SxCR_CT;
 //			
 //			//enable dma
 //			__HAL_DMA_ENABLE(&hdma_usart3_rx);
 //			
 //			//1 frame length is correct data
 //			if(this_time_rx_len == RC_FRAME_LENGTH)
 //			{
 ////         	osThreadFlagsSet(thread_alert,SIGNAL_R12DS_BUF0_REDY);
 //				buf0ready = 1;
 //				detect_hook(R12DS_TOE);
 //			}
 //		}
 //			else
 //			{
 //				__HAL_DMA_DISABLE(&hdma_usart3_rx);
 //				
 //				this_time_rx_len = SBUS_RX_BUF_NUM - hdma_usart3_rx.Instance->NDTR;
 //				
 //				hdma_usart3_rx.Instance->NDTR = SBUS_RX_BUF_NUM;
 //				
 //				//change memory1 to memory0
 //				DMA1_Stream1->CR &= ~(DMA_SxCR_CT);
 //				
 //				__HAL_DMA_ENABLE(&hdma_usart3_rx);
 //				
 //				if(this_time_rx_len == RC_FRAME_LENGTH)
 //				{
 ////           	osThreadFlagsSet(thread_alert,SIGNAL_R12DS_BUF1_REDY);
 //					buf1ready = 1;
 //					detect_hook(R12DS_TOE);
 //				}
 //			}
 //}
 ///* Exported functions ------------------------------------------------------- */
 //int8_t R12ds_DMA_Init(uint8_t *rx1_buf, uint8_t *rx2_buf, uint16_t dma_buf_num)
 //{
 //	    if((thread_alert = osThreadGetId()) == NULL ) return DEVICE_ERR_NULL;
 //	//enable the dma transfer for the receiver request
 //	SET_BIT(huart3.Instance->CR3, USART_CR3_DMAR);
 //	
 //	//enable idle interrupt
 //	__HAL_UART_ENABLE_IT(&huart3, UART_IT_IDLE);
 //	
 //	//disable dma, to change the dma register
 //	__HAL_DMA_DISABLE(&hdma_usart3_rx);
 //	
 //	//disable dma again but why?
 //	//what's the condition?
 //	while(hdma_usart3_rx.Instance->CR & DMA_SxCR_EN)
 //	{
 //		__HAL_DMA_DISABLE(&hdma_usart3_rx);
 //	}
 //	
 //	//??
 //	hdma_usart3_rx.Instance->PAR = (uint32_t) & (USART3->DR);
 //	
 //	//memory buffer 1
 //	hdma_usart3_rx.Instance->M0AR = (uint32_t)(rx1_buf);
 //	
 //	//momory buffer 2
 //	hdma_usart3_rx.Instance->M1AR = (uint32_t)(rx2_buf);
 //	
 //	//data length
 //	hdma_usart3_rx.Instance->NDTR = dma_buf_num;
 //	
 //	//enable double memory buffer
 //	SET_BIT(hdma_usart3_rx.Instance->CR, DMA_SxCR_DBM);
 //	
 //	
 //	//enable dma
 //	__HAL_DMA_ENABLE(&hdma_usart3_rx);
 //	
 //	  BSP_UART_RegisterCallback(BSP_UART_R12DS, BSP_UART_IDLE_LINE_CB,
 //                           R12DS_IdleCallback);
 //  return 1;
 //}
 //int map(int x, int in_min, int in_max, int out_min, int out_max)		//ӳʤگ˽
 //{
 //	return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
 //}
 //int8_t sbus_to_rc(volatile const uint8_t *sbus_buf, CMD_RC_t *rc_ctrl)
 //{
 //    if (sbus_buf == NULL || rc_ctrl == NULL)
 //    {
 //        return 0;
 //    }
 //		
 //		rc_ctrl->ch_x = (sbus_buf[1] | (sbus_buf[2] << 8)) & 0x07ff;        //Channel 1  x
 //    rc_ctrl->mul= ((sbus_buf[2] >> 3) | (sbus_buf[3] << 5)) & 0x07ff; //Channel 2   mul
 //    rc_ctrl->ch_y= ((sbus_buf[3] >> 6) | (sbus_buf[4] << 2) |          //Channel 3  y
 //                         (sbus_buf[5] << 10)) &0x07ff;
 //    rc_ctrl->ch_w = ((sbus_buf[5] >> 1) | (sbus_buf[6] << 7)) & 0x07ff; //Channel 4  w
 //    
 //		rc_ctrl->key[0] = ((int16_t)sbus_buf[6] >> 4 | ((int16_t)sbus_buf[7] << 4 )) & 0x07FF;                   								//Channel 5
 //    rc_ctrl->key[1] = ((int16_t)sbus_buf[7] >> 7 | ((int16_t)sbus_buf[8] << 1 ) | (int16_t)sbus_buf[9] << 9 ) & 0x07FF;    	//Channel 6
 //    rc_ctrl->key[2] = ((int16_t)sbus_buf[9] >> 2 | ((int16_t)sbus_buf[10] << 6 )) & 0x07FF;;                    							//Channel 7
 //    rc_ctrl->key[3] = ((int16_t)sbus_buf[10] >> 5 | ((int16_t)sbus_buf[11] << 3 )) & 0x07FF;                    							//Channel 8
 //    rc_ctrl->key[4] = ((int16_t)sbus_buf[12] << 0 | ((int16_t)sbus_buf[13] << 8 )) & 0x07FF;                  								//Channel 9
 //    rc_ctrl->key[5] = ((int16_t)sbus_buf[13] >> 3 | ((int16_t)sbus_buf[14] << 5 )) & 0x07FF;                               	//Channel 10
 //    rc_ctrl->key[6] = ((int16_t)sbus_buf[14] >> 6 | ((int16_t)sbus_buf[15] << 2 ) | (int16_t)sbus_buf[16] << 10 ) & 0x07FF;	//Channel 11
 //    rc_ctrl->key[7] = ((int16_t)sbus_buf[16] >> 1 | ((int16_t)sbus_buf[17] << 7 )) & 0x07FF;                    							//Channel 12
 //		rc_ctrl->ch_y -= RC_CH_VALUE_OFFSET;
 //    rc_ctrl->ch_x -= RC_CH_VALUE_OFFSET;
 //    rc_ctrl->mul  -= RC_CH_VALUE_OFFSET;
 //    rc_ctrl->ch_w -= RC_CH_VALUE_OFFSET;
 //    
 //		rc_ctrl->ch_y -= 4;	//y(-694,693)
 //		rc_ctrl->ch_x += 3;	//x(-693,694)
 //		rc_ctrl->mul = rc_ctrl->mul;		//m(-518,843)
 //		rc_ctrl->ch_w += 4;		//w(-694,693)
 //	
 //    rc_ctrl->key[2] = map(rc_ctrl->key[2],306,1694,1694,306);	
 //    rc_ctrl->key[3] = map(rc_ctrl->key[3],306,1694,1694,306);	
 //		rc_ctrl->ch_x = map(rc_ctrl->ch_x,700,-800,-700,700);		//x
 //		rc_ctrl->ch_y = map(rc_ctrl->ch_y,-900,796,700,-700);	//y
 //		rc_ctrl->mul = map(rc_ctrl->mul,-632,901,25,0);		//m
 //		rc_ctrl->ch_w = map(rc_ctrl->ch_w,-820,780,-700,700);  
 //		rc_ctrl->ch_w = 0.5f*(rc_ctrl->ch_w);
 //    rc_ctrl->ch_x = -0.5f*(rc_ctrl->ch_x);
 //    rc_ctrl->ch_y = -0.5f*(rc_ctrl->ch_y);		
 ////		
 ////̀死区(-5,5)
 //		if(rc_ctrl->ch_y>-15&&rc_ctrl->ch_y<15) rc_ctrl->ch_y=0;              
 //		if(rc_ctrl->ch_x>-15&&rc_ctrl->ch_x<15) rc_ctrl->ch_x=0;                
 //		if(rc_ctrl->mul>=0&&rc_ctrl->mul<=3) rc_ctrl->mul=0;              
 //		if(rc_ctrl->ch_w>-15&&rc_ctrl->ch_w<15) rc_ctrl->ch_w=0;        
 //   return 0;
 //}
 //bool_t R12Buf0_WaitDmaCplt(int32_t timeout) {
 //	
 //	    if(buf0ready == 1)
 //			{
 //				buf0ready =0;
 //				return 1;
 //			}
 //			else 
 //				return 0;
 ////  return(osThreadFlagsWait(SIGNAL_R12DS_BUF0_REDY, osFlagsWaitAll,timeout) ==
 ////          SIGNAL_R12DS_BUF0_REDY);
 //}
 //bool_t R12Buf1_WaitDmaCplt(int32_t timeout) {
 //		    if(buf1ready == 1)
 //			{
 //				buf1ready =0;
 //				return 1;
 //			}
 //			else 
 //				return 0;
 ////  return(osThreadFlagsWait(SIGNAL_R12DS_BUF1_REDY, osFlagsWaitAll,timeout) ==
 ////          SIGNAL_R12DS_BUF1_REDY);
 //	    
 //}
 //void R12ds_HandleOffline(void) {
 //	 CMD_RC_t *rc;
 //	 rc = R12ds_DataGet();
 //  memset(rc, 0, sizeof(*rc));
 //  rc->key[3] = 2000;//用作遥控器断电后急停使用
 //}
--- a/User/device/r12ds.h
+++ b/User/device/r12ds.h
@ -1,32 +0,0 @@
 #ifndef _R12DS_H
 #define _R12DS_H 
 #include <cmsis_os2.h>
 #include "device.h"
 #include "bsp_usart.h"
 #include "cmd.h"
 #define RC_FRAME_LENGTH 25u
 #define RC_CH_VALUE_OFFSET  ((uint16_t)1024)
 int8_t R12ds_DMA_Init(uint8_t *rx1_buf, uint8_t *rx2_buf, uint16_t dma_buf_num);
 int8_t sbus_to_rc(volatile const uint8_t *sbus_buf,CMD_RC_t *rc_ctrl);
 bool_t R12Buf0_WaitDmaCplt(int32_t timeout);
 bool_t R12Buf1_WaitDmaCplt(int32_t timeout);
 void R12ds_HandleOffline(void);
 #endif
--- a/User/device/remote_control.c
+++ b/User/device/remote_control.c
@ -1,185 +0,0 @@
 /**
  ****************************(C) COPYRIGHT 2019 DJI****************************
  * @file       remote_control.c/h
  * @brief      遥控器处理，遥控器是通过类似SBUS的协议传输，利用DMA传输方式节约CPU
  *             资源，利用串口空闲中断来拉起处理函数，同时提供一些掉线重启DMA，串口
  *             的方式保证热插拔的稳定性。
  * @note       该任务是通过串口中断启动，不是freeRTOS任务
  * @history
  *  Version    Date            Author          Modification
  *  V1.0.0     Dec-01-2019     RM              1. 完成
  *
  @verbatim
  ==============================================================================
  ==============================================================================
  @endverbatim
  ****************************(C) COPYRIGHT 2019 DJI****************************
  */
 #include "remote_control.h"
 #include "main.h"
 extern UART_HandleTypeDef huart3;
 extern DMA_HandleTypeDef hdma_usart3_rx;
 static void sbus_to_rc(volatile const uint8_t *sbus_buf, RC_ctrl_t *rc_ctrl);
 RC_ctrl_t rc_ctrl;
 static uint8_t sbus_rx_buf[2][RC_FRAME_LENGTH];
 uint8_t remote_ready = 0;//遥控器准备完成 
 void remote_control_init(void)
 {
 	RC_init(sbus_rx_buf[0], sbus_rx_buf[1], RC_FRAME_LENGTH);
 }
 //串口中断
 void USART3_IRQHandler(void)
 {
 	//have received data  ???
 	if(huart3.Instance->SR & UART_FLAG_RXNE)
 	{
 		__HAL_UART_CLEAR_FEFLAG(&huart3);
 	}
 	else if(USART3->SR & UART_FLAG_IDLE)
 	{
    static uint16_t this_time_rx_len = 0;
 		__HAL_UART_CLEAR_PEFLAG(&huart3);
 		if( (hdma_usart3_rx.Instance->CR & DMA_SxCR_CT) == RESET)
 		{
 			//current memory buffer used is memory0
 			//disable dma to change dma register
 			__HAL_DMA_DISABLE(&hdma_usart3_rx);
 			//get received data length, length = set_data_length - remain_length 
 			this_time_rx_len = SBUS_RX_BUF_NUM - hdma_usart3_rx.Instance->NDTR;
 			//reset set_data_length
 			hdma_usart3_rx.Instance->NDTR = SBUS_RX_BUF_NUM;
 			//change memory0 to memory1
 			hdma_usart3_rx.Instance->CR |= DMA_SxCR_CT;
 			//enable dma
 			__HAL_DMA_ENABLE(&hdma_usart3_rx);
 			//1 frame length is correct data
 			if(this_time_rx_len == RC_FRAME_LENGTH)
 			{
 				sbus_to_rc(sbus_rx_buf[0], &rc_ctrl);
 			}
 		}
 			else
 			{
 				__HAL_DMA_DISABLE(&hdma_usart3_rx);
 				this_time_rx_len = SBUS_RX_BUF_NUM - hdma_usart3_rx.Instance->NDTR;
 				hdma_usart3_rx.Instance->NDTR = SBUS_RX_BUF_NUM;
 				//change memory1 to memory0
 				DMA1_Stream1->CR &= ~(DMA_SxCR_CT);
 				__HAL_DMA_ENABLE(&hdma_usart3_rx);
 				if(this_time_rx_len == RC_FRAME_LENGTH)
 				{
 					sbus_to_rc(sbus_rx_buf[1], &rc_ctrl);
 				}
 			}
 	}
 }
 static void sbus_to_rc(volatile const uint8_t *sbus_buf, RC_ctrl_t *rc_ctrl)
 {
    if (sbus_buf == NULL || rc_ctrl == NULL)
    {
        return;
    }
 		rc_ctrl->ch[1] = (sbus_buf[1] | (sbus_buf[2] << 8)) & 0x07ff;        //Channel 1
    rc_ctrl->ch[2] = ((sbus_buf[2] >> 3) | (sbus_buf[3] << 5)) & 0x07ff; //Channel 2
    rc_ctrl->ch[0] = ((sbus_buf[3] >> 6) | (sbus_buf[4] << 2) |          //Channel 3
                         (sbus_buf[5] << 10)) &0x07ff;
    rc_ctrl->ch[3] = ((sbus_buf[5] >> 1) | (sbus_buf[6] << 7)) & 0x07ff; //Channel 4
 		rc_ctrl->sw[0] = ((int16_t)sbus_buf[6] >> 4 | ((int16_t)sbus_buf[7] << 4 )) & 0x07FF;                   								//Channel 5
    rc_ctrl->sw[1] = ((int16_t)sbus_buf[7] >> 7 | ((int16_t)sbus_buf[8] << 1 ) | (int16_t)sbus_buf[9] << 9 ) & 0x07FF;    	//Channel 6
    rc_ctrl->sw[2] = ((int16_t)sbus_buf[9] >> 2 | ((int16_t)sbus_buf[10] << 6 )) & 0x07FF;;                    							//Channel 7
    rc_ctrl->sw[3] = ((int16_t)sbus_buf[10] >> 5 | ((int16_t)sbus_buf[11] << 3 )) & 0x07FF;                    							//Channel 8
    rc_ctrl->sw[4] = ((int16_t)sbus_buf[12] << 0 | ((int16_t)sbus_buf[13] << 8 )) & 0x07FF;                  								//Channel 9
    rc_ctrl->sw[5] = ((int16_t)sbus_buf[13] >> 3 | ((int16_t)sbus_buf[14] << 5 )) & 0x07FF;                               	//Channel 10
    rc_ctrl->sw[6] = ((int16_t)sbus_buf[14] >> 6 | ((int16_t)sbus_buf[15] << 2 ) | (int16_t)sbus_buf[16] << 10 ) & 0x07FF;	//Channel 11
    rc_ctrl->sw[7] = ((int16_t)sbus_buf[16] >> 1 | ((int16_t)sbus_buf[17] << 7 )) & 0x07FF;                    							//Channel 12
 	rc_ctrl->ch[0] -= RC_CH_VALUE_OFFSET;
    rc_ctrl->ch[1] -= RC_CH_VALUE_OFFSET;
    rc_ctrl->ch[2] -= RC_CH_VALUE_OFFSET;
    rc_ctrl->ch[3] -= RC_CH_VALUE_OFFSET;
 //		//新遥控器
 		rc_ctrl->ch[0] += 20;	//y(-694,693)
 		rc_ctrl->ch[1] += 20;	//x(-693,694)
 		rc_ctrl->ch[2] = -rc_ctrl->ch[2]+144;		//m(-518,843)
 		rc_ctrl->ch[3] += 4;		//w(-694,693)
    rc_ctrl->sw[2] = map(rc_ctrl->sw[2],306,1694,1694,306);	
    rc_ctrl->sw[3] = map(rc_ctrl->sw[3],306,1694,1694,306);	
 //		rc_ctrl->ch[1] = map(rc_ctrl->ch[1],656,-656,-700,700);		//x
 		rc_ctrl->ch[0] = map(rc_ctrl->ch[0],-800,796,700,-700);	//y
 		rc_ctrl->ch[2] = map(rc_ctrl->ch[2],-632,901,25,0);		//m
 		rc_ctrl->ch[3] = map(rc_ctrl->ch[3],-820,780,-700,700);  
 		rc_ctrl->ch[3] = 0.5*(rc_ctrl->ch[3]);
    rc_ctrl->ch[1] = 0.5*(rc_ctrl->ch[1]);
    rc_ctrl->ch[0] = 0.5*(rc_ctrl->ch[0]);		
 //		
 		//死区(-30,30)
 		if(rc_ctrl->ch[0]>-14&&rc_ctrl->ch[0]<10) rc_ctrl->ch[0]=0;              
 		if(rc_ctrl->ch[1]>-30&&rc_ctrl->ch[1]<20) rc_ctrl->ch[1]=0;                
 		if(rc_ctrl->ch[2]>=0&&rc_ctrl->ch[2]<=3) rc_ctrl->ch[2]=0;              
 		if(rc_ctrl->ch[3]>-22&&rc_ctrl->ch[3]<22) rc_ctrl->ch[3]=0;        
    remote_ready = 1;
 }                                                                            																																					
 int map(int x, int in_min, int in_max, int out_min, int out_max)		//映射函数
 {
 	return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
 }
 /*
 306																							  					  	  306
 sw[]																						 	 					  		sw[7]
 1694																							 					  		1694
 306																														    	306
 sw[6]                                                              sw[4]
 1694                                                               1694
 306         306                                        306        306   
 sw[0]      sw[2]    sw[1]:306-1694   sw[5]:306-1694  sw[]1000   sw[3] 
 1694       1694                                         1694       1694
                                      710
               688                   1425
                |                      |
                |                      |
   54    -616------ch[3]770   -354---------ch[0] 339  0
                |                      |
                |                      |
 	             ch[2]                 ch[1]
               _699                   38   
 */
--- a/User/device/remote_control.h
+++ b/User/device/remote_control.h
@ -1,42 +0,0 @@
 /**
  ****************************(C) COPYRIGHT 2016 DJI****************************
  * @file       remote_control.c/h
  * @brief      遥控器处理，遥控器是通过类似SBUS的协议传输，利用DMA传输方式节约CPU
  *             资源，利用串口空闲中断来拉起处理函数，同时提供一些掉线重启DMA，串口
  *             的方式保证热插拔的稳定性。
  * @note       
  * @history
  *  Version    Date            Author          Modification
  *  V1.0.0     Dec-26-2018     RM              1. 完成
  *
  @verbatim
  ==============================================================================
  ==============================================================================
  @endverbatim
  ****************************(C) COPYRIGHT 2016 DJI****************************
  */
 #ifndef REMOTE_CONTROL_H
 #define REMOTE_CONTROL_H
 #include "struct_typedef.h"
 #include "bsp_rc.h"
 #define SBUS_RX_BUF_NUM 50u
 #define RC_FRAME_LENGTH 25u
 #define RC_CH_VALUE_OFFSET  ((uint16_t)1024)
 typedef  struct
 {
 	int16_t ch[4];
  int16_t sw[8]; 
 }__attribute__((packed)) RC_ctrl_t;
 extern void remote_control_init(void);
 static void sbus_to_rc(volatile const uint8_t *sbus_buf, RC_ctrl_t *rc_ctrl);
 int map(int x, int in_min, int in_max, int out_min, int out_max);		//映射函数
 #endif
--- a/User/task/action_task.c
+++ b/User/task/action_task.c
@ -1,75 +0,0 @@
 /*
  码盘解析任务
  处理码盘测量的实际距离
  将需要的数据共享给其他的线程
 */
 #include "action_task.h"
 #include "Action.h"
 #include "user_task.h"
 #ifdef DEBUG
 Action_POS_t pos;
 #else
 static Action_POS pos;
 #endif
 void Task_action(void *argument)
 {
   (void)argument;
 //    osDelay(TASK_INIT_DELAY_ACTION); 
   const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_ACTION;
   ACTIONRECV_Init(&pos);
   uint32_t tick = osKernelGetTickCount(); /* 获取当前控制任务运行频率的tick*/
   while (1)
   {
 #ifdef DEBUG
    /* 记录任务使用的的栈空闄*/
    task_runtime.stack_water_mark.action =
        osThreadGetStackSpace(osThreadGetId());
 #endif
 		  ACTION_StartReceiving();
 		  ACTION_DataRefresh(&pos); 	   
 /*   使用下面的错误处理函数遇到一些问题
       每10ms置标志位，	 ACTION_WaitDmaCplt 判断此标志位，通过后会执行速度计算
 	   根据计算，任务运行频率大于中断产生频率，每一次运行判断标志位不通过，会导致结构体内数据清0的操作
 	  目前的处理方法：注释掉Action_HandleOffline函数中，对结构体数据清零的函数memset
 	   */
      if(ACTION_WaitDmaCplt()){
 		 ACTION_Parse(&pos);			   
      }
       else{
         Action_HandleOffline(&pos);
       }
      //将解算后的码盘位置值放入消息队列供其他任务使用
      osMessageQueueReset(task_runtime.msgq.cmd.raw.Action);
      osMessageQueuePut(task_runtime.msgq.cmd.raw.Action,(&pos),0,0);
    tick += delay_tick; /* 计算下一个唤醒时刻*/
 		osDelayUntil(tick);
   }
 }
--- a/User/task/action_task.h
+++ b/User/task/action_task.h
@ -1 +0,0 @@
--- a/User/task/chassis_task.c
+++ b/User/task/chassis_task.c
@ -1,94 +0,0 @@
 /*
   底盘控制任务
 */
 /* Includes ----------------------------------------------------------------- */
 #include "Chassis.h"
 #include "user_task.h"
 #include "can_use.h"
 #include <cmsis_os2.h>
 #include "up.h"
 #include "vofa.h"
 static CAN_t can;
 #ifdef DEBUG
 CAN_Output_t out;
 Chassis_t chassis  ;
 CMD_t ctrl;
 UP_t upp;
 #else 
 static CAN_Output_t out;
 static Chassis_t chassis;
 static Chassis_Ctrl_t ctrl;
 static UP_t upp;
 #endif
 /**
 * \brief 底盘任务
 *
 * \param argument 
 */
 void Task_Chassis(void *argument)
 {
 	 (void)argument; /* 未使用argument，消除警告*/
 	const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_UP;
  uint32_t tick = osKernelGetTickCount();
 //	
 //	
 ////	HAL_GPIO_WritePin(FlagForUpper_GPIO_Port,FlagForUpper_Pin,GPIO_PIN_RESET); //拉低电平 避免未清除
 //	
 	while(1)
 		{
 #ifdef DEBUG
    task_runtime.stack_water_mark.chassis = osThreadGetStackSpace(osThreadGetId());
 #endif
 //	/*imu数据获取*/					
 //	osMessageQueueGet(task_runtime.msgq.imu.eulr, &chassis.pos088.imu_eulr, NULL, 0);
 //			
 //	osMessageQueueGet(task_runtime.msgq.imu.gyro, &chassis.pos088.bmi088.gyro,NULL, 0);
 //			
 //	osMessageQueueGet(task_runtime.msgq.imu.accl, &chassis.pos088.bmi088.accl,NULL, 0);
 //	/*can上设备数据获取*/		
 //	osMessageQueueGet(task_runtime.msgq.can.feedback.CAN_feedback, &can, NULL, 0);
 //			
 //	osMessageQueueGet(task_runtime.msgq.cmd.raw.Action, &chassis.Action_pos , NULL, 0);
 	/*底盘控制信息获取,目前dji遥控器*/		
    osMessageQueueGet(task_runtime.msgq.cmd.chassis,&ctrl, NULL, 0);
 	/*锁定RTOS（实时操作系统）内核，防止任务切换，直到 osKernelUnlock() 被调用*/		
 //	osKernelLock();
  /*更新电机反馈	*/	
   Chassis_UpdateFeedback(&chassis, &can);   
 	 /*底盘控制*/
 	Chassis_Control(&chassis,&ctrl,&out); 
 	 /*解锁*/
 //      osKernelUnlock();
 	tick += delay_tick;
        osDelayUntil(tick);
 	  }
 }
--- a/User/task/chassis_task.h
+++ b/User/task/chassis_task.h
@ -1,23 +0,0 @@
 #ifndef CHASSIS_TASK_H
 #define CHASSIS_TASK_H
 extern void Task_Chassis(void *argument);
 #endif
--- a/User/task/cmd_task.c
+++ b/User/task/cmd_task.c
@ -4,16 +4,14 @@
 #ifdef DEBUG
 CMD_t cmd;
 CMD_RC_t rc_ctrl;
 CMD_NUC_t Nuc;
-CMD_ACTION_t cmd_ops_out;
+CMD_RC_t rc_ctrl;
 #else
 static CMD_t cmd;
 static CMD_RC_t rc_ctrl;
 static CMD_NUC_t Nuc;	
-static CMD_ACTION_t cmd_ops_out;
+static CMD_RC_t rc_ctrl;
 #endif 
@ -26,7 +24,6 @@ void Task_cmd(void *argument){
 /* 计算到下一次调度任务所需要的tick数 */
  const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CTRL_CMD;
    /**/
  CMD_Init(&cmd);
  uint32_t tick = osKernelGetTickCount(); /*控制任务运行频率的计时 */
   while(1){
@ -40,25 +37,17 @@ void Task_cmd(void *argument){
    /*将各任务接收到的原始数据解析为通用的控制命令*/
-		 
+	 if(osMessageQueueGet(task_runtime.msgq.cmd.raw.rc, &rc_ctrl, 0, 0) == osOK)//遥控器
-		 
+    CMD_ParseRC(&cmd, &rc_ctrl);
 		 /*注意，不能将nuc和码盘导航一块使用*/
 		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);
    if(osMessageQueueGet(task_runtime.msgq.cmd.raw .ops_9_out , &cmd_ops_out, 0, 0) == osOK)//码盘
 			CMD_ParseAction (&cmd ,&cmd_ops_out);
    osKernelUnlock(); /* 同上 解锁RTOS内核 */	
-    /*将需要与其他任务共享的数据放到消息队列里 此处主要分享给底盘 后续会添加和上层机构的通信 */
+		     osMessageQueueReset(task_runtime.msgq.cmd.up_ctrl);
-     osMessageQueueReset(task_runtime.msgq.cmd.chassis);
+		
-     osMessageQueuePut(task_runtime.msgq.cmd.chassis,&cmd,0,0);
+     osMessageQueuePut(task_runtime.msgq.cmd.up_ctrl,&cmd,0,0);
    tick += delay_tick; /*计算下一个唤醒时刻*/
    osDelayUntil(tick); /*绝对延时 等待下一个唤醒时刻 */
--- a/User/task/error_detect_task.c
+++ b/User/task/error_detect_task.c
@ -51,7 +51,6 @@
  */
 #include "error_detect.h"
 #include "user_task.h"
 #include "r12ds.h"
 #include "nuc.h"
 /**
  * @brief          init error_list, assign  offline_time, online_time, priority.
--- a/User/task/init.c
+++ b/User/task/init.c
@ -11,7 +11,6 @@
 #include "task\user_task.h"
 #include "can_use.h"
 #include "cmd.h"
 #include "Action.h"
 /* Private typedef ---------------------------------------------------------- */
 /* Private define ----------------------------------------------------------- */
@ -34,8 +33,6 @@ void Task_Init(void *argument) {
  /* 任务*/
  task_runtime.thread.atti_esti =
      osThreadNew(Task_AttiEsti, NULL, &attr_atti_esti);
 	task_runtime.thread.dr16 =
      osThreadNew(Task_dr16,NULL,&attr_dr16);
  task_runtime.thread.can =
      osThreadNew(Task_can,NULL,&attr_can);
  task_runtime.thread.cmd =
@ -45,6 +42,8 @@ void Task_Init(void *argument) {
 	task_runtime.thread.up=
      osThreadNew(Task_up,NULL,&attr_up);
 	task_runtime.thread.dr16 =
      osThreadNew(Task_dr16,NULL,&attr_dr16);
 	task_runtime.thread.error_detect =
@ -82,8 +81,7 @@ void Task_Init(void *argument) {
      osMessageQueueNew(2u, sizeof(AHRS_Gyro_t), NULL);
 		/*cmd */
-   task_runtime.msgq.cmd.raw.rc =
+
      osMessageQueueNew(3u, sizeof(CMD_RC_t), NULL);
   task_runtime.msgq.cmd.raw.nuc =
      osMessageQueueNew(3u,sizeof(CMD_NUC_t), NULL);	 
--- a/User/task/navi_task.c
+++ b/User/task/navi_task.c
@ -1,68 +0,0 @@
 #include "user_task.h"
 #include "navi.h"
 #include "map.h"
 #ifdef DEBUG
 ops_t o;
 Action_POS_t pos_get;
 CMD_ACTION_t ops_out;//经过导航算法解算后输出的期望控制值
 #else
 static ops_t o;
 static Action_POS_t pos_get;
 static CMD_ACTION_t ops_out;
 #endif
 void Task_Navi(void *argument){
 	 (void)argument;
   const uint32_t delay_tick = osKernelGetTickFreq() /TASK_FREQ_NAVI;
    Action_init(&o,&(task_runtime.config.chassis_config->ops),&pos_get);
   uint32_t tick = osKernelGetTickCount(); /* 获取当前控制任务运行频率的tick*/
    while (1)
   {
 #ifdef DEBUG
    /* 记录任务使用的的栈空间 */
    task_runtime.stack_water_mark.action =
        osThreadGetStackSpace(osThreadGetId());
 #endif
     osMessageQueueGet(task_runtime.msgq.imu.gyro,&o.ops_gyro,NULL,0);
 	   osMessageQueueGet(task_runtime.msgq.imu.eulr,&o.ops_imu_pos,NULL,0);
 		 osMessageQueueGet(task_runtime.msgq.cmd .raw .Action ,&pos_get,NULL,0);
     go_path(&o,&ops_out);
    //将解算后的导航期望运动值放入消息队列供其他任务使用
    osMessageQueueReset(task_runtime.msgq.cmd.raw.ops_9_out);
    osMessageQueuePut(task_runtime.msgq.cmd.raw.ops_9_out,(&ops_out),0,0);
 	tick += delay_tick; /* 计算下一个唤醒时刻*/
 	osDelayUntil(tick);
   }
 }
 //	
 //	
 //	
 //	
 //	
 //	
 //	
--- a/User/task/navi_task.h
+++ b/User/task/navi_task.h
@ -1 +0,0 @@
 #include "navi.h"
--- a/User/task/r12ds_task.c
+++ b/User/task/r12ds_task.c
@ -1,56 +0,0 @@
 #include "r12ds.h"
 #include "cmsis_os.h"
 #include "user_task.h"
 //#ifdef DEBUG
 //CMD_RC_t cmd_rc;
 //uint8_t sbus_rx_buf[2][RC_FRAME_LENGTH];
 //#else 
 //static CMD_RC_t cmd_rc;
 //#endif
 //CMD_RC_t *R12ds_DataGet(){
 //	return &cmd_rc; 
 //}
 //	
 //void Task_r12ds(void *argument)
 //{
 //   (void)argument;
 //  const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_R12DS;
 //    R12ds_DMA_Init(sbus_rx_buf[0],sbus_rx_buf[1],50);
 //	
 //      uint32_t tick = osKernelGetTickCount();
 //   while (1)
 //   {
 //#ifdef DEBUG
 //    task_runtime.stack_water_mark.rs12ds =
 //        osThreadGetStackSpace(osThreadGetId());
 //#endif
 //		 
 //      if(R12Buf0_WaitDmaCplt(1))
 //			{
 //			 sbus_to_rc(sbus_rx_buf[0],&cmd_rc);
 //			}
 //		  else if(R12Buf1_WaitDmaCplt(1))	
 //			{
 //				sbus_to_rc(sbus_rx_buf[1],&cmd_rc);
 //			}
 //      osMessageQueueReset(task_runtime.msgq.cmd.raw.rc);
 //	    osMessageQueuePut(task_runtime.msgq.cmd.raw.rc,(&cmd_rc),0,0);
 //	 
 //	 	tick += delay_tick; /* 计算下一个唤醒时刄1ķ*/
 //		 osDelay(10);
 //   }
 //    
 // }
--- a/User/task/r12ds_task.h
+++ b/User/task/r12ds_task.h
@ -1,16 +0,0 @@
 #ifndef _R12DS_TASK_H
 #define _R12DS_TASK_H
 #endif
--- a/User/task/up_task.c
+++ b/User/task/up_task.c
@ -19,6 +19,7 @@ CAN_Output_t UP_CAN_out;
 UP_t UP  ;
 CMD_t up_cmd;
 #else 
@ -27,11 +28,13 @@ 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;
 /**
@ -48,26 +51,32 @@ void Task_up(void *argument)
  uint32_t tick = osKernelGetTickCount();
 	up_init(&UP,&(task_runtime.config.chassis_config->up ),TASK_FREQ_UP);
 	while(1)
 		{
 #ifdef DEBUG
    task_runtime.stack_water_mark.up = osThreadGetStackSpace(osThreadGetId());
 #endif
-		UP_UpdateFeedback(&UP, &can) ;
+		UP_UpdateFeedback(&UP, &can,&up_cmd) ;
 //    GM6020_control(&UP, 100) ;
-			UP_angle(&UP, bbb);
+//			UP_angle_control(&UP, bbb,M3508 );
 						UP_angle_control(&UP, bbb,M2006 );
 //			UP_M3508_speed(&UP, 500); 
-//			VESC_M5065_Control(&UP,  20000);
+			VESC_M5065_Control(&UP,  000);
 //			
 //			HAL_GPIO_WritePin (GPIOE ,GPIO_PIN_9 ,GPIO_PIN_SET  );
 //		
 //			HAL_GPIO_WritePin (GPIOE ,GPIO_PIN_11 ,GPIO_PIN_SET  );
-			
+//			GO_SendData(&UP, 1,CCC);
 			GO_SendData(&UP, 1,CCC);
 			GO_SendData(&UP, 0,aaa);
 		ALL_Motor_Control(&UP,&UP_CAN_out);
 //		UP_control(&UP,&UP_CAN_out);
 		osDelay(1);
@ -80,8 +89,10 @@ void Task_up(void *argument)
 	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.up_ctrl, &up_cmd, NULL, 0);
 	/*锁定RTOS（实时操作系统）内核，防止任务切换，直到 osKernelUnlock() 被调用*/		
 	osKernelLock();
--- a/User/task/user_task.c
+++ b/User/task/user_task.c
@ -25,21 +25,6 @@ const osThreadAttr_t attr_atti_esti = {
    .priority = osPriorityRealtime,
    .stack_size = 256 * 4,
 };
 //const osThreadAttr_t attr_ops_9pos = {
 //	  .name = "action",
 //	  .priority = osPriorityRealtime,
 //	  .stack_size = 256 *4,
 //};
 //const osThreadAttr_t attr_chassis = {
 //    .name = "chassis",
 //    .priority = osPriorityAboveNormal,
 //    .stack_size = 512 * 4,
 //};
 const osThreadAttr_t attr_r12ds = {
    .name = "r12ds",
    .priority = osPriorityRealtime,
    .stack_size = 128 * 4,
 };
 const osThreadAttr_t attr_can = {
    .name = "can",
@ -65,20 +50,13 @@ const osThreadAttr_t attr_error_detect = {
 	  .stack_size = 128 *4,
 };
 const osThreadAttr_t attr_dr16 = {
 	   .name = "dr16",
 	   .priority = osPriorityRealtime,
 	   .stack_size = 128 *4,
 };
 //const osThreadAttr_t attr_navi = {
 //	   .name = "navi",
 //	   .priority = osPriorityRealtime,
 //	   .stack_size = 128 *4,
 //};
 const osThreadAttr_t attr_up = {
 	   .name = "up",
 	   .priority = osPriorityRealtime,
 	   .stack_size = 512 * 4,
 };
 const osThreadAttr_t attr_dr16 = {
 	   .name = "dr16",
 	   .priority = osPriorityRealtime,
 	   .stack_size = 128 *4,
 };
--- a/User/task/user_task.h
+++ b/User/task/user_task.h
@ -19,7 +19,7 @@
 #define TASK_FREQ_NUC (500u)
 #define TASK_FREQ_CAN (1000u)
 #define TASK_FREQ_NAVI (500u)
-#define TASK_FREQ_R12DS (1000u)
+#define TASK_FREQ_DR16 (1000u)
 #define TASK_FREQ_ACTION (500u)
 #define TASK_FREQ_UP (500u)  //新加的上层
@ -42,16 +42,12 @@ typedef struct {
  /* 任务 */
  struct {
    osThreadId_t atti_esti;
 //		osThreadId_t chassis;
 		osThreadId_t dr16;
 //	  osThreadId_t action_ops;
 		osThreadId_t up;
 //    osThreadId_t ai;
    osThreadId_t can;
 		osThreadId_t cmd;
    osThreadId_t nuc;
 //		osThreadId_t navi;
 		osThreadId_t error_detect;
  } thread;
@ -73,7 +69,7 @@ typedef struct {
      }raw;
-      osMessageQueueId_t chassis;
+      osMessageQueueId_t up_ctrl;
      osMessageQueueId_t status;
    } cmd;
        /* can任务放入、读取，电机或电容的输入输出 */
@ -119,29 +115,22 @@ typedef struct {
  } stack_water_mark; /* stack使用 */
  struct {
 //    float chassis;
    float can;
    float atti_esti;
-    float r12ds;
+    float dr16;
    float cmd;
    float nuc;
 //	  float action;
 //		float navi;
 		float error_detect;
 		float up;
  } freq; /* 任务运行频率 */
  struct {
 //    float chassis;
    float can;
    float atti_esti;
-//    float ai;
+    float dr16;
    float r12ds;
    float cmd;
    float nuc;
 //		float navi;
 //	  float action;
 		float error_detect;
 		float up;
@ -156,43 +145,32 @@ extern const osThreadAttr_t attr_init;
 extern const osThreadAttr_t attr_atti_esti;
 //extern const osThreadAttr_t attr_chassis;
 extern const osThreadAttr_t attr_can;
 extern const osThreadAttr_t attr_cmd;
 //extern const osThreadAttr_t attr_ops_9pos;
 extern const osThreadAttr_t attr_nuc;
 extern const osThreadAttr_t attr_error_detect;
 extern const osThreadAttr_t attr_dr16;
 //extern const osThreadAttr_t attr_navi;
 extern const osThreadAttr_t attr_up;
 /* Exported functions prototypes -------------------------------------------- */
 void Task_Init(void *argument);
 void Task_AttiEsti(void *argument);
 //void Task_Chassis(void *argument);
 void Task_can(void *argument);
 void Task_cmd(void *argument);
 void Task_nuc(void *argument);
 //void Task_action(void *argument);
 //void Task_Navi(void *argument);
 void Task_up(void *argument);
 void Task_dr16(void *argument);
 void Task_error_detect(void *argument);
 void Task_dr16(void *argument);
 #endif
--- a/keilkill.bat
+++ b/keilkill.bat
@ -0,0 +1,27 @@
 del *.bak /s
 del *.ddk /s
 del *.edk /s
 del *.lst /s
 del *.lnp /s
 del *.mpf /s
 del *.mpj /s
 del *.obj /s
 del *.omf /s
 ::del *.opt /s  ::不允许删除JLINK的设置
 del *.plg /s
 del *.rpt /s
 del *.tmp /s
 del *.__i /s
 del *.crf /s
 del *.o /s
 del *.d /s
 del *.axf /s
 del *.tra /s
 del *.dep /s           
 del JLinkLog.txt /s
 del *.iex /s
 del *.htm /s
 del *.sct /s
 del *.map /s
 exit