feat: add Kalman filter component for chassis

Added kalman_filter.c to the project structure for chassis state estimation. Updated Keil MDK project files (.uvprojx, .uvoptx) to include the new component and modified the debug watch window to monitor chassis variables.
feat(balance_chassis): enable wheel-leg balance mode and adjust parameters
2026-02-09 20:21:32 +08:00 · 2026-02-09 14:59:50 +08:00 · 2026-02-07 21:16:49 +08:00 · 2026-02-05 17:59:49 +08:00 · 2026-02-04 17:39:24 +08:00 · 2026-02-04 16:49:48 +08:00
92 changed files with 10539 additions and 11020 deletions
--- a/.mxproject
+++ b/.mxproject
--- a/Core/Inc/FreeRTOSConfig.h
+++ b/Core/Inc/FreeRTOSConfig.h
@ -72,7 +72,7 @@
 #define configTICK_RATE_HZ                       ((TickType_t)1000)
 #define configMAX_PRIORITIES                     ( 56 )
 #define configMINIMAL_STACK_SIZE                 ((uint16_t)128)
-#define configTOTAL_HEAP_SIZE                    ((size_t)0x20000)
+#define configTOTAL_HEAP_SIZE                    ((size_t)0x10000)
 #define configMAX_TASK_NAME_LEN                  ( 16 )
 #define configGENERATE_RUN_TIME_STATS            1
 #define configUSE_TRACE_FACILITY                 1
--- a/CtrBoard-H7_ALL.ioc
+++ b/CtrBoard-H7_ALL.ioc
@ -587,12 +587,12 @@ ProjectManager.ProjectName=CtrBoard-H7_ALL
 ProjectManager.ProjectStructure=
 ProjectManager.RegisterCallBack=
 ProjectManager.StackSize=0x2000
-ProjectManager.TargetToolchain=CMake
+ProjectManager.TargetToolchain=MDK-ARM V5.32
 ProjectManager.ToolChainLocation=
 ProjectManager.UAScriptAfterPath=
 ProjectManager.UAScriptBeforePath=
 ProjectManager.UnderRoot=false
-ProjectManager.functionlistsort=1-SystemClock_Config-RCC-false-HAL-false,2-MX_GPIO_Init-GPIO-false-HAL-true,3-MX_DMA_Init-DMA-false-HAL-true,5-MX_BDMA_Init-BDMA-false-HAL-true,6-MX_ADC1_Init-ADC1-false-HAL-true,7-MX_TIM12_Init-TIM12-false-HAL-true,8-MX_SPI1_Init-SPI1-false-HAL-true,9-MX_SPI2_Init-SPI2-false-HAL-true,10-MX_TIM3_Init-TIM3-false-HAL-true,11-MX_USART1_UART_Init-USART1-false-HAL-true,12-MX_USART2_UART_Init-USART2-false-HAL-true,13-MX_USART3_UART_Init-USART3-false-HAL-true,14-MX_UART7_Init-UART7-false-HAL-true,15-MX_USART10_UART_Init-USART10-false-HAL-true,16-MX_FDCAN1_Init-FDCAN1-false-HAL-true,17-MX_FDCAN2_Init-FDCAN2-false-HAL-true,18-MX_FDCAN3_Init-FDCAN3-false-HAL-true,19-MX_TIM1_Init-TIM1-false-HAL-true,20-MX_TIM2_Init-TIM2-false-HAL-true,21-MX_OCTOSPI1_Init-OCTOSPI1-false-HAL-true,23-MX_UART5_Init-UART5-false-HAL-true,24-MX_ADC3_Init-ADC3-false-HAL-true,0-MX_CORTEX_M7_Init-CORTEX_M7-false-HAL-true
+ProjectManager.functionlistsort=1-SystemClock_Config-RCC-false-HAL-false,2-MX_GPIO_Init-GPIO-false-HAL-true,3-MX_DMA_Init-DMA-false-HAL-true,4-MX_BDMA_Init-BDMA-false-HAL-true,5-MX_ADC1_Init-ADC1-false-HAL-true,6-MX_TIM12_Init-TIM12-false-HAL-true,7-MX_SPI1_Init-SPI1-false-HAL-true,8-MX_SPI2_Init-SPI2-false-HAL-true,9-MX_TIM3_Init-TIM3-false-HAL-true,10-MX_USART1_UART_Init-USART1-false-HAL-true,11-MX_USART2_UART_Init-USART2-false-HAL-true,12-MX_USART3_UART_Init-USART3-false-HAL-true,13-MX_UART7_Init-UART7-false-HAL-true,14-MX_USART10_UART_Init-USART10-false-HAL-true,15-MX_FDCAN1_Init-FDCAN1-false-HAL-true,16-MX_FDCAN2_Init-FDCAN2-false-HAL-true,17-MX_FDCAN3_Init-FDCAN3-false-HAL-true,18-MX_TIM1_Init-TIM1-false-HAL-true,19-MX_TIM2_Init-TIM2-false-HAL-true,20-MX_OCTOSPI1_Init-OCTOSPI1-false-HAL-true,21-MX_UART5_Init-UART5-false-HAL-true,22-MX_ADC3_Init-ADC3-false-HAL-true,23-MX_USB_OTG_HS_PCD_Init-USB_OTG_HS-false-HAL-true,0-MX_CORTEX_M7_Init-CORTEX_M7-false-HAL-true
 RCC.ADCFreq_Value=96000000
 RCC.AHB12Freq_Value=240000000
 RCC.AHB4Freq_Value=240000000
--- a/MDK-ARM/CtrBoard-H7_ALL.uvoptx
+++ b/MDK-ARM/CtrBoard-H7_ALL.uvoptx
@ -188,17 +188,7 @@
        <Ww>
          <count>6</count>
          <WinNumber>1</WinNumber>
-          <ItemText>ai</ItemText>
-        </Ww>
-        <Ww>
-          <count>7</count>
-          <WinNumber>1</WinNumber>
-          <ItemText>cmd_ai</ItemText>
-        </Ww>
-        <Ww>
-          <count>8</count>
-          <WinNumber>1</WinNumber>
-          <ItemText>gimbal</ItemText>
+          <ItemText>chassis</ItemText>
        </Ww>
      </WatchWindow1>
      <Tracepoint>
@ -334,6 +324,18 @@
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
      <bDave2>0</bDave2>
+      <PathWithFileName>../Core/Src/bdma.c</PathWithFileName>
+      <FilenameWithoutPath>bdma.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
+    <File>
+      <GroupNumber>2</GroupNumber>
+      <FileNumber>7</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
      <PathWithFileName>../Core/Src/dma.c</PathWithFileName>
      <FilenameWithoutPath>dma.c</FilenameWithoutPath>
      <RteFlg>0</RteFlg>
@ -341,7 +343,7 @@
    </File>
    <File>
      <GroupNumber>2</GroupNumber>
-      <FileNumber>7</FileNumber>
+      <FileNumber>8</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -353,7 +355,7 @@
    </File>
    <File>
      <GroupNumber>2</GroupNumber>
-      <FileNumber>8</FileNumber>
+      <FileNumber>9</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -365,7 +367,7 @@
    </File>
    <File>
      <GroupNumber>2</GroupNumber>
-      <FileNumber>9</FileNumber>
+      <FileNumber>10</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -377,7 +379,7 @@
    </File>
    <File>
      <GroupNumber>2</GroupNumber>
-      <FileNumber>10</FileNumber>
+      <FileNumber>11</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -389,7 +391,7 @@
    </File>
    <File>
      <GroupNumber>2</GroupNumber>
-      <FileNumber>11</FileNumber>
+      <FileNumber>12</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -401,7 +403,7 @@
    </File>
    <File>
      <GroupNumber>2</GroupNumber>
-      <FileNumber>12</FileNumber>
+      <FileNumber>13</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -413,7 +415,7 @@
    </File>
    <File>
      <GroupNumber>2</GroupNumber>
-      <FileNumber>13</FileNumber>
+      <FileNumber>14</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -425,7 +427,7 @@
    </File>
    <File>
      <GroupNumber>2</GroupNumber>
-      <FileNumber>14</FileNumber>
+      <FileNumber>15</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -437,7 +439,7 @@
    </File>
    <File>
      <GroupNumber>2</GroupNumber>
-      <FileNumber>15</FileNumber>
+      <FileNumber>16</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -457,7 +459,7 @@
    <RteFlg>0</RteFlg>
    <File>
      <GroupNumber>3</GroupNumber>
-      <FileNumber>16</FileNumber>
+      <FileNumber>17</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -469,7 +471,7 @@
    </File>
    <File>
      <GroupNumber>3</GroupNumber>
-      <FileNumber>17</FileNumber>
+      <FileNumber>18</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -481,7 +483,7 @@
    </File>
    <File>
      <GroupNumber>3</GroupNumber>
-      <FileNumber>18</FileNumber>
+      <FileNumber>19</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -493,7 +495,7 @@
    </File>
    <File>
      <GroupNumber>3</GroupNumber>
-      <FileNumber>19</FileNumber>
+      <FileNumber>20</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -505,7 +507,7 @@
    </File>
    <File>
      <GroupNumber>3</GroupNumber>
-      <FileNumber>20</FileNumber>
+      <FileNumber>21</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -517,7 +519,7 @@
    </File>
    <File>
      <GroupNumber>3</GroupNumber>
-      <FileNumber>21</FileNumber>
+      <FileNumber>22</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -529,7 +531,7 @@
    </File>
    <File>
      <GroupNumber>3</GroupNumber>
-      <FileNumber>22</FileNumber>
+      <FileNumber>23</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -541,7 +543,7 @@
    </File>
    <File>
      <GroupNumber>3</GroupNumber>
-      <FileNumber>23</FileNumber>
+      <FileNumber>24</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -553,7 +555,7 @@
    </File>
    <File>
      <GroupNumber>3</GroupNumber>
-      <FileNumber>24</FileNumber>
+      <FileNumber>25</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -565,7 +567,7 @@
    </File>
    <File>
      <GroupNumber>3</GroupNumber>
-      <FileNumber>25</FileNumber>
+      <FileNumber>26</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -577,7 +579,7 @@
    </File>
    <File>
      <GroupNumber>3</GroupNumber>
-      <FileNumber>26</FileNumber>
+      <FileNumber>27</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -589,7 +591,7 @@
    </File>
    <File>
      <GroupNumber>3</GroupNumber>
-      <FileNumber>27</FileNumber>
+      <FileNumber>28</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -601,7 +603,7 @@
    </File>
    <File>
      <GroupNumber>3</GroupNumber>
-      <FileNumber>28</FileNumber>
+      <FileNumber>29</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -613,7 +615,7 @@
    </File>
    <File>
      <GroupNumber>3</GroupNumber>
-      <FileNumber>29</FileNumber>
+      <FileNumber>30</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -625,7 +627,7 @@
    </File>
    <File>
      <GroupNumber>3</GroupNumber>
-      <FileNumber>30</FileNumber>
+      <FileNumber>31</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -637,7 +639,7 @@
    </File>
    <File>
      <GroupNumber>3</GroupNumber>
-      <FileNumber>31</FileNumber>
+      <FileNumber>32</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -649,7 +651,7 @@
    </File>
    <File>
      <GroupNumber>3</GroupNumber>
-      <FileNumber>32</FileNumber>
+      <FileNumber>33</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -661,7 +663,7 @@
    </File>
    <File>
      <GroupNumber>3</GroupNumber>
-      <FileNumber>33</FileNumber>
+      <FileNumber>34</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -673,7 +675,7 @@
    </File>
    <File>
      <GroupNumber>3</GroupNumber>
-      <FileNumber>34</FileNumber>
+      <FileNumber>35</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -685,7 +687,7 @@
    </File>
    <File>
      <GroupNumber>3</GroupNumber>
-      <FileNumber>35</FileNumber>
+      <FileNumber>36</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -697,7 +699,7 @@
    </File>
    <File>
      <GroupNumber>3</GroupNumber>
-      <FileNumber>36</FileNumber>
+      <FileNumber>37</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -709,7 +711,7 @@
    </File>
    <File>
      <GroupNumber>3</GroupNumber>
-      <FileNumber>37</FileNumber>
+      <FileNumber>38</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -721,7 +723,7 @@
    </File>
    <File>
      <GroupNumber>3</GroupNumber>
-      <FileNumber>38</FileNumber>
+      <FileNumber>39</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -733,7 +735,7 @@
    </File>
    <File>
      <GroupNumber>3</GroupNumber>
-      <FileNumber>39</FileNumber>
+      <FileNumber>40</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -745,7 +747,7 @@
    </File>
    <File>
      <GroupNumber>3</GroupNumber>
-      <FileNumber>40</FileNumber>
+      <FileNumber>41</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -757,7 +759,7 @@
    </File>
    <File>
      <GroupNumber>3</GroupNumber>
-      <FileNumber>41</FileNumber>
+      <FileNumber>42</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -769,7 +771,7 @@
    </File>
    <File>
      <GroupNumber>3</GroupNumber>
-      <FileNumber>42</FileNumber>
+      <FileNumber>43</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -781,7 +783,7 @@
    </File>
    <File>
      <GroupNumber>3</GroupNumber>
-      <FileNumber>43</FileNumber>
+      <FileNumber>44</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -793,7 +795,7 @@
    </File>
    <File>
      <GroupNumber>3</GroupNumber>
-      <FileNumber>44</FileNumber>
+      <FileNumber>45</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -813,7 +815,7 @@
    <RteFlg>0</RteFlg>
    <File>
      <GroupNumber>4</GroupNumber>
-      <FileNumber>45</FileNumber>
+      <FileNumber>46</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -833,7 +835,7 @@
    <RteFlg>0</RteFlg>
    <File>
      <GroupNumber>5</GroupNumber>
-      <FileNumber>46</FileNumber>
+      <FileNumber>47</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -845,7 +847,7 @@
    </File>
    <File>
      <GroupNumber>5</GroupNumber>
-      <FileNumber>47</FileNumber>
+      <FileNumber>48</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -857,7 +859,7 @@
    </File>
    <File>
      <GroupNumber>5</GroupNumber>
-      <FileNumber>48</FileNumber>
+      <FileNumber>49</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -869,7 +871,7 @@
    </File>
    <File>
      <GroupNumber>5</GroupNumber>
-      <FileNumber>49</FileNumber>
+      <FileNumber>50</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -881,7 +883,7 @@
    </File>
    <File>
      <GroupNumber>5</GroupNumber>
-      <FileNumber>50</FileNumber>
+      <FileNumber>51</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -893,7 +895,7 @@
    </File>
    <File>
      <GroupNumber>5</GroupNumber>
-      <FileNumber>51</FileNumber>
+      <FileNumber>52</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -905,7 +907,7 @@
    </File>
    <File>
      <GroupNumber>5</GroupNumber>
-      <FileNumber>52</FileNumber>
+      <FileNumber>53</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -917,7 +919,7 @@
    </File>
    <File>
      <GroupNumber>5</GroupNumber>
-      <FileNumber>53</FileNumber>
+      <FileNumber>54</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -929,7 +931,7 @@
    </File>
    <File>
      <GroupNumber>5</GroupNumber>
-      <FileNumber>54</FileNumber>
+      <FileNumber>55</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -941,7 +943,7 @@
    </File>
    <File>
      <GroupNumber>5</GroupNumber>
-      <FileNumber>55</FileNumber>
+      <FileNumber>56</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -961,7 +963,7 @@
    <RteFlg>0</RteFlg>
    <File>
      <GroupNumber>6</GroupNumber>
-      <FileNumber>56</FileNumber>
+      <FileNumber>57</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -973,7 +975,7 @@
    </File>
    <File>
      <GroupNumber>6</GroupNumber>
-      <FileNumber>57</FileNumber>
+      <FileNumber>58</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -985,7 +987,7 @@
    </File>
    <File>
      <GroupNumber>6</GroupNumber>
-      <FileNumber>58</FileNumber>
+      <FileNumber>59</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -997,7 +999,7 @@
    </File>
    <File>
      <GroupNumber>6</GroupNumber>
-      <FileNumber>59</FileNumber>
+      <FileNumber>60</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1009,7 +1011,7 @@
    </File>
    <File>
      <GroupNumber>6</GroupNumber>
-      <FileNumber>60</FileNumber>
+      <FileNumber>61</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1021,7 +1023,7 @@
    </File>
    <File>
      <GroupNumber>6</GroupNumber>
-      <FileNumber>61</FileNumber>
+      <FileNumber>62</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1033,7 +1035,7 @@
    </File>
    <File>
      <GroupNumber>6</GroupNumber>
-      <FileNumber>62</FileNumber>
+      <FileNumber>63</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1045,7 +1047,7 @@
    </File>
    <File>
      <GroupNumber>6</GroupNumber>
-      <FileNumber>63</FileNumber>
+      <FileNumber>64</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1065,7 +1067,7 @@
    <RteFlg>0</RteFlg>
    <File>
      <GroupNumber>7</GroupNumber>
-      <FileNumber>64</FileNumber>
+      <FileNumber>65</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1077,7 +1079,7 @@
    </File>
    <File>
      <GroupNumber>7</GroupNumber>
-      <FileNumber>65</FileNumber>
+      <FileNumber>66</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1089,7 +1091,7 @@
    </File>
    <File>
      <GroupNumber>7</GroupNumber>
-      <FileNumber>66</FileNumber>
+      <FileNumber>67</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1101,7 +1103,7 @@
    </File>
    <File>
      <GroupNumber>7</GroupNumber>
-      <FileNumber>67</FileNumber>
+      <FileNumber>68</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1113,7 +1115,7 @@
    </File>
    <File>
      <GroupNumber>7</GroupNumber>
-      <FileNumber>68</FileNumber>
+      <FileNumber>69</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1125,7 +1127,7 @@
    </File>
    <File>
      <GroupNumber>7</GroupNumber>
-      <FileNumber>69</FileNumber>
+      <FileNumber>70</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1137,7 +1139,7 @@
    </File>
    <File>
      <GroupNumber>7</GroupNumber>
-      <FileNumber>70</FileNumber>
+      <FileNumber>71</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1149,7 +1151,7 @@
    </File>
    <File>
      <GroupNumber>7</GroupNumber>
-      <FileNumber>71</FileNumber>
+      <FileNumber>72</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1161,7 +1163,7 @@
    </File>
    <File>
      <GroupNumber>7</GroupNumber>
-      <FileNumber>72</FileNumber>
+      <FileNumber>73</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1173,7 +1175,7 @@
    </File>
    <File>
      <GroupNumber>7</GroupNumber>
-      <FileNumber>73</FileNumber>
+      <FileNumber>74</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1185,7 +1187,7 @@
    </File>
    <File>
      <GroupNumber>7</GroupNumber>
-      <FileNumber>74</FileNumber>
+      <FileNumber>75</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1195,6 +1197,18 @@
      <RteFlg>0</RteFlg>
      <bShared>0</bShared>
    </File>
+    <File>
+      <GroupNumber>7</GroupNumber>
+      <FileNumber>76</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>..\User\component\kalman_filter.c</PathWithFileName>
+      <FilenameWithoutPath>kalman_filter.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
  </Group>

  <Group>
@ -1205,7 +1219,7 @@
    <RteFlg>0</RteFlg>
    <File>
      <GroupNumber>8</GroupNumber>
-      <FileNumber>75</FileNumber>
+      <FileNumber>77</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1217,7 +1231,7 @@
    </File>
    <File>
      <GroupNumber>8</GroupNumber>
-      <FileNumber>76</FileNumber>
+      <FileNumber>78</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1229,7 +1243,7 @@
    </File>
    <File>
      <GroupNumber>8</GroupNumber>
-      <FileNumber>77</FileNumber>
+      <FileNumber>79</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1241,7 +1255,7 @@
    </File>
    <File>
      <GroupNumber>8</GroupNumber>
-      <FileNumber>78</FileNumber>
+      <FileNumber>80</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1253,7 +1267,7 @@
    </File>
    <File>
      <GroupNumber>8</GroupNumber>
-      <FileNumber>79</FileNumber>
+      <FileNumber>81</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1265,7 +1279,7 @@
    </File>
    <File>
      <GroupNumber>8</GroupNumber>
-      <FileNumber>80</FileNumber>
+      <FileNumber>82</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1277,7 +1291,7 @@
    </File>
    <File>
      <GroupNumber>8</GroupNumber>
-      <FileNumber>81</FileNumber>
+      <FileNumber>83</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1289,7 +1303,7 @@
    </File>
    <File>
      <GroupNumber>8</GroupNumber>
-      <FileNumber>82</FileNumber>
+      <FileNumber>84</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1301,7 +1315,7 @@
    </File>
    <File>
      <GroupNumber>8</GroupNumber>
-      <FileNumber>83</FileNumber>
+      <FileNumber>85</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1313,7 +1327,7 @@
    </File>
    <File>
      <GroupNumber>8</GroupNumber>
-      <FileNumber>84</FileNumber>
+      <FileNumber>86</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1325,7 +1339,7 @@
    </File>
    <File>
      <GroupNumber>8</GroupNumber>
-      <FileNumber>85</FileNumber>
+      <FileNumber>87</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1335,6 +1349,18 @@
      <RteFlg>0</RteFlg>
      <bShared>0</bShared>
    </File>
+    <File>
+      <GroupNumber>8</GroupNumber>
+      <FileNumber>88</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>..\User\device\mrobot.c</PathWithFileName>
+      <FilenameWithoutPath>mrobot.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
  </Group>

  <Group>
@ -1345,7 +1371,7 @@
    <RteFlg>0</RteFlg>
    <File>
      <GroupNumber>9</GroupNumber>
-      <FileNumber>86</FileNumber>
+      <FileNumber>89</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1357,7 +1383,7 @@
    </File>
    <File>
      <GroupNumber>9</GroupNumber>
-      <FileNumber>87</FileNumber>
+      <FileNumber>90</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1369,7 +1395,7 @@
    </File>
    <File>
      <GroupNumber>9</GroupNumber>
-      <FileNumber>88</FileNumber>
+      <FileNumber>91</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1381,7 +1407,7 @@
    </File>
    <File>
      <GroupNumber>9</GroupNumber>
-      <FileNumber>89</FileNumber>
+      <FileNumber>92</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1401,7 +1427,7 @@
    <RteFlg>0</RteFlg>
    <File>
      <GroupNumber>10</GroupNumber>
-      <FileNumber>90</FileNumber>
+      <FileNumber>93</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1413,7 +1439,7 @@
    </File>
    <File>
      <GroupNumber>10</GroupNumber>
-      <FileNumber>91</FileNumber>
+      <FileNumber>94</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1425,7 +1451,7 @@
    </File>
    <File>
      <GroupNumber>10</GroupNumber>
-      <FileNumber>92</FileNumber>
+      <FileNumber>95</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1437,7 +1463,7 @@
    </File>
    <File>
      <GroupNumber>10</GroupNumber>
-      <FileNumber>93</FileNumber>
+      <FileNumber>96</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1449,7 +1475,7 @@
    </File>
    <File>
      <GroupNumber>10</GroupNumber>
-      <FileNumber>94</FileNumber>
+      <FileNumber>97</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1461,7 +1487,7 @@
    </File>
    <File>
      <GroupNumber>10</GroupNumber>
-      <FileNumber>95</FileNumber>
+      <FileNumber>98</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1473,7 +1499,7 @@
    </File>
    <File>
      <GroupNumber>10</GroupNumber>
-      <FileNumber>96</FileNumber>
+      <FileNumber>99</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1485,7 +1511,7 @@
    </File>
    <File>
      <GroupNumber>10</GroupNumber>
-      <FileNumber>97</FileNumber>
+      <FileNumber>100</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1497,7 +1523,7 @@
    </File>
    <File>
      <GroupNumber>10</GroupNumber>
-      <FileNumber>98</FileNumber>
+      <FileNumber>101</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1509,7 +1535,7 @@
    </File>
    <File>
      <GroupNumber>10</GroupNumber>
-      <FileNumber>99</FileNumber>
+      <FileNumber>102</FileNumber>
      <FileType>5</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1521,7 +1547,7 @@
    </File>
    <File>
      <GroupNumber>10</GroupNumber>
-      <FileNumber>100</FileNumber>
+      <FileNumber>103</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1533,7 +1559,7 @@
    </File>
    <File>
      <GroupNumber>10</GroupNumber>
-      <FileNumber>101</FileNumber>
+      <FileNumber>104</FileNumber>
      <FileType>1</FileType>
      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
@ -1543,6 +1569,38 @@
      <RteFlg>0</RteFlg>
      <bShared>0</bShared>
    </File>
+    <File>
+      <GroupNumber>10</GroupNumber>
+      <FileNumber>105</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>..\User\task\cli.c</PathWithFileName>
+      <FilenameWithoutPath>cli.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
+  </Group>
+
+  <Group>
+    <GroupName>ARM_DSP</GroupName>
+    <tvExp>0</tvExp>
+    <tvExpOptDlg>0</tvExpOptDlg>
+    <cbSel>0</cbSel>
+    <RteFlg>0</RteFlg>
+    <File>
+      <GroupNumber>11</GroupNumber>
+      <FileNumber>106</FileNumber>
+      <FileType>4</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>..\Drivers\CMSIS\DSP\Lib\ARM\arm_cortexM7lfdp_math.lib</PathWithFileName>
+      <FilenameWithoutPath>arm_cortexM7lfdp_math.lib</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
  </Group>

  <Group>
--- a/MDK-ARM/CtrBoard-H7_ALL.uvprojx
+++ b/MDK-ARM/CtrBoard-H7_ALL.uvprojx
@ -314,7 +314,7 @@
          </ArmAdsMisc>
          <Cads>
            <interw>1</interw>
-            <Optim>1</Optim>
+            <Optim>4</Optim>
            <oTime>0</oTime>
            <SplitLS>0</SplitLS>
            <OneElfS>1</OneElfS>
@ -338,9 +338,9 @@
            <v6Rtti>0</v6Rtti>
            <VariousControls>
              <MiscControls></MiscControls>
-              <Define>USE_PWR_LDO_SUPPLY,USE_HAL_DRIVER,STM32H723xx</Define>
+              <Define>USE_PWR_LDO_SUPPLY,USE_HAL_DRIVER,STM32H723xx,FPU_PRESENT=1,FPU_USED=1,ARM_MATH_CM7</Define>
              <Undefine></Undefine>
-              <IncludePath>../Core/Inc;../Drivers/STM32H7xx_HAL_Driver/Inc;../Drivers/STM32H7xx_HAL_Driver/Inc/Legacy;../Middlewares/Third_Party/FreeRTOS/Source/include;../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2;../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F;../Drivers/CMSIS/Device/ST/STM32H7xx/Include;../Drivers/CMSIS/Include;../User</IncludePath>
+              <IncludePath>../Core/Inc;../Drivers/STM32H7xx_HAL_Driver/Inc;../Drivers/STM32H7xx_HAL_Driver/Inc/Legacy;../Middlewares/Third_Party/FreeRTOS/Source/include;../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2;../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F;../Drivers/CMSIS/Device/ST/STM32H7xx/Include;../Drivers/CMSIS/Include;../User;../Middlewares/Third_Party/ARM/DSP;../Drivers/CMSIS/DSP/Include;../Middlewares/ST/ARM/DSP/Inc</IncludePath>
            </VariousControls>
          </Cads>
          <Aads>
@ -358,7 +358,7 @@
              <MiscControls></MiscControls>
              <Define></Define>
              <Undefine></Undefine>
-              <IncludePath>../Core/Inc;../Drivers/STM32H7xx_HAL_Driver/Inc;../Drivers/STM32H7xx_HAL_Driver/Inc/Legacy;../Middlewares/Third_Party/FreeRTOS/Source/include;../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2;../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F;../Drivers/CMSIS/Device/ST/STM32H7xx/Include;../Drivers/CMSIS/Include</IncludePath>
+              <IncludePath>../Core/Inc;../Drivers/STM32H7xx_HAL_Driver/Inc;../Drivers/STM32H7xx_HAL_Driver/Inc/Legacy;../Middlewares/Third_Party/FreeRTOS/Source/include;../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2;../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F;../Drivers/CMSIS/Device/ST/STM32H7xx/Include;../Drivers/CMSIS/Include;../Middlewares/ST/ARM/DSP/Inc</IncludePath>
            </VariousControls>
          </Aads>
          <LDads>
@ -414,6 +414,62 @@
              <FileType>1</FileType>
              <FilePath>../Core/Src/adc.c</FilePath>
            </File>
+            <File>
+              <FileName>bdma.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>../Core/Src/bdma.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>dma.c</FileName>
              <FileType>1</FileType>
@ -784,6 +840,11 @@
              <FileType>1</FileType>
              <FilePath>..\User\component\limiter.c</FilePath>
            </File>
+            <File>
+              <FileName>kalman_filter.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\component\kalman_filter.c</FilePath>
+            </File>
          </Files>
        </Group>
        <Group>
@ -844,6 +905,11 @@
              <FileType>1</FileType>
              <FilePath>..\User\device\vofa.c</FilePath>
            </File>
+            <File>
+              <FileName>mrobot.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\device\mrobot.c</FilePath>
+            </File>
          </Files>
        </Group>
        <Group>
@ -934,6 +1000,21 @@
              <FileType>1</FileType>
              <FilePath>..\User\task\vofa.c</FilePath>
            </File>
+            <File>
+              <FileName>cli.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\task\cli.c</FilePath>
+            </File>
+          </Files>
+        </Group>
+        <Group>
+          <GroupName>ARM_DSP</GroupName>
+          <Files>
+            <File>
+              <FileName>arm_cortexM7lfdp_math.lib</FileName>
+              <FileType>4</FileType>
+              <FilePath>..\Drivers\CMSIS\DSP\Lib\ARM\arm_cortexM7lfdp_math.lib</FilePath>
+            </File>
          </Files>
        </Group>
        <Group>
--- a/MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.axf
+++ b/MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.axf
--- a/MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.hex
+++ b/MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.hex
--- a/README.md
+++ b/README.md
@ -0,0 +1,320 @@
+# 平衡轮足机器人电控系统
+
+基于 STM32H723 的平衡轮腿机器人电控系统，采用 FreeRTOS 实时操作系统，支持 LQR、VMC 等先进控制算法。
+
+## 项目简介
+
+本项目是一个高性能的平衡轮腿机器人电控系统，实现了机器人的平衡控制、运动控制、云台控制、发射机构控制等功能。系统采用模块化设计，具有良好的可扩展性和可维护性。
+
+### 主要特性
+
+- ✅ **高性能硬件平台**：STM32H723 微控制器，480MHz 主频
+- ✅ **实时操作系统**：FreeRTOS，多任务并发处理
+- ✅ **先进控制算法**：
+  - LQR（线性二次调节器）平衡控制
+  - VMC（虚拟模型控制）五连杆运动学解算
+  - PID 控制器
+  - 卡尔曼滤波器
+- ✅ **多种功能模块**：
+  - 平衡底盘控制（轮腿结构）
+  - 云台控制（Yaw/Pitch）
+  - 发射机构控制
+  - 遥控接收（DR16）
+  - AI 视觉系统
+- ✅ **丰富的外设接口**：
+  - CAN/FDCAN 总线
+  - SPI 接口
+  - UART 串口
+  - PWM 输出
+  - ADC 采集
+- ✅ **调试工具**：
+  - CLI 命令行接口
+  - VOFA+ 数据可视化
+  - 监控任务
+
+## 硬件架构
+
+### 主控板
+- **型号**：STM32H723XG
+- **主频**：480MHz
+- **Flash**：1MB
+- **RAM**：1.4MB
+
+### 传感器
+- **IMU**：BMI088（加速度计 + 陀螺仪）
+- **遥控器**：大疆 DR16
+- **电机**：
+  - LZ 直流无刷电机（关节电机）
+  - LK 直流无刷电机（轮子电机）
+  - DM 直流无刷电机（云台电机）
+
+### 通信接口
+- **CAN/FDCAN**：电机通信、视觉通信
+- **SPI**：IMU 数据读取
+- **UART**：遥控接收、CLI、VOFA+
+
+## 软件架构
+
+### 目录结构
+
+```
+├── Core/                 # STM32 HAL 库生成的核心代码
+│   ├── Inc/             # 头文件
+│   └── Src/             # 源文件
+├── Drivers/              # STM32 驱动库
+├── User/                # 用户代码
+│   ├── bsp/             # 板级支持包
+│   │   ├── can.h/.c     # CAN/FDCAN 驱动
+│   │   ├── uart.h/.c    # UART 驱动
+│   │   ├── spi.h/.c     # SPI 驱动
+│   │   ├── gpio.h/.c    # GPIO 驱动
+│   │   ├── flash.h/.c   # Flash 操作
+│   │   └── ...
+│   ├── component/       # 通用组件
+│   │   ├── lqr.h/.c     # LQR 控制器
+│   │   ├── vmc.h/.c     # VMC 虚拟模型控制
+│   │   ├── pid.h/.c     # PID 控制器
+│   │   ├── kalman_filter.h/.c  # 卡尔曼滤波
+│   │   ├── ahrs.h/.c    # 姿态解算
+│   │   ├── filter.h/.c  # 滤波器
+│   │   ├── limiter.c    # 限幅器
+│   │   └── ...
+│   ├── device/          # 设备驱动
+│   │   ├── motor_lz.h/.c     # LZ 电机驱动
+│   │   ├── motor_lk.h/.c     # LK 电机驱动
+│   │   ├── motor_dm.h/.c     # DM 电机驱动
+│   │   ├── dr16.h/.c         # 遥控器驱动
+│   │   ├── bmi088.h/.c       # IMU 驱动
+│   │   ├── vision_bridge.c   # 视觉通信
+│   │   ├── vofa.h/.c         # VOFA+ 调试
+│   │   └── ...
+│   ├── module/          # 功能模块
+│   │   ├── balance_chassis.h/.c  # 平衡底盘
+│   │   ├── gimbal.h/.c          # 云台
+│   │   ├── shoot.h/.c           # 发射机构
+│   │   └── config.h/.c          # 配置管理
+│   └── task/            # 任务管理
+│       ├── user_task.h/.c  # 任务框架
+│       ├── rc.c            # 遥控任务
+│       ├── ctrl_chassis.c  # 底盘控制任务
+│       ├── ctrl_gimbal.c   # 云台控制任务
+│       ├── ctrl_shoot.c    # 发射控制任务
+│       ├── ai.c            # AI 任务
+│       ├── monitor.c       # 监控任务
+│       ├── cli.c           # CLI 任务
+│       └── ...
+├── utils/               # 工具和仿真
+├── MDK-ARM/            # Keil MDK 工程文件
+├── build/              # CMake 构建目录
+└── CMakeLists.txt      # CMake 构建配置
+```
+
+### 任务调度
+
+系统采用 FreeRTOS 实时多任务调度，主要任务包括：
+
+| 任务名称 | 频率 | 功能描述 |
+|---------|------|---------|
+| Task_rc | 500Hz | 遥控器数据处理 |
+| Task_atti_esit | - | 姿态估计 |
+| Task_ctrl_chassis | 500Hz | 底盘控制 |
+| Task_ctrl_gimbal | 500Hz | 云台控制 |
+| Task_ctrl_shoot | 500Hz | 发射控制 |
+| Task_ai | 500Hz | AI 视觉处理 |
+| Task_monitor | 500Hz | 系统监控 |
+| Task_vofa | 500Hz | VOFA+ 数据发送 |
+| Task_cli | - | CLI 命令行处理 |
+| Task_blink | 500Hz | LED 指示灯 |
+
+### 核心算法
+
+#### 1. LQR 控制
+
+线性二次调节器（Linear Quadratic Regulator）用于实现轮腿机器人的平衡控制。
+
+**状态向量**：
+- theta：摆杆角度
+- d_theta：摆杆角速度
+- x：驱动轮位移
+- d_x：驱动轮速度
+- phi：机体俯仰角
+- d_phi：机体俯仰角速度
+
+**控制输出**：
+- T：轮毂力矩
+- Tp：髋关节力矩
+
+#### 2. VMC 控制
+
+虚拟模型控制（Virtual Model Control）实现五连杆运动学解算和力矩分配。
+
+**功能**：
+- 五连杆正运动学解算（关节角度 → 足端位置）
+- 五连杆逆运动学解算（虚拟力 → 关节力矩）
+- 等效摆动杆参数计算
+- 地面接触检测
+
+#### 3. 平衡控制流程
+
+```
+遥控输入 → 运动指令 → LQR 控制 → VMC 解算 → 电机输出 → 平衡控制
+              ↓
+         姿态反馈
+```
+
+## 编译环境
+
+### 推荐开发环境
+
+1. **IDE**：
+   - Keil MDK-ARM 5.x
+   - VS Code + Cortex-Debug 插件
+
+2. **编译工具链**：
+   - ARM GCC Toolchain (arm-none-eabi-gcc)
+   - CMake 3.22+
+
+3. **调试工具**：
+   - ST-Link V2/V3
+   - J-Link
+
+### 依赖库
+
+- STM32 HAL 库
+- FreeRTOS
+- CMSIS DSP 库
+
+## 快速开始
+
+### 1. 克隆项目
+
+```bash
+git clone ssh://git@gitea.qutrobot.top:222/robofish/rm_balance.git
+cd balance_infantry
+```
+
+### 2. 使用 Keil MDK 编译
+
+1. 打开 `MDK-ARM/CtrBoard-H7_ALL.uvprojx`
+2. 选择正确的编译配置（Debug/Release）
+3. 点击编译（F7）
+4. 下载到开发板（F8）
+
+### 3. 使用 CMake 编译
+
+```bash
+# 创建构建目录
+mkdir -p build && cd build
+
+# 配置 CMake
+cmake ..
+
+# 编译
+make -j4
+
+# 生成 HEX 文件位于 build 目录
+```
+
+### 4. 使用 VS Code 开发
+
+项目已配置 `.clangd`，支持：
+- 智能代码补全
+- 语法检查
+- 跳转到定义
+- 重构功能
+
+## 配置说明
+
+### 参数配置
+
+主要参数配置文件位于 `User/module/config.c`，包括：
+
+- **底盘参数**（`Chassis_Params_t`）：
+  - 电机参数
+  - VMC 参数
+  - LQR 增益矩阵
+  - PID 参数
+  - 跳跃参数
+
+- **云台参数**（`Gimbal_Params_t`）：
+  - 电机参数
+  - PID 参数
+
+- **发射参数**（`Shoot_Params_t`）：
+  - 摩擦轮参数
+  - 拨弹轮参数
+
+### 配置文件格式
+
+项目使用 YAML 格式的配置文件（部分模块）：
+- `User/task/config.yaml`
+- `User/bsp/bsp_config.yaml`
+- `User/component/component_config.yaml`
+- `User/device/device_config.yaml`
+
+## 调试工具
+
+### 1. CLI 命令行
+
+通过串口连接，可使用以下命令：
+
+```bash
+help                    # 显示帮助信息
+status                  # 显示系统状态
+motor                   # 电机控制
+chassis                 # 底盘控制
+gimbal                  # 云台控制
+```
+
+### 2. VOFA+ 数据可视化
+
+支持实时数据监控和绘图，配置串口参数后即可使用。
+
+### 3. 监控任务
+
+`Task_monitor` 实时监控系统状态，包括：
+- 电池电压
+- CPU 温度
+- 任务运行频率
+- 栈使用情况
+
+## 注意事项
+
+### 安全事项
+
+⚠️ **重要**：
+1. 首次运行前务必检查电机方向和限位
+2. 调试时使用低电压，避免失控
+3. 确保 IMU 安装正确，校准零点
+4. 测试跳跃功能时做好防护
+
+### 常见问题
+
+1. **编译错误**：
+   - 检查 ARM GCC 工具链是否正确安装
+   - 确认 CMake 版本 >= 3.22
+
+2. **下载失败**：
+   - 检查 ST-Link/J-Link 连接
+   - 确认 BOOT 模式设置正确
+
+3. **平衡控制失效**：
+   - 检查 IMU 数据是否正常
+   - 确认 LQR 参数是否合理
+   - 验证电机反馈数据
+
+## 贡献指南
+
+欢迎提交 Issue 和 Pull Request！
+
+## 许可证
+
+本项目采用 MIT 许可证。
+
+## 联系方式
+
+- 项目地址：ssh://git@gitea.qutrobot.top:222/robofish/rm_balance.git
+
+---
+
+**祝您使用愉快！**
--- a/USAGE.md
+++ b/USAGE.md
@ -0,0 +1,733 @@
+# 平衡轮足机器人使用指南
+
+本指南提供详细的操作步骤，帮助您快速上手使用平衡轮足机器人系统。
+
+## 目录
+
+- [环境准备](#环境准备)
+- [编译与烧录](#编译与烧录)
+- [系统启动](#系统启动)
+- [操作说明](#操作说明)
+- [调试与监控](#调试与监控)
+- [参数调优](#参数调优)
+- [故障排查](#故障排查)
+
+---
+
+## 环境准备
+
+### 1. 硬件准备
+
+**必需设备**：
+- STM32H723 控制板
+- ST-Link V2/V3 或 J-Link 调试器
+- 电池（推荐 24V 锂电池）
+- 大疆 DR16 遥控器
+- USB 转 TTL 串口模块（用于 CLI 调试）
+- 各类电机（LZ、LK、DM）
+
+**可选设备**：
+- VOFA+ 数据可视化软件
+- 示波器
+- 万用表
+
+### 2. 软件准备
+
+**开发工具**：
+- [Keil MDK-ARM 5.x](https://www.keil.com/download/product/)（推荐）
+- 或 [VS Code](https://code.visualstudio.com/) + [Cortex-Debug](https://marketplace.visualstudio.com/items?itemName=marus25.cortex-debug)
+- [STM32CubeMX](https://www.st.com/zh/development-tools/stm32cubemx.html)
+
+**调试工具**：
+- [VOFA+](https://www.vofa.plus/) - 数据可视化
+- 串口调试助手（如 SecureCRT、XShell、Putty）
+
+**编译工具链**（如使用 CMake 编译）：
+- ARM GCC Toolchain: [下载地址](https://developer.arm.com/downloads/-/gnu-rm)
+- CMake: [下载地址](https://cmake.org/download/)
+
+### 3. 驱动安装
+
+1. **ST-Link 驱动**：
+   - 下载 [ST-Link 驱动包](https://www.st.com/zh/development-tools/stsw-link009.html)
+   - 安装完成后，设备管理器中应能看到 ST-Link 设备
+
+2. **串口驱动**（如使用 CH340）：
+   - 下载并安装对应芯片的 USB 串口驱动
+
+---
+
+## 编译与烧录
+
+### 方法一：使用 Keil MDK
+
+#### 步骤 1：打开项目
+
+1. 启动 Keil MDK
+2. 打开项目文件：`MDK-ARM/CtrBoard-H7_ALL.uvprojx`
+
+#### 步骤 2：配置编译选项
+
+1. 点击菜单栏 `Project` → `Options for Target`
+2. 在 `Target` 选项卡中确认：
+   - MCU: STM32H723XG
+   - ARM Compiler: 版本 6.x 或更高
+3. 在 `Debug` 选项卡中选择调试器：
+   - 使用 `ST-Link Debugger` 或 `J-Link Debugger`
+4. 在 `Flash Download` 选项卡中确认：
+   - Programming Algorithm: STM32H7x3 1MB Flash
+   - 选中 `Reset and Run` 选项
+
+#### 步骤 3：编译项目
+
+1. 点击 `Project` → `Build Target`（或按 F7）
+2. 等待编译完成，检查是否有错误
+3. 成功后会在 `MDK-ARM/CtrBoard-H7_ALL/` 目录生成 `.hex` 文件
+
+#### 步骤 4：烧录程序
+
+1. 连接 ST-Link 到控制板
+2. 点击 `Flash` → `Download`（或按 F8）
+3. 等待下载完成，提示 "Programming Done"
+4. 控制板会自动复位运行
+
+### 方法二：使用 CMake
+
+#### 步骤 1：配置构建环境
+
+```bash
+# 进入项目根目录
+cd balance_infantry
+
+# 创建并进入构建目录
+mkdir build && cd build
+
+# 配置 CMake
+cmake ..
+```
+
+#### 步骤 2：编译项目
+
+```bash
+# 使用 4 个线程并行编译
+make -j4
+
+# 编译完成后，生成的文件位于 build 目录
+```
+
+#### 步骤 3：烧录程序
+
+使用 OpenOCD 或 ST-Link 工具烧录：
+
+```bash
+# 使用 OpenOCD 烧录（需要先安装 openocd）
+openocd -f interface/stlink.cfg -f target/stm32h7x.cfg \
+        -c "program CtrBoard-H7_ALL.hex verify reset exit"
+```
+
+或使用 ST-LINK Utility 图形界面工具。
+
+---
+
+## 系统启动
+
+### 1. 首次启动检查清单
+
+⚠️ **重要**：首次启动前请务必完成以下检查：
+
+- [ ] 电池电压正常（推荐 22V-28V）
+- [ ] 所有电机连接正确，没有短路
+- [ ] IMU 安装稳固，方向正确
+- [ ] 遥控器已配对，电量充足
+- [ ] 电机 ID 配置正确（参考 `User/device/` 目录下电机驱动）
+- [ ] 机械结构紧固，无松动
+- [ ] 运动空间充足，无障碍物
+
+### 2. 上电启动流程
+
+1. **连接电源**：
+   - 将电池连接到电源输入端
+   - 确认电压稳定在正常范围
+
+2. **系统初始化**：
+   - 系统会自动运行初始化任务（`Task_Init`）
+   - LED 指示灯开始闪烁
+   - 电机进行零点校准
+
+3. **等待就绪**：
+   - 等待约 2-3 秒，系统进入待机状态
+   - LED 常亮或按固定频率闪烁（根据代码配置）
+
+4. **连接遥控器**：
+   - 打开 DR16 遥控器
+   - 确认遥控器与接收器已连接
+   - 遥控器 LED 常绿表示连接成功
+
+### 3. 模式选择
+
+系统支持多种控制模式，通过遥控器切换：
+
+| 模式 | 说明 | 切换方式 |
+|------|------|---------|
+| 放松模式 | 电机不输出，用于调试 | 遥控器左摇杆按下 |
+| 复位模式 | 机器人回到初始位置 | 遥控器特定按键 |
+| 平衡模式 | 机器人自我平衡 | 遥控器特定按键 |
+| 小陀螺模式 | 陀螺仪辅助平衡 | 遥控器特定按键 |
+
+**具体按键配置请参考 `User/task/rc.c` 文件。**
+
+---
+
+## 操作说明
+
+### 1. 基本运动控制
+
+#### 遥控器操作（DR16）
+
+```
+左摇杆：
+  - 前后推拉：前进/后退
+  - 左右推拉：平移
+  - 按下：模式切换
+
+右摇杆：
+  - 前后推拉：云台 Pitch 轴控制
+  - 左右推拉：云台 Yaw 轴控制
+  - 按下：发射机构控制
+
+左拨轮：调节目标高度
+
+右拨轮：调节运动速度
+```
+
+#### 注意事项
+
+1. **首次操作**：
+   - 从低速开始，逐步熟悉控制手感
+   - 保持足够的缓冲空间
+   - 随时准备切换到放松模式
+
+2. **平衡模式**：
+   - 机器人会自动保持平衡
+   - 输入较小的运动指令测试响应
+   - 观察机器人姿态是否稳定
+
+3. **高度控制**：
+   - 使用左拨轮调整目标高度
+   - 高度变化应平缓，避免突变
+
+### 2. 云台控制
+
+#### 云台 Yaw 轴控制
+
+- 使用遥控器右摇杆左右推拉
+- 控制底盘旋转角度
+- 平稳转动，避免快速摆动
+
+#### 云台 Pitch 轴控制
+
+- 使用遥控器右摇杆前后推拉
+- 控制发射机构俯仰角
+- 注意角度限制，避免碰撞
+
+### 3. 发射机构控制
+
+#### 开启发射机构
+
+1. 切换到发射模式
+2. 按下右摇杆（或特定按键）启动摩擦轮
+3. 等待摩擦轮达到稳定转速
+4. 再次按键发射弹丸
+
+#### 注意事项
+
+- ⚠️ 发射前确保前方安全，无人员
+- ⚠️ 不使用时及时关闭发射机构
+- 定期检查摩擦轮磨损情况
+
+### 4. 跳跃功能
+
+#### 触发跳跃
+
+跳跃功能仅在 `CHASSIS_MODE_WHELL_LEG_BALANCE` 模式下可用。
+
+**跳跃流程**：
+
+1. **蓄力阶段（JUMP_CROUCH）**：
+   - 机器人降低重心，缩短腿长
+   - 蓄力时间：`jump_params.crouch_time_ms`（默认配置）
+
+2. **起跳阶段（JUMP_LAUNCH）**：
+   - 机器人发力向下，产生向上推力
+   - 发力时间：`jump_params.launch_time_ms`
+
+3. **收腿阶段（JUMP_RETRACT）**：
+   - 腿部收缩，准备落地
+   - 收腿时间：`jump_params.retract_time_ms`
+
+4. **落地阶段（JUMP_LAND）**：
+   - 缓冲着陆，恢复平衡
+   - 缓冲时间：`jump_params.land_time_ms`
+
+#### 跳跃参数调整
+
+在 `User/module/config.c` 中修改跳跃参数：
+
+```c
+.jump_params = {
+    .crouch_time_ms = 500,       // 蓄力时间 (ms)
+    .launch_time_ms = 100,      // 起跳发力时间 (ms)
+    .retract_time_ms = 200,     // 收腿时间 (ms)
+    .land_time_ms = 300,        // 落地缓冲时间 (ms)
+    .crouch_leg_length = 0.3,   // 蓄力时腿长 (m)
+    .launch_force = 500.0,      // 起跳力 (N)
+    .retract_leg_length = 0.2,  // 收腿时腿长 (m)
+    .retract_force = -100.0,    // 收腿前馈力 (N)
+},
+```
+
+**调试建议**：
+- 先调整蓄力时间和腿长，找到合适的蓄力状态
+- 逐步增加起跳力，避免跳得太高
+- 根据实际表现调整各阶段时间
+
+---
+
+## 调试与监控
+
+### 1. CLI 命令行调试
+
+#### 连接方式
+
+1. 使用 USB 转 TTL 串口模块连接到开发板的串口
+2. 串口参数配置：
+   - 波特率：115200
+   - 数据位：8
+   - 停止位：1
+   - 校验位：无
+3. 使用串口调试工具打开对应 COM 口
+
+#### 常用命令
+
+```bash
+# 显示帮助信息
+help
+
+# 查看系统状态
+status
+# 输出示例：
+# Battery: 24.5V (80%)
+# CPU Temp: 45°C
+# Tasks: Running normally
+
+# 电机控制
+motor
+# 子命令：
+#   list          列出所有电机状态
+#   calibrate [id] 校准指定电机
+#   enable [id]   启用指定电机
+#   disable [id]  禁用指定电机
+
+# 底盘控制
+chassis
+# 子命令：
+#   mode [mode]   设置底盘模式
+#   balance       进入平衡模式
+#   relax         进入放松模式
+
+# 云台控制
+gimbal
+# 子命令：
+#   reset         复位云台
+#   calibrate     校准云台
+
+# 监控任务信息
+monitor
+# 输出各任务运行频率、栈使用情况
+```
+
+### 2. VOFA+ 数据可视化
+
+#### 配置步骤
+
+1. 打开 VOFA+ 软件
+2. 新建工程，选择 "JustFloat" 协议
+3. 配置串口：
+   - 波特率：115200
+   - 数据位：8
+   - 停止位：1
+4. 点击连接按钮
+
+#### 可视化数据
+
+系统会自动发送以下数据：
+
+- IMU 数据：加速度、陀螺仪、欧拉角
+- 电机数据：位置、速度、电流
+- 控制输出：LQR 输出、VMC 输出
+- 系统状态：电池电压、CPU 温度
+
+#### 数据导出
+
+- 点击 "录制" 按钮开始记录数据
+- 停止录制后，数据可导出为 CSV 格式
+- 使用 Excel、MATLAB 等工具分析数据
+
+### 3. 监控任务
+
+`Task_monitor` 实时监控系统状态，包括：
+
+**监控内容**：
+- 电池电压和电量百分比
+- CPU 温度
+- 各任务运行频率
+- 任务栈使用情况
+- 设备在线状态
+
+**查看方式**：
+- 通过 CLI 命令 `monitor`
+- 或通过 VOFA+ 实时查看
+
+**正常指标**：
+- 电池电压：22V - 28V
+- CPU 温度：< 70°C
+- 任务频率：稳定在设定值（如 500Hz）
+- 栈使用率：< 80%
+
+---
+
+## 参数调优
+
+### 1. LQR 参数调优
+
+#### LQR 增益矩阵
+
+LQR 控制器的增益矩阵存储在 `User/module/config.c` 中：
+
+```c
+LQR_GainMatrix_t lqr_gains = {
+    // K 矩阵第一行（轮毂力矩 T）
+    .k11_coeff = { ... },  // theta 的增益
+    .k12_coeff = { ... },  // d_theta 的增益
+    .k13_coeff = { ... },  // x 的增益
+    .k14_coeff = { ... },  // d_x 的增益
+    .k15_coeff = { ... },  // phi 的增益
+    .k16_coeff = { ... },  // d_phi 的增益
+    
+    // K 矩阵第二行（髋关节力矩 Tp）
+    .k21_coeff = { ... },
+    .k22_coeff = { ... },
+    .k23_coeff = { ... },
+    .k24_coeff = { ... },
+    .k25_coeff = { ... },
+    .k26_coeff = { ... },
+};
+```
+
+#### 调优方法
+
+**步骤 1：确定目标**
+- 平衡性：机器人能稳定平衡
+- 响应性：对指令响应迅速
+- 稳定性：无明显振荡
+
+**步骤 2：逐步调整**
+1. 从小增益开始，逐步增大
+2. 先调整 theta 和 phi 的增益（姿态控制）
+3. 再调整 x 的增益（位置控制）
+4. 最后调整角速度的增益（阻尼）
+
+**步骤 3：测试验证**
+- 每次调整后重新编译烧录
+- 测试平衡性能
+- 使用 VOFA+ 观察响应曲线
+
+**调优技巧**：
+- 增益过小：响应慢，容易失衡
+- 增益过大：振荡，不稳定
+- 角速度增益影响阻尼，防止振荡
+- 位置增益影响响应速度
+
+### 2. PID 参数调优
+
+#### PID 控制器位置
+
+系统中有多个 PID 控制器：
+- Yaw 轴 PID（`pid.yaw`）
+- Roll 轴 PID（`pid.roll`）
+- 摆力矩 PID（`pid.tp`）
+- 腿长 PID（`pid.leg_length`）
+- 摆角 PID（`pid.leg_theta`）
+
+#### 调优步骤（经典方法）
+
+**步骤 1：设置初始值**
+```
+Kp = 0
+Ki = 0
+Kd = 0
+```
+
+**步骤 2：调整 Kp**
+- 逐步增大 Kp
+- 直到系统开始振荡
+- 将 Kp 减半（约为临界值的 50%-70%）
+
+**步骤 3：调整 Kd**
+- 增大 Kd
+- 减小振荡
+- 直到振荡消失
+- 保持适当的阻尼
+
+**步骤 4：调整 Ki**
+- 增大 Ki
+- 消除稳态误差
+- 避免积分饱和
+
+**步骤 5：微调**
+- 小幅调整三个参数
+- 找到最佳平衡点
+
+#### 参数配置示例
+
+```c
+KPID_Params_t yaw = {
+    .kp = 5.0,
+    .ki = 0.1,
+    .kd = 0.5,
+    .integral_limit = 10.0,
+    .output_limit = 10.0,
+};
+```
+
+### 3. VMC 参数调优
+
+#### VMC 运动学参数
+
+```c
+VMC_Param_t vmc_param = {
+    .hip_length = 0.15,      // 髋关节间距 (m)
+    .leg_1 = 0.2,            // 大腿前端长度 (m)
+    .leg_2 = 0.2,            // 大腿后端长度 (m)
+    .leg_3 = 0.25,           // 小腿长度 (m)
+    .leg_4 = 0.1,            // 小腿前端长度 (m)
+    .wheel_radius = 0.06,    // 轮子半径 (m)
+    .wheel_mass = 0.5,       // 轮子质量 (kg)
+};
+```
+
+#### 调优要点
+
+1. **运动学参数**：
+   - 根据实际机械尺寸精确测量
+   - 使用 MATLAB 工具（`utils/Simulation-master/`）验证
+   - 确保单位一致（全部使用米）
+
+2. **滤波参数**：
+   - 调整 `low_pass_cutoff_freq` 优化信号质量
+   - 太高频：噪声大
+   - 太低频：响应慢
+
+### 4. 使用工具辅助调优
+
+项目提供 MATLAB 仿真工具：
+
+1. **LQR 增益计算**：
+   - 运行 `utils/6. 综合运动控制验证/LQR_K.m`
+   - 输入系统参数
+   - 自动计算最优 LQR 增益
+
+2. **运动学仿真**：
+   - 使用 `utils/Simulation-master/` 中的 MATLAB 模型
+   - 验证五连杆运动学
+   - 测试不同参数组合
+
+3. **参数优化脚本**：
+   - 使用 `utils/k_calc/` 中的脚本
+   - 批量测试参数
+   - 找到最优解
+
+---
+
+## 故障排查
+
+### 1. 系统无法启动
+
+**症状**：上电后无反应，LED 不亮
+
+**可能原因**：
+- 电池电压不足或未连接
+- 电源线路故障
+- MCU 未烧录程序
+
+**解决方法**：
+1. 检查电池电压是否正常（22V-28V）
+2. 检查电源线路是否接触良好
+3. 重新烧录程序
+
+### 2. 电机不转动
+
+**症状**：系统运行正常，但电机不转动
+
+**可能原因**：
+- 电机 ID 配置错误
+- CAN 通信故障
+- 电机未使能
+- 电机驱动板故障
+
+**解决方法**：
+1. 使用 CLI 命令 `motor list` 检查电机状态
+2. 检查 CAN 线连接
+3. 确认电机 ID 配置正确
+4. 使用 `motor enable [id]` 使能电机
+5. 检查电机驱动板供电
+
+### 3. IMU 数据异常
+
+**症状**：姿态角不稳定或数据异常
+
+**可能原因**：
+- IMU 供电不稳定
+- IMU 安装不牢固
+- SPI 通信错误
+- IMU 未校准
+
+**解决方法**：
+1. 检查 IMU 供电电压（3.3V）
+2. 重新安装 IMU，确保稳固
+3. 检查 SPI 线连接
+4. 使用 CLI 命令校准 IMU
+5. 更新 AHRS 算法参数
+
+### 4. 机器人无法平衡
+
+**症状**：机器人启动平衡模式后立即倒下
+
+**可能原因**：
+- LQR 参数不合理
+- IMU 数据异常
+- 电机响应不足
+- 机械结构问题
+
+**解决方法**：
+1. 检查 IMU 数据是否正常
+2. 使用 VOFA+ 观察姿态角变化
+3. 重新调整 LQR 参数（参考参数调优章节）
+4. 检查电机输出能力
+5. 检查机械结构，确保无松动
+
+### 5. 通信故障
+
+**症状**：遥控器失联或 VOFA+ 无法连接
+
+**可能原因**：
+- 遥控器未配对
+- 串口参数错误
+- 通信线路故障
+- 任务未正常运行
+
+**解决方法**：
+1. 重新配对 DR16 遥控器
+2. 检查串口参数（115200, 8N1）
+3. 检查通信线路
+4. 使用 CLI 命令 `monitor` 查看任务状态
+
+### 6. 编译错误
+
+**症状**：编译时出现错误
+
+**可能原因**：
+- 工具链版本不兼容
+- CMake 版本过低
+- 路径问题
+- 代码语法错误
+
+**解决方法**：
+1. 更新 ARM GCC 到最新版本
+2. 确认 CMake 版本 >= 3.22
+3. 检查项目路径是否有中文或特殊字符
+4. 查看错误信息，定位代码问题
+5. 清理构建目录后重新编译：`make clean && make`
+
+### 7. 跳跃功能异常
+
+**症状**：跳跃后无法落地或姿态失控
+
+**可能原因**：
+- 跳跃参数不合理
+- 腿长限位设置错误
+- 落地检测失效
+- 控制器响应不足
+
+**解决方法**：
+1. 降低起跳力，从较小值开始测试
+2. 调整各阶段时间参数
+3. 检查腿长限位是否合理
+4. 优化落地缓冲参数
+5. 使用 VOFA+ 观察跳跃过程数据
+
+### 获取更多帮助
+
+如果以上方法无法解决问题：
+
+1. **查看日志**：
+   - 使用 VOFA+ 记录故障时的数据
+   - 使用 CLI 命令查看详细状态
+
+2. **查阅文档**：
+   - 检查代码注释
+   - 查看 `utils/` 目录中的技术文档
+
+3. **提交 Issue**：
+   - 在项目仓库提交详细的问题描述
+   - 包含：硬件型号、软件版本、错误日志、重现步骤
+
+---
+
+## 附录
+
+### A. 术语表
+
+| 术语 | 全称 | 说明 |
+|------|------|------|
+| LQR | Linear Quadratic Regulator | 线性二次调节器 |
+| VMC | Virtual Model Control | 虚拟模型控制 |
+| IMU | Inertial Measurement Unit | 惯性测量单元 |
+| PID | Proportional-Integral-Derivative | 比例-积分-微分控制器 |
+| CAN | Controller Area Network | 控制器局域网 |
+| FDCAN | Flexible Data-rate CAN | 灵活数据速率 CAN |
+| CLI | Command Line Interface | 命令行接口 |
+| AHRS | Attitude and Heading Reference System | 姿态和航向参考系统 |
+
+### B. 串口引脚定义
+
+| 串口 | TX | RX | 功能 |
+|------|----|----|------|
+| USART1 | PA9 | PA10 | CLI 调试 |
+| USART2 | PA2 | PA3 | VOFA+ 数据 |
+| USART3 | PB10 | PB11 | 视觉通信 |
+| UART4 | PA0 | PA1 | 遥控接收 |
+
+### C. 电机 ID 分配
+
+| 电机类型 | ID | 位置 | 用途 |
+|---------|----|----|------|
+| LZ 电机 | 1-4 | 关节 | 髋关节控制 |
+| LK 电机 | 1-2 | 轮子 | 驱动轮控制 |
+| DM 电机 | 1 | 云台 | Yaw 轴控制 |
+| DM 电机 | 2 | 云台 | Pitch 轴控制 |
+
+### D. 常用按键映射
+
+| 按键 | 功能 | 说明 |
+|------|------|------|
+| 左摇杆按下 | 模式切换 | 在不同控制模式间切换 |
+| 右摇杆按下 | 发射控制 | 控制发射机构开关 |
+| 左拨轮 | 高度调节 | 调节机器人目标高度 |
+| 右拨轮 | 速度调节 | 调节运动速度上限 |
+
+---
+
+**祝您调试顺利！**
+
+如有问题，请参考 README.md 中的联系方式获取支持。
--- a/User/bsp/fdcan.c
+++ b/User/bsp/fdcan.c
@ -21,19 +21,19 @@
  #define FDCAN1_FILTER_CONFIG_TABLE(X) \
    X(0,       FDCAN_STANDARD_ID,   FDCAN_FILTER_MASK,   0x000     ,   0x000     ,   0) \
    X(1,       FDCAN_EXTENDED_ID,   FDCAN_FILTER_MASK,   0x00000000,   0x00000000,   0) 
-    #define FDCAN1_GLOBAL_FILTER FDCAN_REJECT, FDCAN_REJECT, FDCAN_FILTER_REMOTE, FDCAN_FILTER_REMOTE/* 全局过滤器参数（用于 HAL_FDCAN_ConfigGlobalFilter） */
+  #define FDCAN1_GLOBAL_FILTER FDCAN_ACCEPT_IN_RX_FIFO0, FDCAN_ACCEPT_IN_RX_FIFO0, FDCAN_FILTER_REMOTE, FDCAN_FILTER_REMOTE
 #endif
 #ifdef FDCAN2_EN
  #define FDCAN2_FILTER_CONFIG_TABLE(X) \
    X(0,       FDCAN_STANDARD_ID,   FDCAN_FILTER_MASK,   0x000     ,   0x000     ,   0) \
    X(1,       FDCAN_EXTENDED_ID,   FDCAN_FILTER_MASK,   0x00000000,   0x00000000,   0) 
-    #define FDCAN2_GLOBAL_FILTER FDCAN_ACCEPT_IN_RX_FIFO1, FDCAN_ACCEPT_IN_RX_FIFO1, FDCAN_FILTER_REMOTE, FDCAN_FILTER_REMOTE/* 全局过滤器参数（用于 HAL_FDCAN_ConfigGlobalFilter） */
+  #define FDCAN2_GLOBAL_FILTER FDCAN_ACCEPT_IN_RX_FIFO1, FDCAN_ACCEPT_IN_RX_FIFO1, FDCAN_FILTER_REMOTE, FDCAN_FILTER_REMOTE
 #endif
 #ifdef FDCAN3_EN
  #define FDCAN3_FILTER_CONFIG_TABLE(X) \
    X(0,       FDCAN_STANDARD_ID,   FDCAN_FILTER_MASK,   0x000     ,   0x000     ,   0) \
    X(1,       FDCAN_EXTENDED_ID,   FDCAN_FILTER_MASK,   0x00000000,   0x00000000,   0) 
-  #define FDCAN3_GLOBAL_FILTER FDCAN_REJECT, FDCAN_ACCEPT_IN_RX_FIFO1, FDCAN_FILTER_REMOTE, FDCAN_FILTER_REMOTE/* 全局过滤器参数（用于 HAL_FDCAN_ConfigGlobalFilter） */
+  #define FDCAN3_GLOBAL_FILTER FDCAN_ACCEPT_IN_RX_FIFO1, FDCAN_ACCEPT_IN_RX_FIFO1, FDCAN_FILTER_REMOTE, FDCAN_FILTER_REMOTE
 #endif

 /* ====宏展开实现==== */
--- a/User/bsp/gpio.c
+++ b/User/bsp/gpio.c
@ -90,6 +90,7 @@ int8_t BSP_GPIO_EnableIRQ(BSP_GPIO_t gpio) {
    default:
      return BSP_ERR;
  }
+  return BSP_OK;
 }

 int8_t BSP_GPIO_DisableIRQ(BSP_GPIO_t gpio) {
@ -107,6 +108,7 @@ int8_t BSP_GPIO_DisableIRQ(BSP_GPIO_t gpio) {
    default:
      return BSP_ERR;
  }
+  return BSP_OK;
 }
 int8_t BSP_GPIO_WritePin(BSP_GPIO_t gpio, bool value){
  if (gpio >= BSP_GPIO_NUM) return BSP_ERR;
--- a/User/device/bmi088.h
+++ b/User/device/bmi088.h
@ -1,6 +1,5 @@
 #pragma once

-#include <sys/_intsup.h>
 #ifdef __cplusplus
 extern "C" {
 #endif
@ -31,7 +30,6 @@ typedef struct {
  } gyro_offset; /* 陀螺仪偏置 */
 } BMI088_Cali_t; /* BMI088校准数据 */

-
 typedef struct {
  DEVICE_Header_t header;
  AHRS_Accl_t accl;
--- a/User/device/device.h
+++ b/User/device/device.h
@ -1,6 +1,5 @@
 #pragma once

-#include <sys/_intsup.h>
 #ifdef __cplusplus
 extern "C" {
 #endif
--- a/User/device/device_config.yaml
+++ b/User/device/device_config.yaml
@ -29,6 +29,10 @@ motor_lz:
 motor_rm:
  bsp_config: {}
  enabled: true
+mrobot:
+  bsp_config:
+    BSP_UART_MROBOT: BSP_UART_VOFA
+  enabled: true
 vofa:
  bsp_config:
    BSP_UART_VOFA: BSP_UART_VOFA
--- a/User/device/mrobot.c
+++ b/User/device/mrobot.c
@ -7,6 +7,7 @@
 #include "device/mrobot.h"
 #include "component/freertos_cli.h"
 #include "bsp/uart.h"
+#include "bsp/mm.h"
 #include <string.h>
 #include <stdio.h>
 #include <stdlib.h>
@ -119,7 +120,7 @@ static void send_string(const char *str) {
    if (str == NULL || *str == '\0') return;
    
    ctx.tx_complete = false;
-    BSP_UART_Transmit(MROBOT_UART_PORT, (uint8_t *)str, strlen(str), true);
+    BSP_UART_Transmit(BSP_UART_VOFA, (uint8_t *)str, strlen(str), true);
    while (!ctx.tx_complete) { osDelay(1); }
 }

@ -150,7 +151,7 @@ static void uart_rx_callback(void) {
        if (ch == 'q' || ch == 'Q' || ch == 27) {
            ctx.htop_exit = true;
        }
-        BSP_UART_Receive(MROBOT_UART_PORT, &ctx.uart_rx_char, 1, false);
+        BSP_UART_Receive(BSP_UART_VOFA, &ctx.uart_rx_char, 1, false);
        return;
    }
    
@ -159,19 +160,20 @@ static void uart_rx_callback(void) {
        if (ctx.cmd_index > 0) {
            ctx.cmd_buffer[ctx.cmd_index] = '\0';
            ctx.cmd_ready = true;
-            BSP_UART_Transmit(MROBOT_UART_PORT, (uint8_t *)"\r\n", 2, false);
+            BSP_UART_Transmit(BSP_UART_VOFA, (uint8_t *)"\r\n", 2, false);
        }
    } else if (ch == 127 || ch == 8) {  /* 退格键 */
        if (ctx.cmd_index > 0) {
            ctx.cmd_index--;
-            BSP_UART_Transmit(MROBOT_UART_PORT, (uint8_t *)ANSI_BACKSPACE, 3, false);
+            BSP_UART_Transmit(BSP_UART_VOFA, (uint8_t *)ANSI_BACKSPACE, 3, false);
        }
    } else if (ch >= 32 && ch < 127 && ctx.cmd_index < sizeof(ctx.cmd_buffer) - 1) {
        ctx.cmd_buffer[ctx.cmd_index++] = ch;
-        BSP_UART_Transmit(MROBOT_UART_PORT, &ch, 1, false);
+        /* 回显：使用静态变量地址，避免异步发送时局部变量已失效 */
+        BSP_UART_Transmit(BSP_UART_VOFA, &ctx.uart_rx_char, 1, false);
    }
    
-    BSP_UART_Receive(MROBOT_UART_PORT, &ctx.uart_rx_char, 1, false);
+    BSP_UART_Receive(BSP_UART_VOFA, &ctx.uart_rx_char, 1, false);
 }

 /* ========================================================================== */
@ -482,9 +484,8 @@ void MRobot_Init(void) {
    /* 创建互斥锁 */
    ctx.mutex = xSemaphoreCreateMutex();
    
-    /* 初始化状态 */
-    memset(ctx.devices, 0, sizeof(ctx.devices));
-    ctx.device_count = 0;
+    /* 初始化状态（保留已注册的设备） */
+    // 注意：不清除 devices 和 device_count，因为其他任务可能已经注册了设备
    ctx.custom_cmd_count = 0;
    ctx.state = MROBOT_STATE_IDLE;
    strcpy(ctx.current_path, "/");
@ -499,11 +500,11 @@ void MRobot_Init(void) {
    }
    
    /* 注册 UART 回调 */
-    BSP_UART_RegisterCallback(MROBOT_UART_PORT, BSP_UART_RX_CPLT_CB, uart_rx_callback);
-    BSP_UART_RegisterCallback(MROBOT_UART_PORT, BSP_UART_TX_CPLT_CB, uart_tx_callback);
+    BSP_UART_RegisterCallback(BSP_UART_VOFA, BSP_UART_RX_CPLT_CB, uart_rx_callback);
+    BSP_UART_RegisterCallback(BSP_UART_VOFA, BSP_UART_TX_CPLT_CB, uart_tx_callback);
    
    /* 启动 UART 接收 */
-    BSP_UART_Receive(MROBOT_UART_PORT, &ctx.uart_rx_char, 1, false);
+    BSP_UART_Receive(BSP_UART_VOFA, &ctx.uart_rx_char, 1, false);
    
    /* 等待用户按下回车 */
    while (ctx.uart_rx_char != '\r' && ctx.uart_rx_char != '\n') {
@ -523,7 +524,7 @@ void MRobot_DeInit(void) {
    /* 释放自定义命令内存 */
    for (uint8_t i = 0; i < ctx.custom_cmd_count; i++) {
        if (ctx.custom_cmds[i] != NULL) {
-            vPortFree(ctx.custom_cmds[i]);
+            BSP_Free(ctx.custom_cmds[i]);
            ctx.custom_cmds[i] = NULL;
        }
    }
@ -616,7 +617,7 @@ MRobot_Error_t MRobot_RegisterCommand(const char *command, const char *help_text
    }
    
    /* 动态分配命令结构体 */
-    CLI_Command_Definition_t *cmd_def = pvPortMalloc(sizeof(CLI_Command_Definition_t));
+    CLI_Command_Definition_t *cmd_def = BSP_Malloc(sizeof(CLI_Command_Definition_t));
    if (cmd_def == NULL) {
        return MROBOT_ERR_ALLOC;
    }
--- a/User/device/mrobot.h
+++ b/User/device/mrobot.h
@ -25,20 +25,15 @@
 * // 2. 实现打印回调
 * static int print_imu(const void *data, char *buf, size_t size) {
 *     const MyIMU_t *imu = (const MyIMU_t *)data;
- *     char ax[16], ay[16], az[16], r[16], p[16], y[16], t[16];
- *     MRobot_FormatFloat(ax, 16, imu->accl[0], 3);
- *     MRobot_FormatFloat(ay, 16, imu->accl[1], 3);
- *     MRobot_FormatFloat(az, 16, imu->accl[2], 3);
- *     MRobot_FormatFloat(r, 16, imu->euler[0], 2);
- *     MRobot_FormatFloat(p, 16, imu->euler[1], 2);
- *     MRobot_FormatFloat(y, 16, imu->euler[2], 2);
- *     MRobot_FormatFloat(t, 16, imu->temp, 1);
- *     return snprintf(buf, size,
+ *     return MRobot_Snprintf(buf, size,
 *         "  Status: %s\r\n"
- *         "  Accel : X=%s Y=%s Z=%s\r\n"
- *         "  Euler : R=%s P=%s Y=%s\r\n"
- *         "  Temp  : %s C\r\n",
- *         imu->online ? "Online" : "Offline", ax, ay, az, r, p, y, t);
+ *         "  Accel : X=%.3f Y=%.3f Z=%.3f\r\n"
+ *         "  Euler : R=%.2f P=%.2f Y=%.2f\r\n"
+ *         "  Temp  : %.1f C\r\n",
+ *         imu->online ? "Online" : "Offline",
+ *         imu->accl[0], imu->accl[1], imu->accl[2],
+ *         imu->euler[0], imu->euler[1], imu->euler[2],
+ *         imu->temp);
 * }
 * 
 * // 3. 注册设备
@ -58,16 +53,13 @@
 * 
 * static int print_motor(const void *data, char *buf, size_t size) {
 *     const MyMotor_t *m = (const MyMotor_t *)data;
- *     char ang[16], spd[16], cur[16];
- *     MRobot_FormatFloat(ang, 16, m->angle, 2);
- *     MRobot_FormatFloat(spd, 16, m->speed, 1);
- *     MRobot_FormatFloat(cur, 16, m->current, 3);
- *     return snprintf(buf, size,
+ *     return MRobot_Snprintf(buf, size,
 *         "  Status : %s\r\n"
- *         "  Angle  : %s deg\r\n"
- *         "  Speed  : %s RPM\r\n"
- *         "  Current: %s A\r\n",
- *         m->online ? "Online" : "Offline", ang, spd, cur);
+ *         "  Angle  : %.2f deg\r\n"
+ *         "  Speed  : %.1f RPM\r\n"
+ *         "  Current: %.3f A\r\n",
+ *         m->online ? "Online" : "Offline",
+ *         m->angle, m->speed, m->current);
 * }
 * 
 * // 注册 4 个电机
@ -87,20 +79,20 @@
 *     const char *param = FreeRTOS_CLIGetParameter(cmd, 1, NULL);
 *     
 *     if (param == NULL) {
- *         return snprintf(buf, size, "Usage: cali <gyro|accel|save>\r\n");
+ *         return MRobot_Snprintf(buf, size, "Usage: cali <gyro|accel|save>\r\n");
 *     }
 *     if (strncmp(param, "gyro", 4) == 0) {
 *         // 采集 1000 次陀螺仪数据求平均
- *         snprintf(buf, size, "Calibrating gyro... keep IMU still!\r\n");
+ *         MRobot_Snprintf(buf, size, "Calibrating gyro... keep IMU still!\r\n");
 *         // ... 校准逻辑 ...
 *         return 0;
 *     }
 *     if (strncmp(param, "save", 4) == 0) {
 *         // 保存到 Flash
- *         snprintf(buf, size, "Calibration saved to flash.\r\n");
+ *         MRobot_Snprintf(buf, size, "Calibration saved to flash.\r\n");
 *         return 0;
 *     }
- *     return snprintf(buf, size, "Unknown: %s\r\n", param);
+ *     return MRobot_Snprintf(buf, size, "Unknown: %s\r\n", param);
 * }
 * 
 * // 注册命令
@ -151,10 +143,6 @@ extern "C" {
 #define MROBOT_HTOP_REFRESH_MS      200     /* htop 刷新间隔 (ms) */
 #endif

-#ifndef MROBOT_UART_PORT
-#define MROBOT_UART_PORT            BSP_UART_VOFA  /* 默认 UART 端口 */
-#endif
-
 #ifndef MROBOT_USER_NAME
 #define MROBOT_USER_NAME            "root"         /* CLI 用户名 */
 #endif
--- a/User/module/balance_chassis.c
+++ b/User/module/balance_chassis.c
@ -7,6 +7,7 @@
 #include "bsp/can.h"
 #include "bsp/time.h"
 #include "component/filter.h"
+#include "component/kalman_filter.h"
 #include "component/user_math.h"
 #include "device/motor_lk.h"
 #include "device/motor_lz.h"
@ -19,12 +20,12 @@

 #define WHEEL_RADIUS        0.068f   /* 轮子半径 (m) */
 #define MAX_ACCELERATION    2.0f     /* 最大加速度 (m/s²) */
-#define WHEEL_GEAR_RATIO    8.0f     /* 轮毂电机扭矩 */
+#define WHEEL_GEAR_RATIO    4.5f     /* 轮毂电机扭矩 */
 #define BASE_LEG_LENGTH     0.16f    /* 基础腿长 (m) */
 #define LEG_LENGTH_RANGE    0.16f    /* 腿长调节范围 (m) */
 #define MIN_LEG_LENGTH      0.10f    /* 最小腿长 (m) */
-#define MAX_LEG_LENGTH      0.34f    /* 最大腿长 (m) */
-#define NON_CONTACT_THETA   0.16f    /* 离地时的摆角目标 (rad) */
+#define MAX_LEG_LENGTH      0.33f    /* 最大腿长 (m) */
+#define NON_CONTACT_THETA   0.0f    /* 离地时的摆角目标 (rad) */
 #define LEFT_BASE_FORCE     60.0f    /* 左腿基础支撑力 (N) */
 #define RIGHT_BASE_FORCE    60.0f    /* 右腿基础支撑力 (N) */

@ -100,6 +101,9 @@ static void Chassis_ResetControllers(Chassis_t *c) {
    LowPassFilter2p_Reset(&c->filter.wheel_out[i], 0.0f);
  }

+  /* 重置卡尔曼滤波器 */
+  KF_Reset(&c->kf_state_est);
+
  /* 清空机体状态 */
  c->chassis_state.position_x = 0.0f;
  c->chassis_state.velocity_x = 0.0f;
@ -168,46 +172,87 @@ static void Chassis_UpdateChassisState(Chassis_t *c) {

  /*
   * 轮子角速度补偿算法
-   * 补偿关节运动对轮子速度的影响，并消除机体俯仰运动的干扰
-   * 公式来源：五连杆机构运动学分析
+   * w = w_wheel - pitch_gyro + joint_speed
   */
-  float left_w_wheel_compensated = left_wheel_speed + left_joint_speed - pitch_gyro;
-  float right_w_wheel_compensated = right_wheel_speed + right_joint_speed - pitch_gyro;
+  float left_w_compensated = left_wheel_speed + left_joint_speed - pitch_gyro;
+  float right_w_compensated = right_wheel_speed + right_joint_speed - pitch_gyro;

  /*
-   * 机体速度计算算法
-   * 综合考虑：
-   * 1. 轮子接触点线速度：omega_wheel * r_wheel
-   * 2. 摆杆运动贡献：L0 * d_theta (摆杆角速度在x方向的分量)
-   * 3. 腿长变化贡献：d_L0 * sin(theta) (腿长变化在x方向的分量)
+   * 机体速度计算 (参考港大开源)
+   * v = w*R + L0*dtheta*cos(theta) + dL0*sin(theta)
   */
-  float left_v_body = left_w_wheel_compensated * WHEEL_RADIUS +
-                      c->vmc_[0].leg.L0 * c->vmc_[0].leg.d_theta +
+  float left_v_body = left_w_compensated * WHEEL_RADIUS +
+                      c->vmc_[0].leg.L0 * c->vmc_[0].leg.d_theta * cosf(c->vmc_[0].leg.theta) +
                      c->vmc_[0].leg.d_L0 * sinf(c->vmc_[0].leg.theta);

-  float right_v_body = right_w_wheel_compensated * WHEEL_RADIUS +
-                       c->vmc_[1].leg.L0 * c->vmc_[1].leg.d_theta +
+  float right_v_body = right_w_compensated * WHEEL_RADIUS +
+                       c->vmc_[1].leg.L0 * c->vmc_[1].leg.d_theta * cosf(c->vmc_[1].leg.theta) +
                       c->vmc_[1].leg.d_L0 * sinf(c->vmc_[1].leg.theta);

  /* 保存历史速度 */
  c->chassis_state.last_velocity_x = c->chassis_state.velocity_x;

-  /* 计算平均速度 */
+  /* 计算平均轮子速度 */
  float vel_measured = (left_v_body + right_v_body) / 2.0f;

  /*
-   * 接地状态检测与速度修正
-   * 当双腿都离地时，基于轮子的速度估计不可靠，将速度设为0
+   * 获取IMU前向加速度（世界坐标系x方向）
+   * 使用旋转矩阵将机体加速度(已去重力)转到世界系
+   * axE = cos(pitch)*ax_body + sin(roll)*sin(pitch)*ay_body + cos(roll)*sin(pitch)*az_body
   */
+  float pitch = c->feedback.imu.euler.pit;
+  float roll  = c->feedback.imu.euler.rol;
+  float ax_body = c->feedback.imu.accl.x;
+  float ay_body = c->feedback.imu.accl.y;
+  float az_body = c->feedback.imu.accl.z;
+
+  /* 去除重力分量（机体系下） */
+  float gravity_bx = -9.81f * sinf(pitch);
+  float gravity_by =  9.81f * sinf(roll) * cosf(pitch);
+  float gravity_bz =  9.81f * cosf(roll) * cosf(pitch);
+  float motion_ax = ax_body - gravity_bx;
+  float motion_ay = ay_body - gravity_by;
+  float motion_az = az_body - gravity_bz;
+
+  /* 旋转到世界坐标系x方向 */
+  float accel_world_x = cosf(pitch) * motion_ax +
+                        sinf(roll) * sinf(pitch) * motion_ay +
+                        cosf(roll) * sinf(pitch) * motion_az;
+
+  /* ==================== 卡尔曼滤波器融合 ==================== */
+  /* 动态更新状态转移矩阵 F（根据实际dt）*/
+  float dt = c->dt;
+  if (dt <= 0.0f || dt > 0.1f) dt = 0.001f;
+
+  /* F = [1, dt; 0, 1] */
+  c->kf_state_est.F_data[0] = 1.0f;
+  c->kf_state_est.F_data[1] = dt;
+  c->kf_state_est.F_data[2] = 0.0f;
+  c->kf_state_est.F_data[3] = 1.0f;
+
+  /* 量测向量: z = [v_wheel, a_imu]
+   * 注意：必须写入 measured_vector，KF_Measure 会将其拷贝到 z_data
+   * 直接写 z_data 会被 KF_Measure 用 measured_vector(全零) 覆盖！ */
+  c->kf_state_est.measured_vector[0] = vel_measured;
+  c->kf_state_est.measured_vector[1] = accel_world_x;
+
+  /* 动态调整R矩阵：离地时降低轮速信任度 */
  if (c->vmc_[0].leg.Fn < 20.0f && c->vmc_[1].leg.Fn < 20.0f) {
-    vel_measured = 0.0f;
+    c->kf_state_est.R_data[0] = 100000.0f;  /* 离地：不信任轮速 */
+  } else {
+    c->kf_state_est.R_data[0] = 100.0f;     /* 接地：信任轮速 */
  }
+  /* R[1][1] = 1000000.0f 保持不变，始终不信任加速度量测 */

-  /* 使用二阶低通滤波器平滑速度估计 */
-  c->chassis_state.velocity_x = LowPassFilter2p_Apply(&c->filter.velocity_est, vel_measured);
+  /* 执行卡尔曼滤波 */
+  float *kf_result = KF_Update(&c->kf_state_est);

-  /* 位置积分更新 */
-  c->chassis_state.position_x += c->chassis_state.velocity_x * c->dt;
+  /* KF输出: [velocity, acceleration] */
+  if (kf_result != NULL) {
+    c->chassis_state.velocity_x = kf_result[0];
+    /* 位移 = 滤波后的速度积分（不让加速度误差被二次积分放大） */
+    c->chassis_state.position_x += c->chassis_state.velocity_x * dt;
+  }
 }

 /**
@ -386,6 +431,62 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) {
  }
  LowPassFilter2p_Init(&c->filter.velocity_est, target_freq, 30.0f);  /* 速度估计滤波截止频率30Hz */

+  /*
+   * 初始化卡尔曼滤波器（参考港大开源方案）
+   * 状态向量 x = [v, a]  (2维：速度和加速度)
+   * 量测向量 z = [v, a]  (2维：轮速和IMU加速度)
+   * 位移在KF外部用滤波后的速度积分，避免加速度误差被二次积分放大
+   */
+  KF_Init(&c->kf_state_est, 2, 0, 2);
+
+  float dt_init = 1.0f / target_freq;
+
+  /* 初始协方差矩阵 P (2x2) */
+  static float P_Init[4] = {
+      1.0f, 0.0f,
+      0.0f, 1.0f,
+  };
+  memcpy(c->kf_state_est.P_data, P_Init, sizeof(P_Init));
+
+  /* 状态转移矩阵 F (2x2): v(k) = v(k-1) + a*dt, a(k) = a(k-1) */
+  float F_Init[4] = {
+      1.0f, dt_init,
+      0.0f, 1.0f,
+  };
+  memcpy(c->kf_state_est.F_data, F_Init, sizeof(F_Init));
+
+  /* 过程噪声协方差矩阵 Q (2x2) */
+  static float Q_Init[4] = {
+      0.1f, 0.0f,
+      0.0f, 0.1f,
+  };
+  memcpy(c->kf_state_est.Q_data, Q_Init, sizeof(Q_Init));
+
+  /* 状态最小方差（防止过度收敛） */
+  static float state_min_var[2] = {0.005f, 0.1f};
+  memcpy(c->kf_state_est.state_min_variance, state_min_var, sizeof(state_min_var));
+
+  /* 关闭自动量测调整（手动管理H/R） */
+  c->kf_state_est.use_auto_adjustment = 0;
+
+  /* 量测矩阵 H (2x2): z = H*x, 直接观测速度和加速度 */
+  static float H_Init[4] = {
+      1.0f, 0.0f,
+      0.0f, 1.0f,
+  };
+  memcpy(c->kf_state_est.H_data, H_Init, sizeof(H_Init));
+
+  /*
+   * 量测噪声协方差矩阵 R (2x2)
+   * 关键：加速度噪声设极大(1000000)，几乎不信任加速度量测
+   * KF主要靠轮速量测修正，加速度仅在预测环节(F矩阵)起辅助作用
+   */
+  static float R_Init[4] = {
+      100.0f,    0.0f,
+      0.0f,      1000000.0f,
+  };
+  memcpy(c->kf_state_est.R_data, R_Init, sizeof(R_Init));
+
  Chassis_MotorEnable(c);

  /* 初始化状态变量 */
@ -427,6 +528,9 @@ int8_t Chassis_UpdateFeedback(Chassis_t *c) {
      c->feedback.joint[i].rotor_abs_angle -= M_2PI;
    }
    c->feedback.joint[i].rotor_abs_angle = -c->feedback.joint[i].rotor_abs_angle;
+    
+    /* 应用零点偏移 */
+    c->feedback.joint[i].rotor_abs_angle -= c->param->joint_zero[i];
  }

  /* 更新轮子电机反馈 */
@ -598,18 +702,22 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
  if (c == NULL || c_cmd == NULL) return CHASSIS_ERR_NULL;

  /* ==================== 速度控制 ==================== */
-  float raw_vx = c_cmd->move_vec.vx * 2.0f;
-  float desired_velocity = c->yaw_control.is_reversed ? -raw_vx : raw_vx;
+  // float raw_vx = c_cmd->move_vec.vx * 2.0f;
+  // float desired_velocity = c->yaw_control.is_reversed ? -raw_vx : raw_vx;

-  /* 加速度限制 */
-  float velocity_change = desired_velocity - c->chassis_state.last_target_velocity_x;
-  float max_velocity_change = MAX_ACCELERATION * c->dt;
-  velocity_change = LIMIT(velocity_change, -max_velocity_change, max_velocity_change);
+  // /* 加速度限制 */
+  // float velocity_change = desired_velocity - c->chassis_state.last_target_velocity_x;
+  // float max_velocity_change = MAX_ACCELERATION * c->dt;
+  // velocity_change = LIMIT(velocity_change, -max_velocity_change, max_velocity_change);
+
+  // float target_dot_x = c->chassis_state.last_target_velocity_x + velocity_change;
+  // c->chassis_state.target_velocity_x = target_dot_x;
+  // c->chassis_state.last_target_velocity_x = target_dot_x;
+  // c->chassis_state.target_x += target_dot_x * c->dt;
+  c->chassis_state.target_velocity_x = c_cmd->move_vec.vx * 2.0f;
+  c->chassis_state.last_target_velocity_x = c->chassis_state.target_velocity_x;
+  c->chassis_state.target_x += c->chassis_state.target_velocity_x * c->dt;

-  float target_dot_x = c->chassis_state.last_target_velocity_x + velocity_change;
-  c->chassis_state.target_velocity_x = target_dot_x;
-  c->chassis_state.last_target_velocity_x = target_dot_x;
-  c->chassis_state.target_x += target_dot_x * c->dt;

  /* ==================== 状态更新 ==================== */
  LQR_State_t current_state = {
@ -635,7 +743,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
  LQR_State_t target_state = {
      .theta = 0.0f,
      .d_theta = 0.0f,
-      .phi = -0.1f,
+      .phi = 0.0f,
      .d_phi = 0.0f,
      .x = c->chassis_state.target_x,
      .d_x = c->chassis_state.target_velocity_x,
--- a/User/module/balance_chassis.h
+++ b/User/module/balance_chassis.h
@ -20,6 +20,7 @@ extern "C" {
 #include "component/lqr.h"
 #include "component/ahrs.h"
 #include "component/filter.h"
+#include "component/kalman_filter.h"
 #include "component/pid.h"
 #include "device/motor.h"
 #include "device/motor_lk.h"
@ -105,6 +106,8 @@ typedef struct {
    float retract_force;          /* 收腿前馈力 (N)，负值表示向上收 */
  } jump_params;

+  float joint_zero[4]; /* 关节电机零点偏移位置 */
+
  float mech_zero_yaw; /* 机械零点 */

  float theta;
@ -189,6 +192,9 @@ typedef struct {
    LowPassFilter2p_t velocity_est;  /* 速度估计滤波器 */
  } filter;

+  /* 卡尔曼滤波器：位移速度融合IMU */
+  KF_t kf_state_est;
+
 } Chassis_t;

 /* Exported functions prototypes -------------------------------------------- */
--- a/User/module/config.c
+++ b/User/module/config.c
@ -203,7 +203,7 @@ Config_RobotParam_t robot_config = {

        .leg_length={
            .k = 40.0f,
-            .p = 20.0f,
+            .p = 30.0f,
            .i = 0.01f,
            .d = 2.0f,
            .i_limit = 0.0f,
@ -298,7 +298,7 @@ Config_RobotParam_t robot_config = {
        },

        .mech_zero_yaw = 1.78040409f, /* 机械零点 */
-
+        .joint_zero = {0.0f, 0.0f, 0.0f, 0.0f}, /* 关节电机零点偏移位置 */
                .vmc_param = {
            { // 左腿
                .leg_1 = 0.215f,      // 后髋连杆长度 (L1) (m)
@ -316,33 +316,34 @@ Config_RobotParam_t robot_config = {
            }
        },

-.lqr_gains = {
-    .k11_coeff = { -2.210764178888050e+02f, 2.426363711747753e+02f, -1.195592871161101e+02f, -3.943946668753056e+00f }, // theta
-    .k12_coeff = { -2.137164311515632e+00f, 1.217737477437830e+00f, -9.525046061205586e+00f, -3.204960462460905e-01f },  // d_theta
-    .k13_coeff = { -5.403421185903361e+01f, 5.400834583224349e+01f, -1.871421701200579e+01f, -2.187399624044644e+00f },  // x
-    .k14_coeff = { -4.592744044784984e+01f, 4.660766512779469e+01f, -1.805265440365850e+01f, -2.699994690713614e+00f },  // d_x
-    .k15_coeff = { -9.685784551581214e+01f, 1.159388980795003e+02f, -5.296171221139453e+01f, 1.123906253873898e+01f }, // phi
-    .k16_coeff = { -2.294801139621741e+01f, 2.749641396600526e+01f, -1.259823164155369e+01f, 2.761284676172917e+00f },  // d_phi
-    .k21_coeff = { 7.727438740554770e+01f, -3.291782819657940e+01f, -1.757884718209812e+01f, 1.497962178015115e+01f }, // theta
-    .k22_coeff = { -4.313876591098404e-01f, 3.014310593976116e+00f, -2.775571423570345e+00f, 1.949396772465259e+00f },  // d_theta
-    .k23_coeff = { -8.163301739920055e+01f, 9.889264843312036e+01f, -4.547892032101780e+01f, 9.369462147227104e+00f },  // x
-    .k24_coeff = { -1.025410522837800e+02f, 1.187387974704523e+02f, -5.202150278239223e+01f, 1.038835798060997e+01f },  // d_x
-    .k25_coeff = { 2.742584002836992e+02f, -2.735816033702905e+02f, 9.479793291043821e+01f, 7.314831370012082e+00f }, // phi
-    .k26_coeff = { 6.734357804702088e+01f, -6.735254573603545e+01f, 2.346143287673895e+01f, 1.501777400084277e+00f },  // d_phi
-},
+    .lqr_gains = {
+        .k11_coeff = { -1.997928012118380e+02f, 2.358970856133577e+02f, -1.221432656066952e+02f, -3.660884265594530e+00f }, // theta
+        .k12_coeff = { 1.144642140244768e+00f, -6.781143989723293e-01f, -8.286867905694857e+00f, -3.233743818654922e-01f },  // d_theta
+        .k13_coeff = { -2.920908857554099e+01f, 3.294053235814564e+01f, -1.331127106026891e+01f, -2.036633181450379e+00f },  // x
+        .k14_coeff = { -2.202395151966686e+01f, 2.624258394298977e+01f, -1.238392622003398e+01f, -2.516594294742349e+00f },  // d_x
+        .k15_coeff = { -5.531234648285231e+01f, 7.666196018790744e+01f, -4.254301644877548e+01f, 1.191958597928930e+01f }, // phi
+        .k16_coeff = { -8.603393557876432e+00f, 1.181868681749069e+01f, -6.519358011847959e+00f, 2.002836931783026e+00f },  // d_phi
+        .k21_coeff = { 3.121841964126664e+02f, -2.644789946321499e+02f, 5.653973783920610e+01f, 1.306067415926613e+01f }, // theta
+        .k22_coeff = { 9.751578045310433e+00f, -8.174866581419979e+00f, 1.060040492386880e+00f, 1.945460420856203e+00f },  // d_theta
+        .k23_coeff = { -1.589159892148102e+01f, 3.771040781828523e+01f, -2.789168930865428e+01f, 8.354369470743295e+00f },  // x
+        .k24_coeff = { -3.304075197263637e+01f, 5.411818719230526e+01f, -3.342861600971858e+01f, 9.256855471266778e+00f },  // d_x
+        .k25_coeff = { 2.574491871362636e+02f, -2.771545105998671e+02f, 1.074894289176479e+02f, 3.130550754680395e+00f }, // phi
+        .k26_coeff = { 4.162978395880715e+01f, -4.533986766238992e+01f, 1.794589550749570e+01f, 2.032213740678163e-01f },  // d_phi
+    },
+
    .jump_params = {
-        .crouch_time_ms = 300,
+        .crouch_time_ms = 100,
        .launch_time_ms = 120,
        .retract_time_ms = 80,
        .land_time_ms = 300,
-        .crouch_leg_length = 0.14f,
+        .crouch_leg_length = 0.1f,
        .launch_force = 200.0f,
-        .retract_leg_length = 0.16f,   /* 收腿目标更短 */
+        .retract_leg_length = 0.1f,   /* 收腿目标更短 */
        .retract_force = -120.0f,      /* 收腿前馈力加大 */
    },
        .theta = 0.0f,
        .x = 0.0f,
-        .phi = -0.1f,
+        .phi = 0.0f,
    
    },

--- a/User/task/ai.c
+++ b/User/task/ai.c
@ -23,6 +23,9 @@ Gimbal_AI_t ai_for_gimbal;
 /* USER STRUCT END */

 /* Private function --------------------------------------------------------- */
+/* USER PRIVATE CODE BEGIN */
+
+/* USER PRIVATE CODE END */
 /* Exported functions ------------------------------------------------------- */
 void Task_ai(void *argument) {
  (void)argument; /* 未使用argument，消除警告 */
--- a/User/task/atti_esit.c
+++ b/User/task/atti_esit.c
@ -13,8 +13,8 @@
 #include "component/QuaternionEKF.h"
 #include "component/pid.h"
 #include "device/bmi088.h"
-#include "device/mrobot.h"
 #include "device/device.h"
+#include "device/mrobot.h"
 #include "module/balance_chassis.h"
 #include "task/user_task.h"
 #include <stdio.h>
@ -51,27 +51,23 @@ static const KPID_Params_t imu_temp_ctrl_pid_param = {
 /* USER STRUCT END */

 /* Private function --------------------------------------------------------- */
+/* USER PRIVATE CODE BEGIN */

 /**
- * @brief IMU 设备打印回调函数
+ * @brief 底盘IMU数据打印回调
 */
-static int print_imu_callback(const void *data, char *buffer, size_t size) {
-    const BMI088_t *imu = (const BMI088_t *)data;
-    #define RAD2DEG 57.2957795131f
-    
-    return MRobot_Snprintf(buffer, size,
-        "  Status : %s\r\n"
-        "  Accel  : X=%.3f  Y=%.3f  Z=%.3f (m/s^2)\r\n"
+static int print_chassis_imu(const void *data, char *buf, size_t size) {
+  const Chassis_IMU_t *imu = (const Chassis_IMU_t *)data;
+  return MRobot_Snprintf(buf, size,
+      "  Accel  : X=%.3f Y=%.3f Z=%.3f (m/s²)\r\n"
      "  Gyro   : X=%.3f Y=%.3f Z=%.3f (rad/s)\r\n"
-        "  Euler  : R=%.2f  P=%.2f  Y=%.2f (deg)\r\n"
-        "  Temp   : %.1f C\r\n",
-        imu->header.online ? "Online" : "Offline",
+      "  Euler  : R=%.2f P=%.2f Y=%.2f (deg)\r\n",
      imu->accl.x, imu->accl.y, imu->accl.z,
      imu->gyro.x, imu->gyro.y, imu->gyro.z,
-        eulr_to_send.rol * RAD2DEG, eulr_to_send.pit * RAD2DEG, eulr_to_send.yaw * RAD2DEG,
-        imu->temp);
+      imu->euler.rol, imu->euler.pit, imu->euler.yaw);
 }

+/* USER PRIVATE CODE END */
 /* Exported functions ------------------------------------------------------- */
 void Task_atti_esit(void *argument) {
  (void)argument; /* 未使用argument，消除警告 */
@ -89,9 +85,7 @@ void Task_atti_esit(void *argument) {
  PID_Init(&imu_temp_ctrl_pid, KPID_MODE_NO_D,
           1.0f / BMI088_GetUpdateFreq(&bmi088), &imu_temp_ctrl_pid_param);
  BSP_PWM_Start(BSP_PWM_IMU_HEAT);
-  
-  /* 注册 IMU 设备到 MRobot */
-  MRobot_RegisterDevice("bmi088", &bmi088, print_imu_callback);
+  MRobot_RegisterDevice("chassis_imu", &imu_for_chassis, print_chassis_imu);
  /* USER CODE INIT END */

  while (1) {
--- a/User/task/blink.c
+++ b/User/task/blink.c
@ -20,6 +20,9 @@ static uint16_t count;
 /* USER STRUCT END */

 /* Private function --------------------------------------------------------- */
+/* USER PRIVATE CODE BEGIN */
+
+/* USER PRIVATE CODE END */
 /* Exported functions ------------------------------------------------------- */
 void Task_blink(void *argument) {
  (void)argument; /* 未使用argument，消除警告 */
--- a/User/task/cli.c
+++ b/User/task/cli.c
@ -18,6 +18,17 @@
 /* USER STRUCT END */

 /* Private function --------------------------------------------------------- */
+/* USER PRIVATE CODE BEGIN */
+/**
+ * @brief hello 命令回调
+ * @note 命令回调必须返回 0 (pdFALSE) 表示完成，返回非0会继续调用
+ */
+static long Cli_Hello(char *buffer, size_t size, const char *cmd) {
+  (void)cmd; /* 未使用cmd，消除警告 */
+  MRobot_Snprintf(buffer, size, "Ciallo～(∠・ω< )⌒★\r\n");
+  return 0;  /* 返回0表示命令执行完成 */
+}
+/* USER PRIVATE CODE END */
 /* Exported functions ------------------------------------------------------- */
 void Task_cli(void *argument) {
  (void)argument; /* 未使用argument，消除警告 */
@ -28,6 +39,7 @@ void Task_cli(void *argument) {
  /* USER CODE INIT BEGIN */
  /* 初始化 MRobot CLI 系统 */
  MRobot_Init();
+  MRobot_RegisterCommand("hello", " --hello: 打印问候语\r\n", Cli_Hello, -1);
  /* USER CODE INIT END */

  while (1) {
--- a/User/task/ctrl_chassis.c
+++ b/User/task/ctrl_chassis.c
@ -9,6 +9,7 @@
 #include "bsp/can.h"
 #include "device/motor_lk.h"
 #include "device/motor_lz.h"
+#include "device/mrobot.h"
 #include "module/config.h"
 #include "module/balance_chassis.h"
 /* USER INCLUDE END */
@ -32,6 +33,32 @@ Chassis_IMU_t chassis_imu;
 /* USER STRUCT END */

 /* Private function --------------------------------------------------------- */
+/* USER PRIVATE CODE BEGIN */
+
+/**
+ * @brief 底盘数据打印回调
+ */
+static int print_chassis(const void *data, char *buf, size_t size) {
+  const Chassis_t *chassis = (const Chassis_t *)data;
+  return MRobot_Snprintf(buf, size,
+      "  Mode    : %d\r\n"
+      "  IMU     : Roll=%.2f Pitch=%.2f Yaw=%.2f (deg)\r\n"
+      "  Motors  : J0=%.1f J1=%.1f J2=%.1f J3=%.1f (rpm)\r\n"
+      "  Wheels  : W0=%.1f W1=%.1f (rpm)\r\n"
+      "  Output  : J0=%.1f J1=%.1f J2=%.1f J3=%.1f\r\n"
+      "  VelX    : %.3f m/s\r\n",
+      chassis->mode,
+      chassis->feedback.imu.euler.rol, chassis->feedback.imu.euler.pit, chassis->feedback.imu.euler.yaw,
+      chassis->feedback.joint[0].rotor_speed, chassis->feedback.joint[1].rotor_speed,
+      chassis->feedback.joint[2].rotor_speed, chassis->feedback.joint[3].rotor_speed,
+      chassis->feedback.wheel[0].rotor_speed, chassis->feedback.wheel[1].rotor_speed,
+      chassis->output.joint[0], chassis->output.joint[1],
+      chassis->output.joint[2], chassis->output.joint[3],
+      chassis->chassis_state.velocity_x);
+  return 0;
+}
+
+/* USER PRIVATE CODE END */
 /* Exported functions ------------------------------------------------------- */
 void Task_ctrl_chassis(void *argument) {
  (void)argument; /* 未使用argument，消除警告 */
@ -46,6 +73,7 @@ void Task_ctrl_chassis(void *argument) {
  /* USER CODE INIT BEGIN */

  Chassis_Init(&chassis, &Config_GetRobotParam()->chassis_param, CTRL_CHASSIS_FREQ);
+  MRobot_RegisterDevice("chassis", &chassis, print_chassis);

  /* USER CODE INIT END */
  
@ -72,6 +100,7 @@ void Task_ctrl_chassis(void *argument) {
    osMessageQueueReset(task_runtime.msgq.chassis.vofa); // 重置消息队列，防止阻塞
    osMessageQueuePut(task_runtime.msgq.chassis.vofa, &chassis, 0, 0);
    /* USER CODE END */
+    
    osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
  }
  
--- a/User/task/ctrl_gimbal.c
+++ b/User/task/ctrl_gimbal.c
@ -7,6 +7,7 @@
 #include "task/user_task.h"
 /* USER INCLUDE BEGIN */
 #include "component/ahrs.h"
+#include "device/mrobot.h"
 #include "module/gimbal.h"
 #include "module/config.h"
 #include "bsp/can.h"
@ -24,6 +25,28 @@ Gimbal_AI_t gimbal_ai;
 /* USER STRUCT END */

 /* Private function --------------------------------------------------------- */
+/* USER PRIVATE CODE BEGIN */
+
+/**
+ * @brief 云台数据打印回调
+ */
+static int print_gimbal(const void *data, char *buf, size_t size) {
+  const Gimbal_t *gimbal = (const Gimbal_t *)data;
+  return MRobot_Snprintf(buf, size,
+      "  Mode   : %d\r\n"
+      "  IMU    : Roll=%.2f Pitch=%.2f Yaw=%.2f (deg)\r\n"
+      "  GyroX  : %.3f  GyroY : %.3f  GyroZ : %.3f (rad/s)\r\n"
+      "  Motor  : Yaw=%.1f(rpm) Pit=%.1f(rpm)\r\n"
+      "  Output : Yaw=%.1f  Pit=%.1f\r\n",
+      gimbal->mode,
+      gimbal->imu.data.eulr.rol, gimbal->imu.data.eulr.pit, gimbal->imu.data.eulr.yaw,
+      gimbal->imu.data.gyro.x, gimbal->imu.data.gyro.y, gimbal->imu.data.gyro.z,
+      gimbal->feedback.motor.yaw.rotor_speed, gimbal->feedback.motor.pit.rotor_speed,
+      gimbal->out.yaw, gimbal->out.pit);
+  return 0;
+}
+
+/* USER PRIVATE CODE END */
 /* Exported functions ------------------------------------------------------- */
 void Task_ctrl_gimbal(void *argument) {
  (void)argument; /* 未使用argument，消除警告 */
@ -36,7 +59,9 @@ void Task_ctrl_gimbal(void *argument) {

  uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
  /* USER CODE INIT BEGIN */
+  
  Gimbal_Init(&gimbal, &Config_GetRobotParam()->gimbal_param, CTRL_GIMBAL_FREQ);
+  MRobot_RegisterDevice("gimbal", &gimbal, print_gimbal);
  /* USER CODE INIT END */
  
  while (1) {
--- a/User/task/ctrl_shoot.c
+++ b/User/task/ctrl_shoot.c
@ -6,6 +6,7 @@
 /* Includes ----------------------------------------------------------------- */
 #include "task/user_task.h"
 /* USER INCLUDE BEGIN */
+#include "device/mrobot.h"
 #include "module/shoot.h"
 #include "module/config.h"
 /* USER INCLUDE END */
@ -20,10 +21,35 @@ Shoot_CMD_t shoot_cmd;
 /* USER STRUCT END */

 /* Private function --------------------------------------------------------- */
+/* USER PRIVATE CODE BEGIN */
+
+/**
+ * @brief 发射模块数据打印回调
+ */
+static int print_shoot(const void *data, char *buf, size_t size) {
+  const Shoot_t *shoot = (const Shoot_t *)data;
+  return MRobot_Snprintf(buf, size,
+      "  State    : %d\r\n"
+      "  Fric0    : %.1f rpm (target: %.1f)\r\n"
+      "  Fric1    : %.1f rpm (target: %.1f)\r\n"
+      "  Fric_Avg : %.1f rpm\r\n"
+      "  Trig     : %.1f rpm (angle: %.2f deg)\r\n"
+      "  Output   : Fric0=%.1f Fric1=%.1f Trig=%.1f\r\n",
+      shoot->running_state,
+      shoot->feedback.fric_rpm[0], shoot->target_variable.target_rpm,
+      shoot->feedback.fric_rpm[1], shoot->target_variable.target_rpm,
+      shoot->feedback.fric_avgrpm,
+      shoot->feedback.trig_rpm, shoot->feedback.trig_angle_cicle,
+      shoot->output.out_fric[0], shoot->output.out_fric[1], shoot->output.outagl_trig);
+  return 0;
+}
+
+/* USER PRIVATE CODE END */
 /* Exported functions ------------------------------------------------------- */
 void Task_ctrl_shoot(void *argument) {
  (void)argument; /* 未使用argument，消除警告 */

+  
  /* 计算任务运行到指定频率需要等待的tick数 */
  const uint32_t delay_tick = osKernelGetTickFreq() / CTRL_SHOOT_FREQ;

@ -31,7 +57,9 @@ void Task_ctrl_shoot(void *argument) {

  uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
  /* USER CODE INIT BEGIN */
+  
  Shoot_Init(&shoot,&Config_GetRobotParam()->shoot_param,CTRL_SHOOT_FREQ);
+  MRobot_RegisterDevice("shoot", &shoot, print_shoot);

  /* USER CODE INIT END */
  
--- a/User/task/monitor.c
+++ b/User/task/monitor.c
@ -18,6 +18,9 @@
 /* USER STRUCT END */

 /* Private function --------------------------------------------------------- */
+/* USER PRIVATE CODE BEGIN */
+
+/* USER PRIVATE CODE END */
 /* Exported functions ------------------------------------------------------- */
 void Task_monitor(void *argument) {
  (void)argument; /* 未使用argument，消除警告 */
--- a/User/task/rc.c
+++ b/User/task/rc.c
@ -7,6 +7,7 @@
 #include "task/user_task.h"
 /* USER INCLUDE BEGIN */
 #include "device/dr16.h"
+#include "device/mrobot.h"
 #include "module/shoot.h"
 #include "module/balance_chassis.h"
 #include "module/gimbal.h"
@ -25,6 +26,32 @@ Gimbal_CMD_t cmd_for_gimbal;
 /* USER STRUCT END */

 /* Private function --------------------------------------------------------- */
+/* USER PRIVATE CODE BEGIN */
+
+/**
+ * @brief DR16数据打印回调
+ */
+static int print_dr16(const void *data, char *buf, size_t size) {
+  const DR16_t *dr16 = (const DR16_t *)data;
+  return MRobot_Snprintf(buf, size,
+      "  Status : %s\r\n"
+      "  ChLX   : %.3f  ChLY : %.3f\r\n"
+      "  ChRX   : %.3f  ChRY : %.3f\r\n"
+      "  ChRes  : %.3f\r\n"
+      "  SwL    : %d    SwR  : %d\r\n"
+      "  Mouse  : X=%d Y=%d Z=%d (L:%d R:%d)\r\n"
+      "  Keys   : 0x%04X\r\n",
+      dr16->header.online ? "Online" : "Offline",
+      dr16->data.ch_l_x, dr16->data.ch_l_y,
+      dr16->data.ch_r_x, dr16->data.ch_r_y,
+      dr16->data.ch_res,
+      dr16->data.sw_l, dr16->data.sw_r,
+      dr16->data.mouse.x, dr16->data.mouse.y, dr16->data.mouse.z,
+      dr16->data.mouse.l_click, dr16->data.mouse.r_click,
+      dr16->data.keyboard.value);
+}
+
+/* USER PRIVATE CODE END */
 /* Exported functions ------------------------------------------------------- */
 void Task_rc(void *argument) {
  (void)argument; /* 未使用argument，消除警告 */
@ -38,6 +65,7 @@ void Task_rc(void *argument) {
  uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
  /* USER CODE INIT BEGIN */
  DR16_Init(&dr16);
+  MRobot_RegisterDevice("dr16", &dr16, print_dr16);
  /* USER CODE INIT END */
  
  while (1) {
@ -132,13 +160,13 @@ switch (dr16.data.sw_l) {
      cmd_for_chassis.mode = CHASSIS_MODE_RELAX;
      break;
    case DR16_SW_MID:
-      cmd_for_chassis.mode = CHASSIS_MODE_RECOVER;
-      // cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
+      // cmd_for_chassis.mode = CHASSIS_MODE_RECOVER;
+      cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
      break;
    case DR16_SW_DOWN:
    // cmd_for_chassis.mode = CHASSIS_MODE_RECOVER;
-      // cmd_for_chassis.mode = CHASSIS_MODE_BALANCE_ROTOR;
-      cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
+      cmd_for_chassis.mode = CHASSIS_MODE_BALANCE_ROTOR;
+      // cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
      // cmd_for_chassis.mode = CHASSIS_MODE_CALIBRATE;
      break;
    default:
--- a/User/task/user_task.c
+++ b/User/task/user_task.c
@ -5,7 +5,7 @@ Task_Runtime_t task_runtime;
 const osThreadAttr_t attr_init = {
    .name = "Task_Init",
    .priority = osPriorityRealtime,
-    .stack_size = 256 * 4,
+    .stack_size = 256 * 16,
 };

 /* User_task */
@ -57,5 +57,5 @@ const osThreadAttr_t attr_vofa = {
 const osThreadAttr_t attr_cli = {
    .name = "cli",
    .priority = osPriorityNormal,
-    .stack_size = 256 * 16,
+    .stack_size = 1024 * 4,
 };
--- a/User/task/vofa.c
+++ b/User/task/vofa.c
@ -19,6 +19,9 @@ Chassis_t chassis_vofa;
 /* USER STRUCT END */

 /* Private function --------------------------------------------------------- */
+/* USER PRIVATE CODE BEGIN */
+
+/* USER PRIVATE CODE END */
 /* Exported functions ------------------------------------------------------- */
 void Task_vofa(void *argument) {
  (void)argument; /* 未使用argument，消除警告 */
--- a/综合运动控制验证/K-L0/K.m
+++ b/综合运动控制验证/K-L0/K.m
@ -0,0 +1,28 @@
+close all;
+clear;clc;
+L0=[0.12 0.13 0.14 0.15 0.16 0.17 0.18 0.19 0.20 0.21 0.22 0.23 0.24 0.25 0.26 0.27 0.28 0.29 0.30 0.31 0.32 0.33 0.34 0.35 0.36];
+K11=[-13.0207 -13.6522 -14.2545 -14.8292 -15.3780 -15.9028 -16.4052 -16.8868 -17.3490 -17.7934 -18.2212 -18.6337 -19.0320 -19.4172 -19.7902 -20.1519 -20.5031 -20.8445 -21.1769 -21.5008 -21.8169 -22.1256 -22.4274 -22.7227 -23.0120];
+K12=[-1.3999 -1.4844 -1.5691 -1.6540 -1.7390 -1.8240 -1.9090 -1.9940 -2.0790 -2.1640 -2.2490 -2.3340 -2.4191 -2.5042 -2.5893 -2.6745 -2.7597 -2.8451 -2.9305 -3.0160 -3.1016 -3.1873 -3.2732 -3.3592 -3.4453];
+K13=[-3.6148 -3.6840 -3.7481 -3.8071 -3.8611 -3.9104 -3.9552 -3.9960 -4.0330 -4.0666 -4.0971 -4.1249 -4.1501 -4.1730 -4.1940 -4.2130 -4.2304 -4.2464 -4.2609 -4.2743 -4.2866 -4.2979 -4.3083 -4.3179 -4.3267];
+K14=[-3.8656 -3.9320 -3.9955 -4.0559 -4.1129 -4.1667 -4.2173 -4.2648 -4.3096 -4.3518 -4.3915 -4.4291 -4.4647 -4.4985 -4.5307 -4.5613 -4.5906 -4.6187 -4.6457 -4.6717 -4.6967 -4.7209 -4.7444 -4.7671 -4.7892];
+K15=[6.2710 6.0483 5.8343 5.6293 5.4333 5.2462 5.0681 4.8986 4.7375 4.5845 4.4393 4.3014 4.1706 4.0464 3.9284 3.8164 3.7100 3.6088 3.5125 3.4209 3.3336 3.2505 3.1712 3.0955 3.0232];
+K16=[1.1563 1.1201 1.0854 1.0522 1.0206 0.9904 0.9617 0.9343 0.9083 0.8836 0.8602 0.8379 0.8167 0.7966 0.7775 0.7593 0.7421 0.7256 0.7100 0.6951 0.6809 0.6674 0.6545 0.6422 0.6304];
+K21=[11.9463 11.9148 11.8490 11.7565 11.6436 11.5153 11.3760 11.2290 11.0769 10.9220 10.7660 10.6102 10.4556 10.3030 10.1531 10.0063 9.8629 9.7231 9.5871 9.4549 9.3266 9.2021 9.0814 8.9645 8.8513];
+K22=[1.8154 1.7992 1.7822 1.7645 1.7465 1.7284 1.7103 1.6924 1.6747 1.6574 1.6405 1.6241 1.6081 1.5926 1.5776 1.5631 1.5491 1.5356 1.5225 1.5099 1.4978 1.4860 1.4747 1.4638 1.4532];
+K23=[5.2662 5.0707 4.8792 4.6930 4.5131 4.3401 4.1744 4.0161 3.8652 3.7216 3.5852 3.4556 3.3327 3.2160 3.1053 3.0003 2.9006 2.8060 2.7162 2.6308 2.5496 2.4724 2.3989 2.3289 2.2622];
+K24=[5.3712 5.1585 4.9545 4.7593 4.5731 4.3960 4.2278 4.0683 3.9172 3.7741 3.6388 3.5107 3.3895 3.2748 3.1663 3.0635 2.9662 2.8739 2.7863 2.7032 2.6243 2.5493 2.4779 2.4100 2.3453];
+K25=[11.6084 11.9794 12.3174 12.6247 12.9036 13.1566 13.3861 13.5942 13.7831 13.9546 14.1105 14.2524 14.3817 14.4997 14.6076 14.7063 14.7968 14.8800 14.9564 15.0268 15.0918 15.1518 15.2074 15.2589 15.3067];
+K26=[1.7058 1.7715 1.8315 1.8863 1.9362 1.9817 2.0231 2.0609 2.0953 2.1267 2.1553 2.1815 2.2055 2.2276 2.2478 2.2664 2.2835 2.2993 2.3139 2.3274 2.3399 2.3515 2.3623 2.3724 2.3817];
+
+% K11=-172.5944*L_0^3+194.4583*L_0^2-102.6573*L_0-3.2160
+% K12=-0.9840*L_0^3+0.4770*L_0^2-8.5684*L_0-0.3764
+% K13=-38.8812*L_0^3+40.3986*L_0^2-15.0820*L_0-2.3214
+% K14=-25.9829*L_0^3+28.2485*L_0^2-12.5490*L_0-2.7215
+% K15=-53.1626*L_0^3+71.4193*L_0^2-37.8708*L_0+9.8810
+% K16=-8.7478*L_0^3+11.6234*L_0^2-6.1338*L_0+1.7402
+% K21=182.7742*L_0^3-134.1998*L_0^2+17.2318*L_0+11.5442
+% K22=5.0007*L_0^3-1.7221*L_0^2-1.6219*L_0+2.0283
+% K23=-34.7106*L_0^3+55.1695*L_0^2-32.5200*L_0+8.4420
+% K24=-55.6192*L_0^3+72.6502*L_0^2-37.0784*L_0+8.8727
+% K25=214.7065*L_0^3-218.2300*L_0^2+79.9589*L_0+4.8045
+% K26=36.3354*L_0^3-37.3084*L_0^2+13.9211*L_0+0.5130
--- a/综合运动控制验证/K-L0/K11.sfit
+++ b/综合运动控制验证/K-L0/K11.sfit
--- a/综合运动控制验证/K-L0/K12.sfit
+++ b/综合运动控制验证/K-L0/K12.sfit
--- a/综合运动控制验证/K-L0/K13.sfit
+++ b/综合运动控制验证/K-L0/K13.sfit
--- a/综合运动控制验证/K-L0/K14.sfit
+++ b/综合运动控制验证/K-L0/K14.sfit
--- a/综合运动控制验证/K-L0/K15.sfit
+++ b/综合运动控制验证/K-L0/K15.sfit
--- a/综合运动控制验证/K-L0/K16.sfit
+++ b/综合运动控制验证/K-L0/K16.sfit
--- a/综合运动控制验证/K-L0/K21.sfit
+++ b/综合运动控制验证/K-L0/K21.sfit
--- a/综合运动控制验证/K-L0/K22.sfit
+++ b/综合运动控制验证/K-L0/K22.sfit
--- a/综合运动控制验证/K-L0/K23.sfit
+++ b/综合运动控制验证/K-L0/K23.sfit
--- a/综合运动控制验证/K-L0/K24.sfit
+++ b/综合运动控制验证/K-L0/K24.sfit
--- a/综合运动控制验证/K-L0/K25.sfit
+++ b/综合运动控制验证/K-L0/K25.sfit
--- a/综合运动控制验证/K-L0/K26.sfit
+++ b/综合运动控制验证/K-L0/K26.sfit
--- a/综合运动控制验证/LQR_K.m
+++ b/综合运动控制验证/LQR_K.m
@ -0,0 +1,84 @@
+close all;
+clear;clc;
+%% 参数
+global g R L L_M l M m_p m_w I_p I_M I_w;
+L0=0.16;
+g=9.80665;
+R=0.155/2;
+L=L0/2;
+L_M=L0/2;
+l=0.02;
+M=10;
+m_p=1;
+m_w=1;
+I_p=1/12*m_p*(L+L_M)^2;
+I_M=1/12*M*(0.1^2+0.3^2);
+I_w=1/2*m_w*R^2;
+
+A21=(I_M*I_w*L*g*m_p+I_M*L*M^2*R^2*g+I_M*L_M*M^2*R^2*g+I_w*L*M^2*g*l^2+I_w*L_M*M^2*g*l^2+I_M*L*R^2*g*m_p^2+I_M*I_w*L*M*g+I_M*I_w*L_M*M*g+L*M*R^2*g*l^2*m_p^2+L*M^2*R^2*g*l^2*m_p+L*M^2*R^2*g*l^2*m_w+L_M*M^2*R^2*g*l^2*m_p+L_M*M^2*R^2*g*l^2*m_w+2*I_M*L*M*R^2*g*m_p+I_M*L*M*R^2*g*m_w+I_M*L_M*M*R^2*g*m_p+I_M*L_M*M*R^2*g*m_w+I_w*L*M*g*l^2*m_p+I_M*L*R^2*g*m_p*m_w+L*M*R^2*g*l^2*m_p*m_w)/(I_M*I_p*I_w+I_p*I_w*M*l^2+I_M*I_w*L^2*m_p+I_M*I_p*R^2*m_p+I_M*I_p*R^2*m_w+I_M*I_w*L^2*M+I_M*I_w*L_M^2*M+I_M*I_p*M*R^2+I_M*L^2*M*R^2*m_w+I_M*L_M^2*M*R^2*m_p+I_M*L_M^2*M*R^2*m_w+2*I_M*I_w*L*L_M*M+I_w*L^2*M*l^2*m_p+I_p*M*R^2*l^2*m_p+I_p*M*R^2*l^2*m_w+I_M*L^2*R^2*m_p*m_w+2*I_M*L*L_M*M*R^2*m_w+L^2*M*R^2*l^2*m_p*m_w);
+A25=(I_w*L*M^2*g*l^2+I_w*L_M*M^2*g*l^2+L*M^2*R^2*g*l^2*m_w+L_M*M^2*R^2*g*l^2*m_p+L_M*M^2*R^2*g*l^2*m_w)/(I_M*I_p*I_w+I_p*I_w*M*l^2+I_M*I_w*L^2*m_p+I_M*I_p*R^2*m_p+I_M*I_p*R^2*m_w+I_M*I_w*L^2*M+I_M*I_w*L_M^2*M+I_M*I_p*M*R^2+I_M*L^2*M*R^2*m_w+I_M*L_M^2*M*R^2*m_p+I_M*L_M^2*M*R^2*m_w+2*I_M*I_w*L*L_M*M+I_w*L^2*M*l^2*m_p+I_p*M*R^2*l^2*m_p+I_p*M*R^2*l^2*m_w+I_M*L^2*R^2*m_p*m_w+2*I_M*L*L_M*M*R^2*m_w+L^2*M*R^2*l^2*m_p*m_w);
+A41=-(R*(R*g*L^2*M^2*l^2*m_p+I_M*R*g*L^2*M^2+R*g*L^2*M*l^2*m_p^2+2*I_M*R*g*L^2*M*m_p+I_M*R*g*L^2*m_p^2+R*g*L*L_M*M^2*l^2*m_p+2*I_M*R*g*L*L_M*M^2+2*I_M*R*g*L*L_M*M*m_p+I_M*R*g*L_M^2*M^2))/(I_M*I_p*I_w+I_p*I_w*M*l^2+I_M*I_w*L^2*m_p+I_M*I_p*R^2*m_p+I_M*I_p*R^2*m_w+I_M*I_w*L^2*M+I_M*I_w*L_M^2*M+I_M*I_p*M*R^2+I_M*L^2*M*R^2*m_w+I_M*L_M^2*M*R^2*m_p+I_M*L_M^2*M*R^2*m_w+2*I_M*I_w*L*L_M*M+I_w*L^2*M*l^2*m_p+I_p*M*R^2*l^2*m_p+I_p*M*R^2*l^2*m_w+I_M*L^2*R^2*m_p*m_w+2*I_M*L*L_M*M*R^2*m_w+L^2*M*R^2*l^2*m_p*m_w);
+A45=(R*(I_p*M^2*R*g*l^2-L*L_M*M^2*R*g*l^2*m_p))/(I_M*I_p*I_w+I_p*I_w*M*l^2+I_M*I_w*L^2*m_p+I_M*I_p*R^2*m_p+I_M*I_p*R^2*m_w+I_M*I_w*L^2*M+I_M*I_w*L_M^2*M+I_M*I_p*M*R^2+I_M*L^2*M*R^2*m_w+I_M*L_M^2*M*R^2*m_p+I_M*L_M^2*M*R^2*m_w+2*I_M*I_w*L*L_M*M+I_w*L^2*M*l^2*m_p+I_p*M*R^2*l^2*m_p+I_p*M*R^2*l^2*m_w+I_M*L^2*R^2*m_p*m_w+2*I_M*L*L_M*M*R^2*m_w+L^2*M*R^2*l^2*m_p*m_w);
+A61=(g*l*m_w*L^2*M^2*R^2+I_w*g*l*L^2*M^2+g*l*m_w*L^2*M*R^2*m_p+I_w*g*l*L^2*M*m_p+g*l*L*L_M*M^2*R^2*m_p+2*g*l*m_w*L*L_M*M^2*R^2+2*I_w*g*l*L*L_M*M^2+g*l*L*L_M*M*R^2*m_p^2+g*l*m_w*L*L_M*M*R^2*m_p+I_w*g*l*L*L_M*M*m_p+g*l*L_M^2*M^2*R^2*m_p+g*l*m_w*L_M^2*M^2*R^2+I_w*g*l*L_M^2*M^2)/(I_M*I_p*I_w+I_p*I_w*M*l^2+I_M*I_w*L^2*m_p+I_M*I_p*R^2*m_p+I_M*I_p*R^2*m_w+I_M*I_w*L^2*M+I_M*I_w*L_M^2*M+I_M*I_p*M*R^2+I_M*L^2*M*R^2*m_w+I_M*L_M^2*M*R^2*m_p+I_M*L_M^2*M*R^2*m_w+2*I_M*I_w*L*L_M*M+I_w*L^2*M*l^2*m_p+I_p*M*R^2*l^2*m_p+I_p*M*R^2*l^2*m_w+I_M*L^2*R^2*m_p*m_w+2*I_M*L*L_M*M*R^2*m_w+L^2*M*R^2*l^2*m_p*m_w);
+A65=(I_p*I_w*M*g*l+I_w*L^2*M^2*g*l+I_w*L_M^2*M^2*g*l+I_p*M^2*R^2*g*l+L^2*M^2*R^2*g*l*m_w+L_M^2*M^2*R^2*g*l*m_p+L_M^2*M^2*R^2*g*l*m_w+2*I_w*L*L_M*M^2*g*l+I_w*L^2*M*g*l*m_p+I_p*M*R^2*g*l*m_p+I_p*M*R^2*g*l*m_w+2*L*L_M*M^2*R^2*g*l*m_w+L^2*M*R^2*g*l*m_p*m_w)/(I_M*I_p*I_w + I_p*I_w*M*l^2+I_M*I_w*L^2*m_p+I_M*I_p*R^2*m_p+I_M*I_p*R^2*m_w+I_M*I_w*L^2*M+I_M*I_w*L_M^2*M+I_M*I_p*M*R^2+I_M*L^2*M*R^2*m_w+I_M*L_M^2*M*R^2*m_p+I_M*L_M^2*M*R^2*m_w+2*I_M*I_w*L*L_M*M+I_w*L^2*M*l^2*m_p+I_p*M*R^2*l^2*m_p+I_p*M*R^2*l^2*m_w+I_M*L^2*R^2*m_p*m_w+2*I_M*L*L_M*M*R^2*m_w+L^2*M*R^2*l^2*m_p*m_w);
+A=[ 0   1   0   0   0   0;
+    A21 0   0   0   A25 0;
+    0   0   0   1   0   0;
+    A41 0   0   0   A45 0;
+    0   0   0   0   0   1;
+    A61 0   0   0   A65 0];
+
+B21=-(I_M*I_w+I_M*M*R^2+I_w*M*l^2+I_M*R^2*m_p+I_M*R^2*m_w+I_M*L*M*R+I_M*L_M*M*R+M*R^2*l^2*m_p+M*R^2*l^2*m_w+I_M*L*R*m_p+L*M*R*l^2*m_p)/(I_M*I_p*I_w+I_p*I_w*M*l^2+I_M*I_w*L^2*m_p+I_M*I_p*R^2*m_p+I_M*I_p*R^2*m_w+I_M*I_w*L^2*M+I_M*I_w*L_M^2*M+I_M*I_p*M*R^2+I_M*L^2*M*R^2*m_w+I_M*L_M^2*M*R^2*m_p+I_M*L_M^2*M*R^2*m_w+2*I_M*I_w*L*L_M*M+I_w*L^2*M*l^2*m_p+I_p*M*R^2*l^2*m_p+I_p*M*R^2*l^2*m_w+I_M*L^2*R^2*m_p*m_w+2*I_M*L*L_M*M*R^2*m_w+L^2*M*R^2*l^2*m_p*m_w);
+B22=(I_M*I_w+I_M*M*R^2+I_w*M*l^2+I_M*R^2*m_p+I_M*R^2*m_w+M*R^2*l^2*m_p+M*R^2*l^2*m_w+I_w*L*M*l+I_w*L_M*M*l+L*M*R^2*l*m_w+L_M*M*R^2*l*m_p+L_M*M*R^2*l*m_w)/(I_M*I_p*I_w+I_p*I_w*M*l^2+I_M*I_w*L^2*m_p+I_M*I_p*R^2*m_p+I_M*I_p*R^2*m_w+I_M*I_w*L^2*M+I_M*I_w*L_M^2*M+I_M*I_p*M*R^2+I_M*L^2*M*R^2*m_w+I_M*L_M^2*M*R^2*m_p+I_M*L_M^2*M*R^2*m_w+2*I_M*I_w*L*L_M*M+I_w*L^2*M*l^2*m_p+I_p*M*R^2*l^2*m_p+I_p*M*R^2*l^2*m_w+I_M*L^2*R^2*m_p*m_w+2*I_M*L*L_M*M*R^2*m_w+L^2*M*R^2*l^2*m_p*m_w);
+B41=(R*(I_M*I_p+I_M*L^2*M+I_M*L_M^2*M+I_p*M*l^2+I_M*L^2*m_p+2*I_M*L*L_M*M+L^2*M*l^2*m_p+I_M*L*M*R+I_M*L_M*M*R+I_M*L*R*m_p+L*M*R*l^2*m_p))/(I_M*I_p*I_w+I_p*I_w*M*l^2+I_M*I_w*L^2*m_p+I_M*I_p*R^2*m_p+I_M*I_p*R^2*m_w+I_M*I_w*L^2*M+I_M*I_w*L_M^2*M+I_M*I_p*M*R^2+I_M*L^2*M*R^2*m_w+I_M*L_M^2*M*R^2*m_p+I_M*L_M^2*M*R^2*m_w+2*I_M*I_w*L*L_M*M+I_w*L^2*M*l^2*m_p+I_p*M*R^2*l^2*m_p+I_p*M*R^2*l^2*m_w+I_M*L^2*R^2*m_p*m_w+2*I_M*L*L_M*M*R^2*m_w+L^2*M*R^2*l^2*m_p*m_w);
+B42=-(R*(I_M*L*M*R+I_M*L_M*M*R-I_p*M*R*l+I_M*L*R*m_p+L*M*R*l^2*m_p+L*L_M*M*R*l*m_p))/(I_M*I_p*I_w+I_p*I_w*M*l^2+I_M*I_w*L^2*m_p+I_M*I_p*R^2*m_p+I_M*I_p*R^2*m_w+I_M*I_w*L^2*M+I_M*I_w*L_M^2*M+I_M*I_p*M*R^2+I_M*L^2*M*R^2*m_w+I_M*L_M^2*M*R^2*m_p+I_M*L_M^2*M*R^2*m_w+2*I_M*I_w*L*L_M*M+I_w*L^2*M*l^2*m_p+I_p*M*R^2*l^2*m_p+I_p*M*R^2*l^2*m_w+I_M*L^2*R^2*m_p*m_w+2*I_M*L*L_M*M*R^2*m_w+L^2*M*R^2*l^2*m_p*m_w);
+B61=-(I_w*L*M*l+I_w*L_M*M*l-I_p*M*R*l+L*M*R^2*l*m_w+L_M*M*R^2*l*m_p+L_M*M*R^2*l*m_w+L*L_M*M*R*l*m_p)/(I_M*I_p*I_w+I_p*I_w*M*l^2+I_M*I_w*L^2*m_p+I_M*I_p*R^2*m_p+I_M*I_p*R^2*m_w+I_M*I_w*L^2*M+I_M*I_w*L_M^2*M+I_M*I_p*M*R^2+I_M*L^2*M*R^2*m_w+I_M*L_M^2*M*R^2*m_p+I_M*L_M^2*M*R^2*m_w+2*I_M*I_w*L*L_M*M+I_w*L^2*M*l^2*m_p+I_p*M*R^2*l^2*m_p+I_p*M*R^2*l^2*m_w+I_M*L^2*R^2*m_p*m_w+2*I_M*L*L_M*M*R^2*m_w+L^2*M*R^2*l^2*m_p*m_w);
+B62=(I_p*I_w+I_w*L^2*M+I_w*L_M^2*M+I_p*M*R^2+I_w*L^2*m_p+I_p*R^2*m_p+I_p*R^2*m_w+L^2*M*R^2*m_w+L_M^2*M*R^2*m_p+L_M^2*M*R^2*m_w+2*I_w*L*L_M*M+L^2*R^2*m_p*m_w+I_w*L*M*l+I_w*L_M*M*l+2*L*L_M*M*R^2*m_w+L*M*R^2*l*m_w+L_M*M*R^2*l*m_p+L_M*M*R^2*l*m_w)/(I_M*I_p*I_w+I_p*I_w*M*l^2+I_M*I_w*L^2*m_p+I_M*I_p*R^2*m_p+I_M*I_p*R^2*m_w+I_M*I_w*L^2*M+I_M*I_w*L_M^2*M+I_M*I_p*M*R^2+I_M*L^2*M*R^2*m_w+I_M*L_M^2*M*R^2*m_p+I_M*L_M^2*M*R^2*m_w+2*I_M*I_w*L*L_M*M+I_w*L^2*M*l^2*m_p+I_p*M*R^2*l^2*m_p+I_p*M*R^2*l^2*m_w+I_M*L^2*R^2*m_p*m_w+2*I_M*L*L_M*M*R^2*m_w+L^2*M*R^2*l^2*m_p*m_w);
+B=[ 0   0;
+    B21 B22;
+    0   0;
+    B41 B42;
+    0   0;
+    B61 B62];
+
+C=[ 1   0   0   0   0   0;
+    0   1   0   0   0   0;
+    0   0   1   0   0   0;
+    0   0   0   1   0   0;
+    0   0   0   0   1   0;
+    0   0   0   0   0   1];
+D=[ 0   0;
+    0   0;
+    0   0;
+    0   0;
+    0   0;
+    0   0];
+
+Q=[ 1   0   0   0   0   0;
+    0   1   0   0   0   0;
+    0   0   20   0   0   0;
+    0   0   0   5   0   0;
+    0   0   0   0   50   0;
+    0   0   0   0   0   1];
+R_=[1   0;
+    0   0.25];
+%% 
+Qc=ctrb(A,B);
+rank(Qc);
+%% 计算增益矩阵
+[K,S,P]=lqr(A,B,Q,R_);
+K
+%% 模拟系统响应
+% figure(1);
+% x0=[10/180*pi;
+%     0;
+%     0;
+%     0;
+%     10/180*pi;
+%     0];%初始状态
+% t=0:0.05:10;
+% u=[ zeros(size(t));
+%     zeros(size(t));];
+% [y,x]=lsim(A-B*K,B,C,D,u,t,x0);%模拟系统响应
+% plot(t,y);
+% legend('θ(t)','dθ(t)','x(t)','dx(t)','ɸ','dɸ')%添加图例
--- a/综合运动控制验证/Model.slx
+++ b/综合运动控制验证/Model.slx
--- a/综合运动控制验证/Model.slxc
+++ b/综合运动控制验证/Model.slxc
--- a/综合运动控制验证/slprj/_jitprj/jitEngineAccessInfo.mat
+++ b/综合运动控制验证/slprj/_jitprj/jitEngineAccessInfo.mat
--- a/综合运动控制验证/slprj/_jitprj/s0vwwxbTK9CDedxV2kbjKiH.l
+++ b/综合运动控制验证/slprj/_jitprj/s0vwwxbTK9CDedxV2kbjKiH.l
--- a/综合运动控制验证/slprj/_jitprj/s0vwwxbTK9CDedxV2kbjKiH.mat
+++ b/综合运动控制验证/slprj/_jitprj/s0vwwxbTK9CDedxV2kbjKiH.mat
--- a/综合运动控制验证/slprj/_jitprj/s6GqfsbzkYp8msFZ3ezEj3D.l
+++ b/综合运动控制验证/slprj/_jitprj/s6GqfsbzkYp8msFZ3ezEj3D.l
--- a/综合运动控制验证/slprj/_jitprj/s6GqfsbzkYp8msFZ3ezEj3D.mat
+++ b/综合运动控制验证/slprj/_jitprj/s6GqfsbzkYp8msFZ3ezEj3D.mat
--- a/综合运动控制验证/slprj/_jitprj/s8YnbwTZehsvcOspDxGpyP.l
+++ b/综合运动控制验证/slprj/_jitprj/s8YnbwTZehsvcOspDxGpyP.l
--- a/综合运动控制验证/slprj/_jitprj/s8YnbwTZehsvcOspDxGpyP.mat
+++ b/综合运动控制验证/slprj/_jitprj/s8YnbwTZehsvcOspDxGpyP.mat
--- a/综合运动控制验证/slprj/_jitprj/sB8EMV89BknB3IzvYkqe0sE.l
+++ b/综合运动控制验证/slprj/_jitprj/sB8EMV89BknB3IzvYkqe0sE.l
--- a/综合运动控制验证/slprj/_jitprj/sB8EMV89BknB3IzvYkqe0sE.mat
+++ b/综合运动控制验证/slprj/_jitprj/sB8EMV89BknB3IzvYkqe0sE.mat
--- a/综合运动控制验证/slprj/_jitprj/sByosmr5EQsC08grpLuIp5G.l
+++ b/综合运动控制验证/slprj/_jitprj/sByosmr5EQsC08grpLuIp5G.l
--- a/综合运动控制验证/slprj/_jitprj/sByosmr5EQsC08grpLuIp5G.mat
+++ b/综合运动控制验证/slprj/_jitprj/sByosmr5EQsC08grpLuIp5G.mat
--- a/综合运动控制验证/slprj/_jitprj/sijTRwvBORnsM02ONsB5BKG.l
+++ b/综合运动控制验证/slprj/_jitprj/sijTRwvBORnsM02ONsB5BKG.l
--- a/综合运动控制验证/slprj/_jitprj/sijTRwvBORnsM02ONsB5BKG.mat
+++ b/综合运动控制验证/slprj/_jitprj/sijTRwvBORnsM02ONsB5BKG.mat
--- a/综合运动控制验证/slprj/_jitprj/siv1NDWcWUqaBAS1K1NPqrF.l
+++ b/综合运动控制验证/slprj/_jitprj/siv1NDWcWUqaBAS1K1NPqrF.l
--- a/综合运动控制验证/slprj/_jitprj/siv1NDWcWUqaBAS1K1NPqrF.mat
+++ b/综合运动控制验证/slprj/_jitprj/siv1NDWcWUqaBAS1K1NPqrF.mat
--- a/综合运动控制验证/slprj/_jitprj/sodilRL7jpmqbxDZvN66MnE.l
+++ b/综合运动控制验证/slprj/_jitprj/sodilRL7jpmqbxDZvN66MnE.l
--- a/综合运动控制验证/slprj/_jitprj/sodilRL7jpmqbxDZvN66MnE.mat
+++ b/综合运动控制验证/slprj/_jitprj/sodilRL7jpmqbxDZvN66MnE.mat
--- a/综合运动控制验证/slprj/_sfprj/EMLReport/emlReportAccessInfo.mat
+++ b/综合运动控制验证/slprj/_sfprj/EMLReport/emlReportAccessInfo.mat
--- a/综合运动控制验证/slprj/_sfprj/EMLReport/s0vwwxbTK9CDedxV2kbjKiH.mat
+++ b/综合运动控制验证/slprj/_sfprj/EMLReport/s0vwwxbTK9CDedxV2kbjKiH.mat
--- a/综合运动控制验证/slprj/_sfprj/EMLReport/s6GqfsbzkYp8msFZ3ezEj3D.mat
+++ b/综合运动控制验证/slprj/_sfprj/EMLReport/s6GqfsbzkYp8msFZ3ezEj3D.mat
--- a/综合运动控制验证/slprj/_sfprj/EMLReport/s8YnbwTZehsvcOspDxGpyP.mat
+++ b/综合运动控制验证/slprj/_sfprj/EMLReport/s8YnbwTZehsvcOspDxGpyP.mat
--- a/综合运动控制验证/slprj/_sfprj/EMLReport/sB8EMV89BknB3IzvYkqe0sE.mat
+++ b/综合运动控制验证/slprj/_sfprj/EMLReport/sB8EMV89BknB3IzvYkqe0sE.mat
--- a/综合运动控制验证/slprj/_sfprj/EMLReport/sByosmr5EQsC08grpLuIp5G.mat
+++ b/综合运动控制验证/slprj/_sfprj/EMLReport/sByosmr5EQsC08grpLuIp5G.mat
--- a/综合运动控制验证/slprj/_sfprj/EMLReport/sijTRwvBORnsM02ONsB5BKG.mat
+++ b/综合运动控制验证/slprj/_sfprj/EMLReport/sijTRwvBORnsM02ONsB5BKG.mat
--- a/综合运动控制验证/slprj/_sfprj/EMLReport/siv1NDWcWUqaBAS1K1NPqrF.mat
+++ b/综合运动控制验证/slprj/_sfprj/EMLReport/siv1NDWcWUqaBAS1K1NPqrF.mat
--- a/综合运动控制验证/slprj/_sfprj/EMLReport/sodilRL7jpmqbxDZvN66MnE.mat
+++ b/综合运动控制验证/slprj/_sfprj/EMLReport/sodilRL7jpmqbxDZvN66MnE.mat
--- a/综合运动控制验证/slprj/_sfprj/Model/_self/sfun/info/binfo.mat
+++ b/综合运动控制验证/slprj/_sfprj/Model/_self/sfun/info/binfo.mat
--- a/综合运动控制验证/slprj/_sfprj/Model/amsi_serial.mat
+++ b/综合运动控制验证/slprj/_sfprj/Model/amsi_serial.mat
--- a/综合运动控制验证/slprj/_sfprj/precompile/DKm7VD4RS4KiqU7UcUp7XF.mat
+++ b/综合运动控制验证/slprj/_sfprj/precompile/DKm7VD4RS4KiqU7UcUp7XF.mat
--- a/综合运动控制验证/slprj/_sfprj/precompile/I34YT8nshSXJXgI7sPqvUD.mat
+++ b/综合运动控制验证/slprj/_sfprj/precompile/I34YT8nshSXJXgI7sPqvUD.mat
--- a/综合运动控制验证/slprj/_sfprj/precompile/R1LpZUs7xcKZ5htyENMI2D.mat
+++ b/综合运动控制验证/slprj/_sfprj/precompile/R1LpZUs7xcKZ5htyENMI2D.mat
--- a/综合运动控制验证/slprj/_sfprj/precompile/RIMNwLfnzIcgTJT66Qo8.mat
+++ b/综合运动控制验证/slprj/_sfprj/precompile/RIMNwLfnzIcgTJT66Qo8.mat
--- a/综合运动控制验证/slprj/_sfprj/precompile/TIGnDpcVdFryz8vINGhCEE.mat
+++ b/综合运动控制验证/slprj/_sfprj/precompile/TIGnDpcVdFryz8vINGhCEE.mat
--- a/综合运动控制验证/slprj/_sfprj/precompile/Yx6TgKee4N4T2ot09TXbBG.mat
+++ b/综合运动控制验证/slprj/_sfprj/precompile/Yx6TgKee4N4T2ot09TXbBG.mat
--- a/综合运动控制验证/slprj/_sfprj/precompile/autoInferAccessInfo.mat
+++ b/综合运动控制验证/slprj/_sfprj/precompile/autoInferAccessInfo.mat
--- a/综合运动控制验证/slprj/_sfprj/precompile/d1zFJZIj1S9dYQEXX5AFlG.mat
+++ b/综合运动控制验证/slprj/_sfprj/precompile/d1zFJZIj1S9dYQEXX5AFlG.mat
--- a/综合运动控制验证/slprj/_sfprj/precompile/gfaJRXgtc6p3TDS6SE8s9C.mat
+++ b/综合运动控制验证/slprj/_sfprj/precompile/gfaJRXgtc6p3TDS6SE8s9C.mat
--- a/综合运动控制验证/slprj/_sfprj/precompile/hYU3tajIOO2KbScZdLHog.mat
+++ b/综合运动控制验证/slprj/_sfprj/precompile/hYU3tajIOO2KbScZdLHog.mat
--- a/综合运动控制验证/slprj/_sfprj/precompile/l6RTgSsb40bUdzK9VpJs8C.mat
+++ b/综合运动控制验证/slprj/_sfprj/precompile/l6RTgSsb40bUdzK9VpJs8C.mat
--- a/综合运动控制验证/slprj/_sfprj/precompile/m5D2pLZZ8rHTm7xQkmUPEE.mat
+++ b/综合运动控制验证/slprj/_sfprj/precompile/m5D2pLZZ8rHTm7xQkmUPEE.mat
--- a/综合运动控制验证/slprj/_sfprj/precompile/nNYhtnI1LwJcnS3byEmn6B.mat
+++ b/综合运动控制验证/slprj/_sfprj/precompile/nNYhtnI1LwJcnS3byEmn6B.mat
--- a/综合运动控制验证/slprj/_sfprj/precompile/vrEWI1U4wZ6I6vIUJNS4oE.mat
+++ b/综合运动控制验证/slprj/_sfprj/precompile/vrEWI1U4wZ6I6vIUJNS4oE.mat
--- a/综合运动控制验证/场地/Floor.slx
+++ b/综合运动控制验证/场地/Floor.slx
--- a/综合运动控制验证/场地/untitled.slx.autosave
+++ b/综合运动控制验证/场地/untitled.slx.autosave
--- a/utils/Simulation-master/balance/series_legs/get_k_length.m
+++ b/utils/Simulation-master/balance/series_legs/get_k_length.m
@ -17,10 +17,10 @@ function K = get_k_length(leg_length)
    R1=0.068;                         %驱动轮半径
    L1=leg_length/2;                  %摆杆重心到驱动轮轴距离
    LM1=leg_length/2;                 %摆杆重心到其转轴距离
-    l1=0.01;                          %机体质心距离转轴距离
+    l1=0.03;                          %机体质心距离转轴距离
    mw1=0.60;                         %驱动轮质量
    mp1=1.8;                          %杆质量
-    M1=12.0;                          %机体质量
+    M1=16.0;                          %机体质量
    Iw1=mw1*R1^2;                     %驱动轮转动惯量
    Ip1=mp1*((L1+LM1)^2+0.05^2)/12.0; %摆杆转动惯量
    IM1=M1*(0.3^2+0.12^2)/12.0;       %机体绕质心转动惯量
@ -48,8 +48,8 @@ function K = get_k_length(leg_length)
    B=subs(B,[R,L,LM,l,mw,mp,M,Iw,Ip,IM,g],[R1,L1,LM1,l1,mw1,mp1,M1,Iw1,Ip1,IM1,9.8]);
    B=double(B);
    
-    Q=diag([1500 200 5000 2000 20000 1000]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
-    R=[240 0;0 60];                %T Tp
+    Q=diag([1  1  20  5  50 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
+    R=[1.2 0;0 0.25];                %T Tp
    
    K=lqr(A,B,Q,R);
Author	SHA1	Message	Date
Robofish	5b89b3c4f4	feat: add Kalman filter component for chassis Added kalman_filter.c to the project structure for chassis state estimation. Updated Keil MDK project files (.uvprojx, .uvoptx) to include the new component and modified the debug watch window to monitor chassis variables.	2026-02-09 20:21:32 +08:00
Robofish	ec22b87dfa	feat(balance_chassis): enable wheel-leg balance mode and adjust parameters activate wheel-leg balance mode in RC switch case update crouch and retract leg lengths in config adjust non-contact theta threshold in chassis module add README and USAGE documentation files	2026-02-09 14:59:50 +08:00
Robofish	b5af2b9967	暂时关闭速度规划	2026-02-07 21:16:49 +08:00
Robofish	79bc3f5f89	改参数	2026-02-05 17:59:49 +08:00
Robofish	6ed012e759	修好了	2026-02-04 17:39:24 +08:00
Robofish	3f9f3ef734	修mr	2026-02-04 16:49:48 +08:00
Robofish	62fdf6e4ea	跟心mr	2026-02-04 16:16:53 +08:00
Robofish	05dae2219e	OK了	2026-02-04 15:24:56 +08:00