更新变形舵

Core/Inc/main.h

@@ -59,11 +59,21 @@ void Error_Handler(void);
 /* Private defines -----------------------------------------------------------*/
 #define CMPS_RST_Pin GPIO_PIN_6
 #define CMPS_RST_GPIO_Port GPIOG
+#define LED_Red_Pin GPIO_PIN_12
+#define LED_Red_GPIO_Port GPIOH
 #define CMPS_INT_Pin GPIO_PIN_3
 #define CMPS_INT_GPIO_Port GPIOG
 #define CMPS_INT_EXTI_IRQn EXTI3_IRQn
-#define step_Pin GPIO_PIN_1
+#define LED_Green_Pin GPIO_PIN_11
-#define step_GPIO_Port GPIOC
+#define LED_Green_GPIO_Port GPIOH
+#define LED_Blue_Pin GPIO_PIN_10
+#define LED_Blue_GPIO_Port GPIOH
+#define PUL_P_Pin GPIO_PIN_1
+#define PUL_P_GPIO_Port GPIOC
+#define PUL_N_Pin GPIO_PIN_2
+#define PUL_N_GPIO_Port GPIOC
+#define DIR_N_Pin GPIO_PIN_2
+#define DIR_N_GPIO_Port GPIOB
 #define USER_KEY_Pin GPIO_PIN_0
 #define USER_KEY_GPIO_Port GPIOA
 #define USER_KEY_EXTI_IRQn EXTI0_IRQn
@@ -75,8 +85,8 @@ void Error_Handler(void);
 #define GYRO_INT_Pin GPIO_PIN_5
 #define GYRO_INT_GPIO_Port GPIOC
 #define GYRO_INT_EXTI_IRQn EXTI9_5_IRQn
-#define Step_direction_Pin GPIO_PIN_1
+#define DIR_P_Pin GPIO_PIN_1
-#define Step_direction_GPIO_Port GPIOB
+#define DIR_P_GPIO_Port GPIOB
 #define GYRO_CS_Pin GPIO_PIN_0
 #define GYRO_CS_GPIO_Port GPIOB
 

Core/Inc/stm32f4xx_it.h

@@ -22,7 +22,7 @@
 #define __STM32F4xx_IT_H
 
 #ifdef __cplusplus
- extern "C" {
+extern "C" {
 #endif
 
 /* Private includes ----------------------------------------------------------*/
@@ -62,6 +62,7 @@ void CAN1_RX0_IRQHandler(void);
 void CAN1_RX1_IRQHandler(void);
 void EXTI9_5_IRQHandler(void);
 void TIM1_UP_TIM10_IRQHandler(void);
+void USART1_IRQHandler(void);
 void USART3_IRQHandler(void);
 void DMA2_Stream1_IRQHandler(void);
 void DMA2_Stream2_IRQHandler(void);

Core/Src/gpio.c

@@ -57,64 +57,77 @@ void MX_GPIO_Init(void)
   HAL_GPIO_WritePin(CMPS_RST_GPIO_Port, CMPS_RST_Pin, GPIO_PIN_RESET);
 
   /*Configure GPIO pin Output Level */
-  HAL_GPIO_WritePin(step_GPIO_Port, step_Pin, GPIO_PIN_RESET);
+  HAL_GPIO_WritePin(GPIOH, LED_Red_Pin|LED_Green_Pin|LED_Blue_Pin, GPIO_PIN_RESET);
+
+  /*Configure GPIO pin Output Level */
+  HAL_GPIO_WritePin(PUL_P_GPIO_Port, PUL_P_Pin, GPIO_PIN_SET);
+
+  /*Configure GPIO pin Output Level */
+  HAL_GPIO_WritePin(PUL_N_GPIO_Port, PUL_N_Pin, GPIO_PIN_RESET);
+
+  /*Configure GPIO pin Output Level */
+  HAL_GPIO_WritePin(GPIOB, DIR_N_Pin|DIR_P_Pin, GPIO_PIN_RESET);
 
   /*Configure GPIO pin Output Level */
   HAL_GPIO_WritePin(ACCL_CS_GPIO_Port, ACCL_CS_Pin, GPIO_PIN_SET);
 
-  /*Configure GPIO pin Output Level */
-  HAL_GPIO_WritePin(Step_direction_GPIO_Port, Step_direction_Pin, GPIO_PIN_RESET);
-
   /*Configure GPIO pin Output Level */
   HAL_GPIO_WritePin(GYRO_CS_GPIO_Port, GYRO_CS_Pin, GPIO_PIN_SET);
 
-  /*Configure GPIO pin : PtPin */
+  /*Configure GPIO pin : CMPS_RST_Pin */
   GPIO_InitStruct.Pin = CMPS_RST_Pin;
   GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
   GPIO_InitStruct.Pull = GPIO_PULLUP;
   GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_MEDIUM;
   HAL_GPIO_Init(CMPS_RST_GPIO_Port, &GPIO_InitStruct);
 
-  /*Configure GPIO pin : PtPin */
+  /*Configure GPIO pins : LED_Red_Pin LED_Green_Pin LED_Blue_Pin */
+  GPIO_InitStruct.Pin = LED_Red_Pin|LED_Green_Pin|LED_Blue_Pin;
+  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
+  GPIO_InitStruct.Pull = GPIO_PULLUP;
+  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+  HAL_GPIO_Init(GPIOH, &GPIO_InitStruct);
+
+  /*Configure GPIO pin : CMPS_INT_Pin */
   GPIO_InitStruct.Pin = CMPS_INT_Pin;
   GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING;
   GPIO_InitStruct.Pull = GPIO_PULLUP;
   HAL_GPIO_Init(CMPS_INT_GPIO_Port, &GPIO_InitStruct);
 
-  /*Configure GPIO pin : PtPin */
+  /*Configure GPIO pins : PUL_P_Pin PUL_N_Pin */
-  GPIO_InitStruct.Pin = step_Pin;
+  GPIO_InitStruct.Pin = PUL_P_Pin|PUL_N_Pin;
   GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
   GPIO_InitStruct.Pull = GPIO_PULLUP;
   GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
-  HAL_GPIO_Init(step_GPIO_Port, &GPIO_InitStruct);
+  HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
 
-  /*Configure GPIO pin : PtPin */
+  /*Configure GPIO pins : DIR_N_Pin DIR_P_Pin */
+  GPIO_InitStruct.Pin = DIR_N_Pin|DIR_P_Pin;
+  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
+  GPIO_InitStruct.Pull = GPIO_PULLUP;
+  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+  HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
+
+  /*Configure GPIO pin : USER_KEY_Pin */
   GPIO_InitStruct.Pin = USER_KEY_Pin;
   GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING;
   GPIO_InitStruct.Pull = GPIO_PULLUP;
   HAL_GPIO_Init(USER_KEY_GPIO_Port, &GPIO_InitStruct);
 
-  /*Configure GPIO pin : PtPin */
+  /*Configure GPIO pin : ACCL_CS_Pin */
   GPIO_InitStruct.Pin = ACCL_CS_Pin;
   GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
   GPIO_InitStruct.Pull = GPIO_PULLUP;
   GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_MEDIUM;
   HAL_GPIO_Init(ACCL_CS_GPIO_Port, &GPIO_InitStruct);
 
-  /*Configure GPIO pins : PCPin PCPin */
+  /*Configure GPIO pins : ACCL_INT_Pin GYRO_INT_Pin */
   GPIO_InitStruct.Pin = ACCL_INT_Pin|GYRO_INT_Pin;
   GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING;
   GPIO_InitStruct.Pull = GPIO_PULLUP;
   HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
 
-  /*Configure GPIO pin : PtPin */
+  /*Configure GPIO pin : GYRO_CS_Pin */
-  GPIO_InitStruct.Pin = Step_direction_Pin;
-  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
-  GPIO_InitStruct.Pull = GPIO_PULLUP;
-  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
-  HAL_GPIO_Init(Step_direction_GPIO_Port, &GPIO_InitStruct);
-
-  /*Configure GPIO pin : PtPin */
   GPIO_InitStruct.Pin = GYRO_CS_Pin;
   GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
   GPIO_InitStruct.Pull = GPIO_PULLUP;

Core/Src/main.c

@@ -110,9 +110,7 @@ int main(void)
   /* USER CODE END 2 */
 
   /* Init scheduler */
-  osKernelInitialize();
+  osKernelInitialize();  /* Call init function for freertos objects (in cmsis_os2.c) */
-
-  /* Call init function for freertos objects (in cmsis_os2.c) */
   MX_FREERTOS_Init();
 
   /* Start scheduler */
@@ -194,8 +192,7 @@ void Error_Handler(void)
   }
   /* USER CODE END Error_Handler_Debug */
 }
-
+#ifdef USE_FULL_ASSERT
-#ifdef  USE_FULL_ASSERT
 /**
   * @brief  Reports the name of the source file and the source line number
   *         where the assert_param error has occurred.

Core/Src/stm32f4xx_hal_msp.c

@@ -1,4 +1,3 @@
-
 /* USER CODE BEGIN Header */
 /**
   ******************************************************************************

Core/Src/stm32f4xx_it.c

@@ -66,6 +66,7 @@ extern TIM_HandleTypeDef htim10;
 extern DMA_HandleTypeDef hdma_usart3_rx;
 extern DMA_HandleTypeDef hdma_usart6_rx;
 extern DMA_HandleTypeDef hdma_usart6_tx;
+extern UART_HandleTypeDef huart1;
 extern UART_HandleTypeDef huart3;
 extern UART_HandleTypeDef huart6;
 /* USER CODE BEGIN EV */
@@ -318,6 +319,21 @@ void TIM1_UP_TIM10_IRQHandler(void)
   /* USER CODE END TIM1_UP_TIM10_IRQn 1 */
 }
 
+/**
+  * @brief This function handles USART1 global interrupt.
+  */
+void USART1_IRQHandler(void)
+{
+  /* USER CODE BEGIN USART1_IRQn 0 */
+
+  /* USER CODE END USART1_IRQn 0 */
+  HAL_UART_IRQHandler(&huart1);
+  /* USER CODE BEGIN USART1_IRQn 1 */
+  BSP_UART_IRQHandler(&huart1);
+
+  /* USER CODE END USART1_IRQn 1 */
+}
+
 /**
   * @brief This function handles USART3 global interrupt.
   */

Core/Src/usart.c

@@ -181,6 +181,9 @@ void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle)
     GPIO_InitStruct.Alternate = GPIO_AF7_USART1;
     HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
 
+    /* USART1 interrupt Init */
+    HAL_NVIC_SetPriority(USART1_IRQn, 5, 0);
+    HAL_NVIC_EnableIRQ(USART1_IRQn);
   /* USER CODE BEGIN USART1_MspInit 1 */
 
   /* USER CODE END USART1_MspInit 1 */
@@ -340,6 +343,8 @@ void HAL_UART_MspDeInit(UART_HandleTypeDef* uartHandle)
 
     HAL_GPIO_DeInit(GPIOA, GPIO_PIN_9);
 
+    /* USART1 interrupt Deinit */
+    HAL_NVIC_DisableIRQ(USART1_IRQn);
   /* USER CODE BEGIN USART1_MspDeInit 1 */
 
   /* USER CODE END USART1_MspDeInit 1 */

MDK-ARM/Steering Wheel_Infatry.uvoptx

@@ -120,7 +120,7 @@
         <SetRegEntry>
           <Number>0</Number>
           <Key>JL2CM3</Key>
-          <Name>-U20760100 -O14 -S2 -ZTIFSpeedSel5000 -A0 -C0 -JU1 -JI127.0.0.1 -JP0 -RST0 -TO18 -TC10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -TB1 -TFE0 -FO7 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)</Name>
+          <Name>-U20760100 -O78 -S2 -ZTIFSpeedSel5000 -A0 -C0 -JU1 -JI127.0.0.1 -JP0 -RST0 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO18 -TC10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -TB1 -TFE0 -FO7 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)</Name>
         </SetRegEntry>
         <SetRegEntry>
           <Number>0</Number>
@@ -142,11 +142,6 @@
           <Key>ARMDBGFLAGS</Key>
           <Name></Name>
         </SetRegEntry>
-        <SetRegEntry>
-          <Number>0</Number>
-          <Key>DLGUARM</Key>
-          <Name></Name>
-        </SetRegEntry>
         <SetRegEntry>
           <Number>0</Number>
           <Key>UL2CM3</Key>
@@ -170,6 +165,46 @@
           <WinNumber>1</WinNumber>
           <ItemText>et16s,0x0A</ItemText>
         </Ww>
+        <Ww>
+          <count>2</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>cmd</ItemText>
+        </Ww>
+        <Ww>
+          <count>3</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>g_adapters</ItemText>
+        </Ww>
+        <Ww>
+          <count>4</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>prev_state</ItemText>
+        </Ww>
+        <Ww>
+          <count>5</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>StepMotor_param,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>6</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>et16s_key_A</ItemText>
+        </Ww>
+        <Ww>
+          <count>7</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>Key_A</ItemText>
+        </Ww>
+        <Ww>
+          <count>8</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>key1,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>9</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>gimbal</ItemText>
+        </Ww>
       </WatchWindow1>
       <Tracepoint>
         <THDelay>0</THDelay>
@@ -1073,7 +1108,7 @@
 
   <Group>
     <GroupName>device</GroupName>
-    <tvExp>1</tvExp>
+    <tvExp>0</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
@@ -1209,6 +1244,42 @@
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
+    <File>
+      <GroupNumber>8</GroupNumber>
+      <FileNumber>78</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>..\User\device\step_motor.c</PathWithFileName>
+      <FilenameWithoutPath>step_motor.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
+    <File>
+      <GroupNumber>8</GroupNumber>
+      <FileNumber>79</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>..\User\device\led.c</PathWithFileName>
+      <FilenameWithoutPath>led.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
+    <File>
+      <GroupNumber>8</GroupNumber>
+      <FileNumber>80</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>..\User\device\vofa.c</PathWithFileName>
+      <FilenameWithoutPath>vofa.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
   </Group>
 
   <Group>
@@ -1219,7 +1290,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>78</FileNumber>
+      <FileNumber>81</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1231,7 +1302,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>79</FileNumber>
+      <FileNumber>82</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1243,7 +1314,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>80</FileNumber>
+      <FileNumber>83</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1255,7 +1326,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>81</FileNumber>
+      <FileNumber>84</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1265,18 +1336,6 @@
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
-    <File>
-      <GroupNumber>9</GroupNumber>
-      <FileNumber>82</FileNumber>
-      <FileType>1</FileType>
-      <tvExp>0</tvExp>
-      <tvExpOptDlg>0</tvExpOptDlg>
-      <bDave2>0</bDave2>
-      <PathWithFileName>..\User\module\step_motor.c</PathWithFileName>
-      <FilenameWithoutPath>step_motor.c</FilenameWithoutPath>
-      <RteFlg>0</RteFlg>
-      <bShared>0</bShared>
-    </File>
   </Group>
 
   <Group>
@@ -1287,7 +1346,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>83</FileNumber>
+      <FileNumber>85</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1299,7 +1358,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>84</FileNumber>
+      <FileNumber>86</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1311,7 +1370,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>85</FileNumber>
+      <FileNumber>87</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1323,7 +1382,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>86</FileNumber>
+      <FileNumber>88</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1335,7 +1394,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>87</FileNumber>
+      <FileNumber>89</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1347,7 +1406,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>88</FileNumber>
+      <FileNumber>90</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1359,7 +1418,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>89</FileNumber>
+      <FileNumber>91</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1371,7 +1430,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>90</FileNumber>
+      <FileNumber>92</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1383,7 +1442,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>91</FileNumber>
+      <FileNumber>93</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1395,7 +1454,19 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>92</FileNumber>
+      <FileNumber>94</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>..\User\task\vofa.c</PathWithFileName>
+      <FilenameWithoutPath>vofa.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
+    <File>
+      <GroupNumber>10</GroupNumber>
+      <FileNumber>95</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1407,7 +1478,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>93</FileNumber>
+      <FileNumber>96</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1421,13 +1492,13 @@
 
   <Group>
     <GroupName>cmd</GroupName>
-    <tvExp>1</tvExp>
+    <tvExp>0</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>11</GroupNumber>
-      <FileNumber>94</FileNumber>
+      <FileNumber>97</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1439,7 +1510,7 @@
     </File>
     <File>
       <GroupNumber>11</GroupNumber>
-      <FileNumber>95</FileNumber>
+      <FileNumber>98</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1451,7 +1522,7 @@
     </File>
     <File>
       <GroupNumber>11</GroupNumber>
-      <FileNumber>96</FileNumber>
+      <FileNumber>99</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1463,7 +1534,7 @@
     </File>
     <File>
       <GroupNumber>11</GroupNumber>
-      <FileNumber>97</FileNumber>
+      <FileNumber>100</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>

MDK-ARM/Steering Wheel_Infatry.uvprojx

@@ -1893,6 +1893,21 @@
               <FileType>1</FileType>
               <FilePath>..\User\device\motor_lk.c</FilePath>
             </File>
+            <File>
+              <FileName>step_motor.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\device\step_motor.c</FilePath>
+            </File>
+            <File>
+              <FileName>led.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\device\led.c</FilePath>
+            </File>
+            <File>
+              <FileName>vofa.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\device\vofa.c</FilePath>
+            </File>
           </Files>
         </Group>
         <Group>
@@ -1918,11 +1933,6 @@
               <FileType>1</FileType>
               <FilePath>..\User\module\chassis.c</FilePath>
             </File>
-            <File>
-              <FileName>step_motor.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\User\module\step_motor.c</FilePath>
-            </File>
           </Files>
         </Group>
         <Group>
@@ -1973,6 +1983,11 @@
               <FileType>1</FileType>
               <FilePath>..\User\task\step_motor.c</FilePath>
             </File>
+            <File>
+              <FileName>vofa.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\task\vofa.c</FilePath>
+            </File>
             <File>
               <FileName>init.c</FileName>
               <FileType>1</FileType>

MDK-ARM/Steering Wheel_Infatry/Steering Wheel_Infatry.build_log.htm

@@ -22,7 +22,7 @@ Dialog DLL:      TCM.DLL V1.48.0.0
  
 <h2>Project:</h2>
 D:\yunha\git_gimbal\RM\Steering Wheel_Infatry\MDK-ARM\Steering Wheel_Infatry.uvprojx
-Project File Date:  01/22/2026
+Project File Date:  01/26/2026
 
 <h2>Output:</h2>
 *** Using Compiler 'V6.16', folder: 'D:\Keil_v5\ARM\ARMCLANG\Bin'
@@ -35,7 +35,12 @@ Note: source file '..\User\task\ai.c' - object file renamed from 'Steering Wheel
 Note: source file '..\User\task\ET16s.c' - object file renamed from 'Steering Wheel_Infatry\ET16s.o' to 'Steering Wheel_Infatry\et16s_1.o'.
 Note: source file '..\User\task\dr16.c' - object file renamed from 'Steering Wheel_Infatry\dr16.o' to 'Steering Wheel_Infatry\dr16_1.o'.
 Note: source file '..\User\task\step_motor.c' - object file renamed from 'Steering Wheel_Infatry\step_motor.o' to 'Steering Wheel_Infatry\step_motor_1.o'.
+Note: source file '..\User\task\vofa.c' - object file renamed from 'Steering Wheel_Infatry\vofa.o' to 'Steering Wheel_Infatry\vofa_1.o'.
 Note: source file '..\User\module\cmd\cmd.c' - object file renamed from 'Steering Wheel_Infatry\cmd.o' to 'Steering Wheel_Infatry\cmd_1.o'.
+compiling init.c...
+linking...
+Program Size: Code=67008 RO-data=1720 RW-data=1224 ZI-data=119848  
+FromELF: creating hex file...
 "Steering Wheel_Infatry\Steering Wheel_Infatry.axf" - 0 Error(s), 0 Warning(s).
 
 <h2>Software Packages used:</h2>
@@ -60,7 +65,7 @@ Package Vendor: Keil
 
    * Component: ARM::CMSIS:CORE:5.4.0
       Include file:  CMSIS\Core\Include\tz_context.h
-Build Time Elapsed:  00:00:01
+Build Time Elapsed:  00:00:03
 </pre>
 </body>
 </html>

MDK-ARM/Steering Wheel_Infatry/Steering Wheel_Infatry.lnp

@@ -76,11 +76,13 @@
 "steering wheel_infatry\dr16.o"
 "steering wheel_infatry\oid.o"
 "steering wheel_infatry\motor_lk.o"
+"steering wheel_infatry\step_motor.o"
+"steering wheel_infatry\led.o"
+"steering wheel_infatry\vofa.o"
 "steering wheel_infatry\config.o"
 "steering wheel_infatry\gimbal.o"
 "steering wheel_infatry\shoot.o"
 "steering wheel_infatry\chassis.o"
-"steering wheel_infatry\step_motor.o"
 "steering wheel_infatry\ai_1.o"
 "steering wheel_infatry\cmd.o"
 "steering wheel_infatry\et16s_1.o"
@@ -90,6 +92,7 @@
 "steering wheel_infatry\chassis_ctrl.o"
 "steering wheel_infatry\shoot_ctrl.o"
 "steering wheel_infatry\step_motor_1.o"
+"steering wheel_infatry\vofa_1.o"
 "steering wheel_infatry\init.o"
 "steering wheel_infatry\user_task.o"
 "steering wheel_infatry\cmd_1.o"

MDK-ARM/Steering Wheel_Infatry/chassis.d

@@ -55,4 +55,4 @@ steering\ wheel_infatry/chassis.o: ..\User\module\chassis.c \
   ..\Middlewares\Third_Party\FreeRTOS\Source\include\task.h \
   ..\Middlewares\Third_Party\FreeRTOS\Source\include\list.h \
   ..\Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.h \
-  ..\User\bsp\time.h
+  ..\User\bsp\time.h D:\Keil_v5\ARM\ARMCLANG\Bin\..\include\stdlib.h

MDK-ARM/Steering Wheel_Infatry/init.d

@@ -59,4 +59,5 @@ steering\ wheel_infatry/init.o: ..\User\task\init.c \
   ..\User\device\motor_rm.h ..\User\device\motor.h \
   ..\User\module\chassis.h ..\User\module\struct_typedef.h \
   ..\User\device\bmi088.h ..\User\component\user_math.h \
-  ..\User\device\dr16.h ..\User\device\device.h ..\User\device\et16s.h
+  ..\User\device\dr16.h ..\User\device\device.h ..\User\device\et16s.h \
+  ..\User\device\step_motor.h

MDK-ARM/Steering Wheel_Infatry/led.d

@@ -0,0 +1,4 @@
+steering\ wheel_infatry/led.o: ..\User\device\led.c ..\User\device\led.h \
+  D:\Keil_v5\ARM\ARMCLANG\Bin\..\include\stdint.h ..\User\bsp\bsp.h \
+  ..\User\device\device.h \
+  D:\Keil_v5\ARM\ARMCLANG\Bin\..\include\stdbool.h

MDK-ARM/Steering Wheel_Infatry/step_motor.d

@@ -1,4 +1,4 @@
-steering\ wheel_infatry/step_motor.o: ..\User\module\step_motor.c \
+steering\ wheel_infatry/step_motor.o: ..\User\device\step_motor.c \
   ..\Core\Inc\main.h ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal.h \
   ..\Core\Inc\stm32f4xx_hal_conf.h \
   ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_rcc.h \
@@ -33,6 +33,7 @@ steering\ wheel_infatry/step_motor.o: ..\User\module\step_motor.c \
   ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_tim.h \
   ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_tim_ex.h \
   ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_uart.h \
-  ..\User\module\step_motor.h ..\User\bsp\gpio.h \
+  ..\User\device\step_motor.h \
-  D:\Keil_v5\ARM\ARMCLANG\Bin\..\include\stdbool.h ..\User\bsp\bsp.h \
+  ..\Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.h \
-  ..\Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.h
+  D:\Keil_v5\ARM\ARMCLANG\Bin\..\include\stdbool.h ..\User\bsp\gpio.h \
+  ..\User\bsp\bsp.h

MDK-ARM/Steering Wheel_Infatry/step_motor_1.d

@@ -11,4 +11,8 @@ steering\ wheel_infatry/step_motor_1.o: ..\User\task\step_motor.c \
   ..\Middlewares\Third_Party\FreeRTOS\Source\portable\RVDS\ARM_CM4F\portmacro.h \
   ..\Middlewares\Third_Party\FreeRTOS\Source\include\mpu_wrappers.h \
   ..\Middlewares\Third_Party\FreeRTOS\Source\include\task.h \
-  ..\Middlewares\Third_Party\FreeRTOS\Source\include\list.h
+  ..\Middlewares\Third_Party\FreeRTOS\Source\include\list.h \
+  ..\User\device\step_motor.h \
+  D:\Keil_v5\ARM\ARMCLANG\Bin\..\include\stdbool.h \
+  ..\User\device\ET16s.h ..\User\device\device.h ..\User\bsp\gpio.h \
+  ..\User\bsp\bsp.h

MDK-ARM/Steering Wheel_Infatry/vofa.d

@@ -0,0 +1,40 @@
+steering\ wheel_infatry/vofa.o: ..\User\device\vofa.c \
+  D:\Keil_v5\ARM\ARMCLANG\Bin\..\include\stdio.h \
+  D:\Keil_v5\ARM\ARMCLANG\Bin\..\include\string.h ..\User\device\vofa.h \
+  ..\User\bsp\uart.h ..\Core\Inc\usart.h ..\Core\Inc\main.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal.h \
+  ..\Core\Inc\stm32f4xx_hal_conf.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_rcc.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_def.h \
+  ..\Drivers\CMSIS\Device\ST\STM32F4xx\Include\stm32f4xx.h \
+  ..\Drivers\CMSIS\Device\ST\STM32F4xx\Include\stm32f407xx.h \
+  ..\Drivers\CMSIS\Include\core_cm4.h \
+  D:\Keil_v5\ARM\ARMCLANG\Bin\..\include\stdint.h \
+  ..\Drivers\CMSIS\Include\cmsis_version.h \
+  ..\Drivers\CMSIS\Include\cmsis_compiler.h \
+  ..\Drivers\CMSIS\Include\cmsis_armclang.h \
+  ..\Drivers\CMSIS\Include\mpu_armv7.h \
+  ..\Drivers\CMSIS\Device\ST\STM32F4xx\Include\system_stm32f4xx.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\Legacy\stm32_hal_legacy.h \
+  D:\Keil_v5\ARM\ARMCLANG\Bin\..\include\stddef.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_rcc_ex.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_gpio.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_gpio_ex.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_exti.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_dma.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_dma_ex.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_cortex.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_can.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_flash.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_flash_ex.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_flash_ramfunc.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_i2c.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_i2c_ex.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_pwr.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_pwr_ex.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_spi.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_tim.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_tim_ex.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_uart.h \
+  D:\Keil_v5\ARM\ARMCLANG\Bin\..\include\stdbool.h ..\User\bsp\bsp.h \
+  ..\User\device\device.h

MDK-ARM/Steering Wheel_Infatry/vofa_1.d

@@ -0,0 +1,49 @@
+steering\ wheel_infatry/vofa_1.o: ..\User\task\vofa.c \
+  ..\User\task\user_task.h \
+  ..\Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.h \
+  D:\Keil_v5\ARM\ARMCLANG\Bin\..\include\stdint.h \
+  D:\Keil_v5\ARM\ARMCLANG\Bin\..\include\stddef.h \
+  ..\Middlewares\Third_Party\FreeRTOS\Source\include\FreeRTOS.h \
+  ..\Core\Inc\FreeRTOSConfig.h \
+  ..\Middlewares\Third_Party\FreeRTOS\Source\include\projdefs.h \
+  ..\Middlewares\Third_Party\FreeRTOS\Source\include\portable.h \
+  ..\Middlewares\Third_Party\FreeRTOS\Source\include\deprecated_definitions.h \
+  ..\Middlewares\Third_Party\FreeRTOS\Source\portable\RVDS\ARM_CM4F\portmacro.h \
+  ..\Middlewares\Third_Party\FreeRTOS\Source\include\mpu_wrappers.h \
+  ..\Middlewares\Third_Party\FreeRTOS\Source\include\task.h \
+  ..\Middlewares\Third_Party\FreeRTOS\Source\include\list.h \
+  ..\User\device\vofa.h ..\User\bsp\uart.h ..\Core\Inc\usart.h \
+  ..\Core\Inc\main.h ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal.h \
+  ..\Core\Inc\stm32f4xx_hal_conf.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_rcc.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_def.h \
+  ..\Drivers\CMSIS\Device\ST\STM32F4xx\Include\stm32f4xx.h \
+  ..\Drivers\CMSIS\Device\ST\STM32F4xx\Include\stm32f407xx.h \
+  ..\Drivers\CMSIS\Include\core_cm4.h \
+  ..\Drivers\CMSIS\Include\cmsis_version.h \
+  ..\Drivers\CMSIS\Include\cmsis_compiler.h \
+  ..\Drivers\CMSIS\Include\cmsis_armclang.h \
+  ..\Drivers\CMSIS\Include\mpu_armv7.h \
+  ..\Drivers\CMSIS\Device\ST\STM32F4xx\Include\system_stm32f4xx.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\Legacy\stm32_hal_legacy.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_rcc_ex.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_gpio.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_gpio_ex.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_exti.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_dma.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_dma_ex.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_cortex.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_can.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_flash.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_flash_ex.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_flash_ramfunc.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_i2c.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_i2c_ex.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_pwr.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_pwr_ex.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_spi.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_tim.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_tim_ex.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_uart.h \
+  D:\Keil_v5\ARM\ARMCLANG\Bin\..\include\stdbool.h ..\User\bsp\bsp.h \
+  ..\User\device\device.h

Steering Wheel_Infatry.ioc

@@ -75,10 +75,13 @@ Dma.USART6_TX.4.PeriphDataAlignment=DMA_PDATAALIGN_BYTE
 Dma.USART6_TX.4.PeriphInc=DMA_PINC_DISABLE
 Dma.USART6_TX.4.Priority=DMA_PRIORITY_LOW
 Dma.USART6_TX.4.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode
-FREERTOS.IPParameters=Tasks01,configTOTAL_HEAP_SIZE,configUSE_STATS_FORMATTING_FUNCTIONS,configGENERATE_RUN_TIME_STATS,configCHECK_FOR_STACK_OVERFLOW
+FREERTOS.HEAP_NUMBER=4
+FREERTOS.IPParameters=Tasks01,configTOTAL_HEAP_SIZE,configUSE_STATS_FORMATTING_FUNCTIONS,configGENERATE_RUN_TIME_STATS,configCHECK_FOR_STACK_OVERFLOW,HEAP_NUMBER,configTICK_RATE_HZ,configMINIMAL_STACK_SIZE
 FREERTOS.Tasks01=defaultTask,24,128,StartDefaultTask,Default,NULL,Dynamic,NULL,NULL
 FREERTOS.configCHECK_FOR_STACK_OVERFLOW=2
 FREERTOS.configGENERATE_RUN_TIME_STATS=1
+FREERTOS.configMINIMAL_STACK_SIZE=128
+FREERTOS.configTICK_RATE_HZ=1000
 FREERTOS.configTOTAL_HEAP_SIZE=0x19999
 FREERTOS.configUSE_STATS_FORMATTING_FUNCTIONS=1
 File.Version=6
@@ -122,32 +125,37 @@ Mcu.Pin21=PH1-OSC_OUT
 Mcu.Pin22=PF1
 Mcu.Pin23=PG6
 Mcu.Pin24=PF6
-Mcu.Pin25=PG3
+Mcu.Pin25=PH12
-Mcu.Pin26=PC1
+Mcu.Pin26=PG3
-Mcu.Pin27=PA0-WKUP
+Mcu.Pin27=PH11
-Mcu.Pin28=PA4
+Mcu.Pin28=PH10
-Mcu.Pin29=PC4
+Mcu.Pin29=PC1
 Mcu.Pin3=PB3
-Mcu.Pin30=PC5
+Mcu.Pin30=PC2
-Mcu.Pin31=PB12
+Mcu.Pin31=PB2
-Mcu.Pin32=PA7
+Mcu.Pin32=PA0-WKUP
-Mcu.Pin33=PB1
+Mcu.Pin33=PA4
-Mcu.Pin34=PB0
+Mcu.Pin34=PC4
-Mcu.Pin35=VP_FREERTOS_VS_CMSIS_V2
+Mcu.Pin35=PC5
-Mcu.Pin36=VP_SYS_VS_Systick
+Mcu.Pin36=PB12
-Mcu.Pin37=VP_TIM10_VS_ClockSourceINT
+Mcu.Pin37=PA7
+Mcu.Pin38=PB1
+Mcu.Pin39=PB0
 Mcu.Pin4=PA14
+Mcu.Pin40=VP_FREERTOS_VS_CMSIS_V2
+Mcu.Pin41=VP_SYS_VS_Systick
+Mcu.Pin42=VP_TIM10_VS_ClockSourceINT
 Mcu.Pin5=PA13
 Mcu.Pin6=PB9
 Mcu.Pin7=PB7
 Mcu.Pin8=PB6
 Mcu.Pin9=PD6
-Mcu.PinsNb=38
+Mcu.PinsNb=43
 Mcu.ThirdPartyNb=0
 Mcu.UserConstants=
 Mcu.UserName=STM32F407IGHx
-MxCube.Version=6.12.1
+MxCube.Version=6.16.1
-MxDb.Version=DB.6.0.121
+MxDb.Version=DB.6.0.161
 NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
 NVIC.CAN1_RX0_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
 NVIC.CAN1_RX1_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
@@ -177,6 +185,7 @@ NVIC.SavedSvcallIrqHandlerGenerated=true
 NVIC.SavedSystickIrqHandlerGenerated=true
 NVIC.SysTick_IRQn=true\:15\:0\:false\:false\:true\:true\:false\:true\:false
 NVIC.TIM1_UP_TIM10_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
+NVIC.USART1_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
 NVIC.USART3_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
 NVIC.USART6_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
 NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
@@ -210,13 +219,18 @@ PB0.Locked=true
 PB0.PinState=GPIO_PIN_SET
 PB0.Signal=GPIO_Output
 PB1.GPIOParameters=GPIO_PuPd,GPIO_Label
-PB1.GPIO_Label=Step_direction
+PB1.GPIO_Label=DIR_P
 PB1.GPIO_PuPd=GPIO_PULLUP
 PB1.Locked=true
 PB1.Signal=GPIO_Output
 PB12.Locked=true
 PB12.Mode=CAN_Activate
 PB12.Signal=CAN2_RX
+PB2.GPIOParameters=GPIO_PuPd,GPIO_Label
+PB2.GPIO_Label=DIR_N
+PB2.GPIO_PuPd=GPIO_PULLUP
+PB2.Locked=true
+PB2.Signal=GPIO_Output
 PB3.Mode=Full_Duplex_Master
 PB3.Signal=SPI1_SCK
 PB4.Mode=Full_Duplex_Master
@@ -229,10 +243,11 @@ PB8.Mode=I2C
 PB8.Signal=I2C1_SCL
 PB9.Mode=I2C
 PB9.Signal=I2C1_SDA
-PC1.GPIOParameters=GPIO_PuPd,GPIO_Label
+PC1.GPIOParameters=PinState,GPIO_PuPd,GPIO_Label
-PC1.GPIO_Label=step
+PC1.GPIO_Label=PUL_P
 PC1.GPIO_PuPd=GPIO_PULLUP
 PC1.Locked=true
+PC1.PinState=GPIO_PIN_SET
 PC1.Signal=GPIO_Output
 PC10.Mode=Asynchronous
 PC10.Signal=USART3_TX
@@ -242,6 +257,11 @@ PC14-OSC32_IN.Mode=LSE-External-Oscillator
 PC14-OSC32_IN.Signal=RCC_OSC32_IN
 PC15-OSC32_OUT.Mode=LSE-External-Oscillator
 PC15-OSC32_OUT.Signal=RCC_OSC32_OUT
+PC2.GPIOParameters=GPIO_PuPd,GPIO_Label
+PC2.GPIO_Label=PUL_N
+PC2.GPIO_PuPd=GPIO_PULLUP
+PC2.Locked=true
+PC2.Signal=GPIO_Output
 PC4.GPIOParameters=GPIO_PuPd,GPIO_Label,GPIO_ModeDefaultEXTI
 PC4.GPIO_Label=ACCL_INT
 PC4.GPIO_ModeDefaultEXTI=GPIO_MODE_IT_FALLING
@@ -296,6 +316,21 @@ PH0-OSC_IN.Mode=HSE-External-Oscillator
 PH0-OSC_IN.Signal=RCC_OSC_IN
 PH1-OSC_OUT.Mode=HSE-External-Oscillator
 PH1-OSC_OUT.Signal=RCC_OSC_OUT
+PH10.GPIOParameters=GPIO_PuPd,GPIO_Label
+PH10.GPIO_Label=LED_Blue
+PH10.GPIO_PuPd=GPIO_PULLUP
+PH10.Locked=true
+PH10.Signal=GPIO_Output
+PH11.GPIOParameters=GPIO_PuPd,GPIO_Label
+PH11.GPIO_Label=LED_Green
+PH11.GPIO_PuPd=GPIO_PULLUP
+PH11.Locked=true
+PH11.Signal=GPIO_Output
+PH12.GPIOParameters=GPIO_PuPd,GPIO_Label
+PH12.GPIO_Label=LED_Red
+PH12.GPIO_PuPd=GPIO_PULLUP
+PH12.Locked=true
+PH12.Signal=GPIO_Output
 PinOutPanel.CurrentBGAView=Top
 PinOutPanel.RotationAngle=0
 ProjectManager.AskForMigrate=true

User/bsp/bsp_config.yaml

@@ -24,16 +24,26 @@ gpio:
     ioc_label: GYRO_CS
     pin: PB0
     type: OUTPUT
-  - custom_name: STEP__DIRECTION
+  - custom_name: DIR_P
     has_exti: false
-    ioc_label: Step_direction
+    ioc_label: DIR_P
     pin: PB1
     type: OUTPUT
-  - custom_name: STEP_MOTOR
+  - custom_name: DIR_N
     has_exti: false
-    ioc_label: step
+    ioc_label: DIR_N
+    pin: PB2
+    type: OUTPUT
+  - custom_name: PUL_P
+    has_exti: false
+    ioc_label: PUL_P
     pin: PC1
     type: OUTPUT
+  - custom_name: PUL_N
+    has_exti: false
+    ioc_label: PUL_N
+    pin: PC2
+    type: OUTPUT
   - custom_name: ACCL_INT
     has_exti: true
     ioc_label: ACCL_INT
@@ -54,6 +64,21 @@ gpio:
     ioc_label: CMPS_RST
     pin: PG6
     type: OUTPUT
+  - custom_name: LED_BLUE
+    has_exti: false
+    ioc_label: LED_Blue
+    pin: PH10
+    type: OUTPUT
+  - custom_name: LED_GREEN
+    has_exti: false
+    ioc_label: LED_Green
+    pin: PH11
+    type: OUTPUT
+  - custom_name: LED_RED
+    has_exti: false
+    ioc_label: LED_Red
+    pin: PH12
+    type: OUTPUT
   enabled: true
 i2c:
   devices:

User/bsp/gpio.c

@@ -29,12 +29,17 @@ static const BSP_GPIO_MAP_t GPIO_Map[BSP_GPIO_NUM] = {
     {USER_KEY_Pin, USER_KEY_GPIO_Port},
     {ACCL_CS_Pin, ACCL_CS_GPIO_Port},
     {GYRO_CS_Pin, GYRO_CS_GPIO_Port},
-    {Step_direction_Pin, Step_direction_GPIO_Port},
+    {DIR_P_Pin, DIR_P_GPIO_Port},
-    {step_Pin, step_GPIO_Port},
+    {DIR_N_Pin, DIR_N_GPIO_Port},
+    {PUL_P_Pin, PUL_P_GPIO_Port},
+    {PUL_N_Pin, PUL_N_GPIO_Port},
     {ACCL_INT_Pin, ACCL_INT_GPIO_Port},
     {GYRO_INT_Pin, GYRO_INT_GPIO_Port},
     {CMPS_INT_Pin, CMPS_INT_GPIO_Port},
     {CMPS_RST_Pin, CMPS_RST_GPIO_Port},
+    {LED_Blue_Pin, LED_Blue_GPIO_Port},
+    {LED_Green_Pin, LED_Green_GPIO_Port},
+    {LED_Red_Pin, LED_Red_GPIO_Port},
 };
 
 static void (*GPIO_Callback[16])(void);

User/bsp/gpio.h

@@ -25,12 +25,17 @@ typedef enum {
   BSP_GPIO_USER_KEY,
   BSP_GPIO_ACCL_CS,
   BSP_GPIO_GYRO_CS,
-  BSP_GPIO_STEP__DIRECTION,
+  BSP_GPIO_DIR_P,
-  BSP_GPIO_STEP_MOTOR,
+  BSP_GPIO_DIR_N,
+  BSP_GPIO_PUL_P,
+  BSP_GPIO_PUL_N,
   BSP_GPIO_ACCL_INT,
   BSP_GPIO_GYRO_INT,
   BSP_GPIO_CMPS_INT,
   BSP_GPIO_CMPS_RST,
+  BSP_GPIO_LED_BLUE,
+  BSP_GPIO_LED_GREEN,
+  BSP_GPIO_LED_RED,
   BSP_GPIO_NUM,
   BSP_GPIO_ERR,
 } BSP_GPIO_t;

User/device/ET16s.c

@@ -165,18 +165,23 @@ int8_t ET16S_ParseRC(ET16s_t *et16s) {
   et16s->ET16s.key_B = Keymap(et16s->raw_data.sw[1]);
   et16s->ET16s.key_C = Keymap(et16s->raw_data.sw[2]);
   et16s->ET16s.key_D = Keymap(et16s->raw_data.sw[3]);
-  et16s->ET16s.key_E = Keymap(et16s->raw_data.sw[4]);
+	et16s->ET16s.key_E = Keymap(et16s->raw_data.sw[4]);
   et16s->ET16s.key_F = Keymap(et16s->raw_data.sw[5]);
   et16s->ET16s.key_G = Keymap(et16s->raw_data.sw[6]);	
   et16s->ET16s.key_H = Keymap(et16s->raw_data.sw[7]);
   // rc->ET16s.knob_left = ET16s->sw[7]; //200 330 479 629 778 928 1075 1224 1373 1522 1670 1800
-
+	/* 通道5出了bug，只能这样解决 */
-  /*离线处理，和dr16位置不同*/
+//	switch(et16s->raw_data.sw[4]){
-	if(et16s->raw_data.sw[6]==1695)
+//		case 353:
-	{
+//			et16s->ET16s.key_E=ET16S_SW_UP;
-//		ET16s_HandleOffline(et16s);
+//			break;
-//    memset(cbuf, 0, sizeof(cbuf));	//有时候会出现消息数组错位，所以直接清空，在离线和指定按键不对的情况下，原数据不可信	
+//		case 1024:
-	}
+//			et16s->ET16s.key_E=ET16S_SW_MID;
+//			break;	
+//		case 1695:
+//			et16s->ET16s.key_E=ET16S_SW_DOWN;
+//			break;		
+//	}
 #endif
   return DEVICE_OK;
 }

User/device/ET16s.h

@@ -57,26 +57,7 @@ typedef struct
   int16_t sw[8]; 
 } ET16s_raw_t;
 
-// 遥控器数据
+typedef struct{
-typedef struct
-{
-
-    rc_type_t rc_type;
-
-//    struct
-//    {
-//        float ch_l_x; /* 遥控器左侧摇杆横轴值，上为正 */
-//        float ch_l_y; /* 遥控器左侧摇杆纵轴值，右为正 */
-//        float ch_r_x; /* 遥控器右侧摇杆横轴值，上为正 */
-//        float ch_r_y; /* 遥控器右侧摇杆纵轴值，右为正 */
-
-//        ET16S_SwitchPos_t sw_r; /* 右侧拨杆位置 */
-//        ET16S_SwitchPos_t sw_l; /* 左侧拨杆位置 */
-
-//    } __attribute__((packed)) DJ;
-
-    struct
-    {
         float ch_l_x; /* 遥控器左侧摇杆横轴值，上为正 */
         float ch_l_y; /* 遥控器左侧摇杆纵轴值，右为正 */
         float ch_r_x; /* 遥控器右侧摇杆横轴值，上为正 */
@@ -94,7 +75,11 @@ typedef struct
         int16_t knob_left;  // 左旋钮
         int16_t knob_right; // 右旋钮
     } __attribute__((packed)) ET16s;
-		
+// 遥控器数据
+typedef struct
+{
+     rc_type_t rc_type;
+			ET16s ET16s;
 			ET16s_raw_t raw_data;
 		  DEVICE_Header_t header;
 } __attribute__((packed)) ET16s_t;

User/device/device_config.yaml

@@ -16,6 +16,9 @@ ist8310:
     CMPS_INT_Pin: BSP_GPIO_CMPS_INT
     CMPS_RST_Pin: BSP_GPIO_CMPS_RST
   enabled: true
+led:
+  bsp_config: {}
+  enabled: true
 motor:
   bsp_config: {}
   enabled: true
@@ -31,3 +34,7 @@ motor_lz:
 motor_rm:
   bsp_config: {}
   enabled: true
+vofa:
+  bsp_config:
+    BSP_UART_VOFA: BSP_UART_1
+  enabled: true

User/device/led.c

@@ -0,0 +1,37 @@
+/*
+   led控制
+*/
+/*Includes   -----------------------------------------*/
+#include "device/led.h"
+#include "device.h"
+
+
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+#ifdef LED_PWM
+int8_t DEVICE_LED_PWM_Set(BSP_PWM_Channel_t channel, float duty_cycle)
+{
+    if (duty_cycle < 0.0f || duty_cycle > 1.0f) {
+        return DEVICE_ERR_NULL; // 错误：占空比超出范围
+    }
+    uint16_t pulse = (uint16_t)(duty_cycle * (float)UINT16_MAX);
+    BSP_PWM_Start(channel);
+    BSP_PWM_SetComp(channel, pulse);
+    return DEVICE_OK;
+}
+#endif
+
+#ifdef LED_GPIO
+int8_t DEVICE_LED_GPIO_Set(BSP_GPIO_t gpio, bool value)
+{
+	if (value) {
+		BSP_GPIO_WritePin(gpio, true);
+	} else {
+		BSP_GPIO_WritePin(gpio, false);
+	}
+	return DEVICE_OK;
+}
+#endif
+
+

User/device/led.h

@@ -0,0 +1,63 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+/* USER DEFIN BEGIN */
+
+/* USER DEFIN END */
+#include <stdint.h>
+#ifdef LED_GPIO
+#include "bsp/gpio.h"
+#endif
+#ifdef LED_PWM
+#include "bsp/pwm.h"
+#endif
+#include "bsp/bsp.h"
+/* Exported constants ------------------------------------------------------- */
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+
+
+typedef struct {
+#ifdef LED_GPIO
+BSP_GPIO_t gpio;
+#endif
+#ifdef LED_PWM
+BSP_PWM_Channel_t channel;	
+#endif
+} DEVICE_LED_t;
+
+
+ extern DEVICE_LED_t LED_Map;
+/* Exported functions prototypes -------------------------------------------- */
+#ifdef LED_PWM
+int8_t DEVICE_LED_PWM_Set(BSP_PWM_Channel_t channel, float duty_cycle)
+{
+    if (duty_cycle < 0.0f || duty_cycle > 1.0f) {
+        return DEVICE_ERR_NULL; // 错误：占空比超出范围
+    }
+    uint16_t pulse = (uint16_t)(duty_cycle * (float)UINT16_MAX);
+    BSP_PWM_Start(channel);
+    BSP_PWM_SetComp(channel, pulse);
+    return DEVICE_OK;
+}
+#endif
+
+#ifdef LED_GPIO
+int8_t DEVICE_LED_GPIO_Set(BSP_GPIO_t gpio, bool value)
+{
+	if (value) {
+		BSP_GPIO_WritePin(gpio, true);
+	} else {
+		BSP_GPIO_WritePin(gpio, false);
+	}
+	return DEVICE_OK;
+}
+#endif
+
+#ifdef __cplusplus
+}
+#endif

User/device/step_motor.c

@@ -0,0 +1,32 @@
+/* 底盘固定模组,用步进 */
+#include "main.h"
+#include "step_motor.h"
+#include "bsp/gpio.h"
+#include "cmsis_os2.h"     
+
+
+
+int8_t Step_Motor_Ctrl(STEP_MOTOR *param){
+
+//	if(param->state==1){
+	/* 控制方向 */
+	BSP_GPIO_WritePin(BSP_GPIO_DIR_P, param->direction);
+	osDelay(10); // 方向稳定时间
+//	for(int i;i >= param->pulse;i++){
+	HAL_GPIO_WritePin(GPIOC, GPIO_PIN_1, 1);
+	HAL_GPIO_WritePin(GPIOC, GPIO_PIN_2, 0);	
+//	BSP_GPIO_WritePin(BSP_GPIO_PUL_P,true);
+//	BSP_GPIO_WritePin(BSP_GPIO_PUL_N,false);		
+	osDelay(param->time);
+	HAL_GPIO_WritePin(GPIOC, GPIO_PIN_1, 1);
+	HAL_GPIO_WritePin(GPIOC, GPIO_PIN_2, 0);		
+//	BSP_GPIO_WritePin(BSP_GPIO_PUL_P,true);	
+//	BSP_GPIO_WritePin(BSP_GPIO_PUL_N,false);			
+	osDelay(param->time);
+//	}
+//	param->state=1;
+//}
+	return 0;
+}
+
+

User/device/step_motor.h

@@ -1,5 +1,5 @@
 /*
- * 云台模组
+* 步进电机
  */
 
 #pragma once
@@ -9,16 +9,27 @@ extern "C" {
 #endif
 
 /* Includes ----------------------------------------------------------------- */
-
+#include "cmsis_os2.h"   
-
+#include <stdbool.h>  
 /* Exported constants ------------------------------------------------------- */
 
 
 /* Exported macro ----------------------------------------------------------- */
 /* Exported types ----------------------------------------------------------- */
+typedef struct{
 
+	/* 脉冲个数 */
+	int pulse;
+	/* 脉冲间隔 */
+	float time;
 
-
+	/* 方向 */
+	bool direction;
+	/* 状态 */
+	int state;
+		
+}STEP_MOTOR;
+int8_t Step_Motor_Ctrl(STEP_MOTOR *param);
 #ifdef __cplusplus
 }
 #endif

User/device/vofa.c

@@ -0,0 +1,106 @@
+/* Includes ----------------------------------------------------------------- */
+#include <stdio.h>
+#include <string.h>
+#include "device/vofa.h"
+#include "bsp/uart.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* Private define ----------------------------------------------------------- */
+
+#define MAX_CHANNEL  64u   		// 根据实际最大通道数调整
+
+#define JUSTFLOAT_TAIL 0x7F800000
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+/* USER STRUCT BEGIN */
+
+/* USER STRUCT END */
+
+/* Private variables -------------------------------------------------------- */
+static uint8_t vofa_tx_buf[sizeof(float) * MAX_CHANNEL + sizeof(uint32_t)];
+static VOFA_Protocol_t current_protocol = VOFA_PROTOCOL_FIREWATER;  // 默认协议
+
+/* Private function  -------------------------------------------------------- */
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
+
+/************************  RawData  *************************/
+void VOFA_RawData_Send(const char* data, bool dma) {
+    BSP_UART_Transmit(BSP_UART_1, (uint8_t*)data, strlen(data), dma);
+}
+
+/************************ FireWater *************************/
+void VOFA_FireWater_Send(float *channels, uint8_t channel_count, bool dma)
+{
+    if (channel_count == 0 || channel_count > MAX_CHANNEL)
+        return;
+
+    char *buf = (char *)vofa_tx_buf;
+    size_t len = 0;
+
+    for (uint8_t i = 0; i < channel_count; ++i) {
+        len += snprintf(buf + len,
+                        sizeof(vofa_tx_buf) - len,
+                        "%s%.2f",
+                        (i ? "," : ""),
+                        channels[i]);
+    }
+    snprintf(buf + len, sizeof(vofa_tx_buf) - len, "\n");
+
+    BSP_UART_Transmit(BSP_UART_1, vofa_tx_buf, strlen(buf), dma);
+}
+
+/************************ JustFloat *************************/
+void VOFA_JustFloat_Send(float *channels, uint8_t channel_count, bool dma)
+{
+    if (channel_count == 0 || channel_count > MAX_CHANNEL)
+        return;
+    memcpy(vofa_tx_buf, channels, channel_count * sizeof(float));
+
+    uint32_t tail = JUSTFLOAT_TAIL;                     // 0x7F800000
+    memcpy(vofa_tx_buf + channel_count * sizeof(float), &tail, sizeof(tail));
+
+    BSP_UART_Transmit(BSP_UART_1, vofa_tx_buf, channel_count * sizeof(float) + sizeof(tail), dma);
+}
+
+/* Exported functions ------------------------------------------------------- */
+int8_t VOFA_init(VOFA_Protocol_t protocol) {
+    current_protocol = protocol;
+    return DEVICE_OK;
+}
+
+int8_t VOFA_Send(float* channels, uint8_t channel_count, bool dma) {
+    switch (current_protocol) {
+        case VOFA_PROTOCOL_RAWDATA:
+            {
+                char data[256];
+                if (channel_count >= 1) {
+                    sprintf(data, "Channel1: %.2f", channels[0]);
+                    if (channel_count >= 2) {
+                        sprintf(data + strlen(data), ", Channel2: %.2f", channels[1]);
+                    }
+                    strcat(data, "\n");
+                    VOFA_RawData_Send(data, dma);
+                }
+            }
+            break;
+        case VOFA_PROTOCOL_FIREWATER:
+            VOFA_FireWater_Send(channels, channel_count, dma);
+            break;
+        case VOFA_PROTOCOL_JUSTFLOAT:
+            VOFA_JustFloat_Send(channels, channel_count, dma);
+            break;
+        default:
+            return DEVICE_ERR;
+    }
+    return DEVICE_OK;
+}

User/device/vofa.h

@@ -0,0 +1,39 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include "bsp/uart.h"
+#include "device/device.h"
+/* Exported constants ------------------------------------------------------- */
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+/* Exported functions prototypes -------------------------------------------- */
+
+typedef enum {
+    VOFA_PROTOCOL_RAWDATA,
+    VOFA_PROTOCOL_FIREWATER,
+    VOFA_PROTOCOL_JUSTFLOAT,
+} VOFA_Protocol_t;
+
+/**
+ * @brief 初始化VOFA设备
+ * @param protocol 设置通信协议
+ * @return 
+ */
+int8_t VOFA_init(VOFA_Protocol_t protocol);
+
+/**
+ * @brief 发送数据到VOFA
+ * @param channels 要发送的通道数据
+ * @param channel_count 通道数量
+ * @param dma 是否使用DMA发送
+ * @return 
+ */
+int8_t VOFA_Send(float* channels, uint8_t channel_count, bool dma);
+
+#ifdef __cplusplus
+}
+#endif

User/module/chassis.c

@@ -19,7 +19,7 @@ chassis_init(&chassis,&Config_GetRobotParam()->chassis,CHASSIS_FREQ);
     if(osMessageQueueGet(task_runtime.msgq.cmd.chassis, &cmd_chassis, NULL, 0)==osOK)
     {
       Chassis_update(&chassis);
-      Chassis_Control(&chassis, &cmd_chassis);
+      Chassis_Control(&chassis, &cmd_chassis,tick);
     }else
 {
 	 Chassis_CMD_t safe_cmd = {.mode = STOP, .Vx = 0, .Vy = 0, .Vw = 0};
@@ -41,6 +41,8 @@ chassis_init(&chassis,&Config_GetRobotParam()->chassis,CHASSIS_FREQ);
 #include "component/pid.h"
 #include "component/filter.h"
 
+#include "stdlib.h"
+
 /*舵轮舵向校准方法：注释掉关于6020反馈角度的处理以及6020数据的发送这两处(define.h里有快捷方法)，
 进debug将四个轮子编码器朝右（左右无所谓，可能会导致5065方向反，在解算里加个负号就行）
 查看6020反馈值，将6020反馈值放入motor_offset中*/
@@ -89,7 +91,7 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_mode_t mode ,uint32_t now)
  *
  * @param min wz产生最小速度
  * @param max wz产生最大速度
- * @param c->last_wakeup ctrl_chassis的tick数
+ * @param now ctrl_chassis的tick数
  * @return float
  */
 static float Chassis_CalcWz(const float min, const float max, uint32_t now) {
@@ -110,6 +112,8 @@ int8_t chassis_init(Chassis_t *c, Chassis_Param_t *param, float target_freq)
   c->mode = CHASSIS_MODE_RELAX; // 默认模式为停止锁死底盘
   c->mech_zero = c->param->mech_zero;/*云台6020的机械中点*/
   c->travel = c->param->travel ;/*云台6020的机械行程*/
+  c->chassis2006_setangle = c->param->chassis2006_setangle; // 变形舵轮2006目标角度  大概是48圈
+  c->chassis3508_setangle = c->param->chassis3508_setangle; // 变形舵轮3508目标角度  大概是60度左右
   /*初始化can*/
   BSP_CAN_Init();
   /*注册3508电机*/
@@ -122,6 +126,7 @@ int8_t chassis_init(Chassis_t *c, Chassis_Param_t *param, float target_freq)
   {
     MOTOR_RM_Register(&(c->param->motor_6020_param[i]));
   }
+ MOTOR_RM_Register(&(c->param->motor_2006_param)); // 注册2006电机
 
   // 舵轮安装时的6020机械误差，机械校准时1号轮在左前方，所有轮的编码器朝向右面
   MotorOffset_t motor_offset = {{2.09311676 / M_PI * 180.0f, 0.50467968 / M_PI * 180.0f, 3.63706851 / M_PI * 180.0f, 5.26232147 / M_PI * 180.0f}}; // 右右右右
@@ -138,6 +143,15 @@ int8_t chassis_init(Chassis_t *c, Chassis_Param_t *param, float target_freq)
     PID_Init(&c->pid.chassis_6020anglePid[i], KPID_MODE_CALC_D, target_freq, &c->param->C6020Angle_param);
   }
   PID_Init(&c->pid.chassis_follow_gimbal_pid, KPID_MODE_CALC_D, target_freq, &c->param->chassis_follow_gimbalPID); 
+  /*2006位置环pid*/
+  PID_Init(&c->pid.chassis_2006_anglepid,KPID_MODE_NO_D,target_freq , &c->param->chassis_2006_angle_param);
+  PID_Init(&c->pid.chassis_2006_OmegaPid,KPID_MODE_CALC_D,target_freq , &c->param->chassis_2006_Omega_param);
+/*底盘轮向3508在控制展开时的位置双环pid*/
+  for(int i=0;i<4;i++)
+  {
+    PID_Init(&c->pid.chassis_3508_anglepid[i],KPID_MODE_NO_D,target_freq , &c->param->chassis_3508_angle_param);
+    PID_Init(&c->pid.chassis_3508_OmegaPid[i],KPID_MODE_CALC_D,target_freq , &c->param->chassis_3508_Omega_param);
+  }
 
   LowPassFilter2p_Init(&c->filled[0], target_freq, 20.0f); // vx
   LowPassFilter2p_Init(&c->filled[1], target_freq, 20.0f); // vy
@@ -153,6 +167,8 @@ int8_t chassis_init(Chassis_t *c, Chassis_Param_t *param, float target_freq)
   LowPassFilter2p_Init(&c->filled[9], target_freq, 20.0f); // 6020-3
   LowPassFilter2p_Init(&c->filled[10], target_freq, 20.0f); // 6020-4 
 
+  LowPassFilter2p_Init(&c->filled[11], target_freq, 10.0f); // 2006角度
+
   
   return CHASSIS_OK;
 }
@@ -245,9 +261,30 @@ void Chassis_speed_calculate(Chassis_t *c, Chassis_CMD_t *c_cmd)
 
       break;
 
+      case HUIHANG_BIANXING_DUOLUN:
+      c->hopemotorout.rotor6020_jiesuan_1[0] = 90;
+      c->hopemotorout.rotor6020_jiesuan_1[1] = 90;
+      c->hopemotorout.rotor6020_jiesuan_1[2] = 270;
+      c->hopemotorout.rotor6020_jiesuan_1[3] = 270;
   
-    }
+      for (int i = 0; i < 4; i++)
+      {
+        c->hopemotorout.rotor3508_jiesuan_1[i] = 0.0f;
+      }
+      float motor_2006omega = PID_Calc(&c->pid.chassis_2006_anglepid,c->chassis2006_setangle,c->motorfeedback.motor2006_angle,0.0f,c->dt);
+      c->final_out.final_2006out=PID_Calc(&c->pid.chassis_2006_OmegaPid,motor_2006omega,c->motorfeedback.motor2006_rpm,0.0f,c->dt);
+
+      // c->hopemotorout.rotor3508_jiesuan_1[0] = PID_Calc(&c->pid.chassis_3508_anglepid[0],c->chassis3508_setangle,c->motorfeedback.rotor_angle3508[0],0.0f,c->dt);
+      // c->hopemotorout.rotor3508_jiesuan_1[1] = PID_Calc(&c->pid.chassis_3508_anglepid[1],c->chassis3508_setangle,c->motorfeedback.rotor_angle3508[1],0.0f,c->dt);
+      // c->hopemotorout.rotor3508_jiesuan_1[2] = PID_Calc(&c->pid.chassis_3508_anglepid[2],-c->chassis3508_setangle,c->motorfeedback.rotor_angle3508[2],0.0f,c->dt);
+      // c->hopemotorout.rotor3508_jiesuan_1[3] = PID_Calc(&c->pid.chassis_3508_anglepid[3],-c->chassis3508_setangle,c->motorfeedback.rotor_angle3508[3],0.0f,c->dt);
+
+      // c->out.rotor3508_out[0]=PID_Calc(&c->pid.chassis_3508_OmegaPid[0],c->hopemotorout.rotor3508_jiesuan_1[0],c->motorfeedback.rotor_rpm3508[0],0.0f,c->dt);
+      // c->out.rotor3508_out[1]=PID_Calc(&c->pid.chassis_3508_OmegaPid[1],c->hopemotorout.rotor3508_jiesuan_1[1],c->motorfeedback.rotor_rpm3508[1],0.0f,c->dt);
+      // c->out.rotor3508_out[2]=PID_Calc(&c->pid.chassis_3508_OmegaPid[2],c->hopemotorout.rotor3508_jiesuan_1[2],c->motorfeedback.rotor_rpm3508[2],0.0f,c->dt);
+      // c->out.rotor3508_out[3]=PID_Calc(&c->pid.chassis_3508_OmegaPid[3],c->hopemotorout.rotor3508_jiesuan_1[3],c->motorfeedback.rotor_rpm3508[3],0.0f,c->dt);
   }
+}
   // 角度归化到0°——360°
 
   for (uint8_t i = 0; i < 4; i++)
@@ -302,21 +339,24 @@ int8_t Chassis_update(Chassis_t *c)
   MOTOR_RM_UpdateAll();
 
   /*更新电机反馈*/
-  for (int i = 0; i < 4; i++)
+  // for (int i = 0; i < 4; i++)
-  {
+  // {
-    // c->motorfeedback.rotor_angle6020[i] = (MOTOR_GetRotorAbsAngle(&(MOTOR_RM_GetMotor(&c->param->motor_6020_param[i])->motor)) );
+  //   // c->motorfeedback.rotor_angle6020[i] = (MOTOR_GetRotorAbsAngle(&(MOTOR_RM_GetMotor(&c->param->motor_6020_param[i])->motor)) );
-    c->motorfeedback.rotor_angle6020[i] = (MOTOR_GetRotorAbsAngle(&(MOTOR_RM_GetMotor(&c->param->motor_6020_param[i])->motor)) / M_PI * 180.0f) ;
+  //   c->motorfeedback.rotor_angle6020[i] = (MOTOR_GetRotorAbsAngle(&(MOTOR_RM_GetMotor(&c->param->motor_6020_param[i])->motor)) / M_PI * 180.0f) ;
-    c->motorfeedback.rotor_rpm6020[i] = (MOTOR_GetRotorSpeed(&(MOTOR_RM_GetMotor(&c->param->motor_6020_param[i])->motor)) / 320);
+  //   c->motorfeedback.rotor_rpm6020[i] = (MOTOR_GetRotorSpeed(&(MOTOR_RM_GetMotor(&c->param->motor_6020_param[i])->motor)) / 320);
-    c->motorfeedback.rotor_rpm3508[i] = MOTOR_GetRotorSpeed(&(MOTOR_RM_GetMotor(&c->param->motor_3508_param[i])->motor)) /10000;
+  //   c->motorfeedback.rotor_rpm3508[i] = MOTOR_GetRotorSpeed(&(MOTOR_RM_GetMotor(&c->param->motor_3508_param[i])->motor)) /10000;
-    //      c->motorfeedback.rotor_angle6020[i] = MOTOR_GetRotorAbsAngle(&(MOTOR_RM_GetMotor(&c->param->motor_6020_param[i])->motor));
+  //   c->motorfeedback.rotor_angle3508[i] = MOTOR_GetRotorAbsAngle(&(MOTOR_RM_GetMotor(&c->param->motor_3508_param[i])->motor)) ;
+  //   //      c->motorfeedback.rotor_angle6020[i] = MOTOR_GetRotorAbsAngle(&(MOTOR_RM_GetMotor(&c->param->motor_6020_param[i])->motor));
 
-    c->motorfeedback.rotor_angle6020[i] = fmod(c->motorfeedback.rotor_angle6020[i] - c->motoroffset.MOTOR_OFFSET[i], 360.0);
+  //   // 角度归化到0°——360°并且进行偏移校准
-    if (c->motorfeedback.rotor_angle6020[i] < 0)
+  //   c->motorfeedback.rotor_angle6020[i] = fmod(c->motorfeedback.rotor_angle6020[i] - c->motoroffset.MOTOR_OFFSET[i], 360.0);
-    {
+  //   if (c->motorfeedback.rotor_angle6020[i] < 0)
-      c->motorfeedback.rotor_angle6020[i] += 360;
+  //   {
-    }
+  //     c->motorfeedback.rotor_angle6020[i] += 360;
-    
+  //   }
-  }
+  // }
+    c->motorfeedback.motor2006_angle = MOTOR_GetRotorAbsAngle(&(MOTOR_RM_GetMotor(&c->param->motor_2006_param)->motor)) ;
+    c->motorfeedback.motor2006_rpm = MOTOR_GetRotorSpeed(&(MOTOR_RM_GetMotor(&c->param->motor_2006_param)->motor)) ;
 
   return CHASSIS_OK;
 }
@@ -327,7 +367,7 @@ void ChassisrolPrevent(Chassis_t *c)
 {
 }
 
-int8_t Chassis_Control(Chassis_t *c, Chassis_CMD_t *c_cmd)
+int8_t Chassis_Control(Chassis_t *c, Chassis_CMD_t *c_cmd,uint32_t now)
 {
 
   if (c == NULL || c_cmd == NULL)
@@ -335,13 +375,13 @@ int8_t Chassis_Control(Chassis_t *c, Chassis_CMD_t *c_cmd)
     return CHASSIS_ERR_NULL; // 参数错误
   }
 
-   c->dt = (BSP_TIME_Get_us() - c->last_wakeup) / 1000.0f; /* 计算两次调用的时间间隔，单位秒 */
+  // c->dt = (BSP_TIME_Get_us() - c->last_wakeup) / 1000000.0f; /* 计算两次调用的时间间隔，单位秒 */
-   c->last_wakeup = BSP_TIME_Get_us();
+  // c->last_wakeup = BSP_TIME_Get_us();
-//  c->dt = (float)(c->last_wakeup - c->last_wakeup) / 1000.0f; /* 计算两次调用的时间间隔，单位秒 */
+  c->dt = (float)(now - c->last_wakeup) / 1000.0f; /* 计算两次调用的时间间隔，单位秒 */
-//  c->last_wakeup = c->last_wakeup;
+  c->last_wakeup = now;
 
   /*设置底盘模式*/
-  if (Chassis_SetMode(c, c_cmd->mode,c->last_wakeup) != CHASSIS_OK)
+  if (Chassis_SetMode(c, c_cmd->mode,now) != CHASSIS_OK)
   {
     return CHASSIS_ERR_MODE; /* 设置模式失败 */
   }
@@ -373,7 +413,7 @@ int8_t Chassis_Control(Chassis_t *c, Chassis_CMD_t *c_cmd)
     // 小陀螺模式
     c->move_vec.Vx =c_cmd->y_l;
     c->move_vec.Vy =c_cmd->x_l;
-    c->move_vec.Vw =c->wz_multi* Chassis_CalcWz(CHASSIS_ROTOR_WZ_MIN,CHASSIS_ROTOR_WZ_MAX, c->last_wakeup);
+    c->move_vec.Vw =c->wz_multi* Chassis_CalcWz(CHASSIS_ROTOR_WZ_MIN,CHASSIS_ROTOR_WZ_MAX, now);
     break;
 
     case CHASSIS_MODE_FOLLOW_GIMBAL:
@@ -381,9 +421,14 @@ int8_t Chassis_Control(Chassis_t *c, Chassis_CMD_t *c_cmd)
     c->move_vec.Vx =c_cmd->y_l;
     c->move_vec.Vy =c_cmd->x_l;
     c->move_vec.Vw = -PID_Calc(&c->pid.chassis_follow_gimbal_pid, 2.0f ,c->motorfeedback.gimbal_yaw_encoder, 0.0f, c->dt);
-
     break;
 
+    case HUIHANG_BIANXING_DUOLUN:
+    // 会航变形舵轮模式
+    c->move_vec.Vx = 0.0f;
+    c->move_vec.Vy = 0.0f;
+    c->move_vec.Vw = 0.0f;
+    break;
 
 
   default:
@@ -413,6 +458,8 @@ int8_t Chassis_Control(Chassis_t *c, Chassis_CMD_t *c_cmd)
     c->final_out.final_3508out[i] = PID_Calc(&(c->pid.chassis_3508VPID[i]), c->hopemotorout.motor3508_target[i],
                                              c->motorfeedback.rotor_rpm3508[i], 0.0f, c->dt);
     c->out.rotor3508_out[i] = LowPassFilter2p_Apply(&c->filled[3+i], c->final_out.final_3508out[i]);
+
+    c->out.motor2006_out=c->final_out.final_2006out;
   }
   return CHASSIS_OK;
 }
@@ -420,13 +467,15 @@ int8_t Chassis_Control(Chassis_t *c, Chassis_CMD_t *c_cmd)
 /*电机输出设定和发送*/
 void Chassis_Setoutput(Chassis_t *c)
 {
-  for (int i = 0; i < 4; i++)
+  // for (int i = 0; i < 4; i++)
-  {
+  // {
-    MOTOR_RM_SetOutput(&(c->param->motor_3508_param[i]), c->out.rotor3508_out[i]);
+  //   MOTOR_RM_SetOutput(&(c->param->motor_3508_param[i]), c->out.rotor3508_out[i]);
-    MOTOR_RM_SetOutput(&(c->param->motor_6020_param[i]), c->out.rotor6020_out[i]);
+  //   MOTOR_RM_SetOutput(&(c->param->motor_6020_param[i]), c->out.rotor6020_out[i]);
-  }
+  // }
+  MOTOR_RM_SetOutput(&(c->param->motor_2006_param), c->out.motor2006_out);
   // MOTOR_RM_SetOutput(&(c->param->motor_3508_param[0]), 1.0f);
-  MOTOR_RM_Ctrl(&(c->param->motor_3508_param[0]));
+  // MOTOR_RM_Ctrl(&(c->param->motor_3508_param[0]));
-  MOTOR_RM_Ctrl(&(c->param->motor_6020_param[0]));
+  // MOTOR_RM_Ctrl(&(c->param->motor_6020_param[0]));
+  MOTOR_RM_Ctrl(&(c->param->motor_2006_param));
 
 }

User/module/chassis.h

@@ -5,7 +5,7 @@ extern "C"
 {
 #endif
 
-#include "module/struct_typedef.h"
+#include "struct_typedef.h"
 #include "component/filter.h"
 #include "component/pid.h"
 #include "component/ahrs.h"
@@ -22,23 +22,19 @@ extern "C"
 #define CHASSIS_ERR_TYPE (-4) /*Chassis_Type_t */
 
 
-//     纵向/横向
-#define radians atan(1.0f * 390 /390) // 角度制
-
-
 /*底盘模式*/
 typedef enum
 {
     STOP, // 底盘平行
     RC,   // 遥控模式
     CHASSIS_MODE_BREAK, /* 刹车模式，电机闭环控制保持静止。用于机器人停止状态 */
-    CHASSIS_MODE_RELAX, /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */
+     CHASSIS_MODE_RELAX, /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */
-    CHASSIS_MODE_FOLLOW_GIMBAL, /* 通过闭环控制使车头方向跟随云台 */
+     CHASSIS_MODE_FOLLOW_GIMBAL, /* 通过闭环控制使车头方向跟随云台 */
-    CHASSIS_MODE_ROTOR, /* 小陀螺模式，通过闭环控制使底盘不停旋转 */
+     CHASSIS_MODE_ROTOR, /* 小陀螺模式，通过闭环控制使底盘不停旋转 */
     
+     HUIHANG_BIANXING_DUOLUN, /*会航将军变形舵轮底盘*/
 } Chassis_mode_t;
 
-
 typedef struct
 {
     int cmd_power_on_safe; // 上电安全标志
@@ -55,6 +51,9 @@ typedef struct
     fp32 y_l;
 } Chassis_CMD_t;
 
+//     纵向/横向
+#define radians atan(1.0f * 390 /390) // 角度制
+
 	// 四个舵轮的安装误差
 	typedef struct
 	{
@@ -86,12 +85,23 @@ typedef struct
 		KPID_Params_t M3508v_param;
 		KPID_Params_t chassis_follow_gimbalPID;
 
+		KPID_Params_t chassis_2006_angle_param;
+		KPID_Params_t chassis_2006_Omega_param;
+		KPID_Params_t chassis_3508_angle_param;
+		KPID_Params_t chassis_3508_Omega_param;
+
 		MOTOR_RM_Param_t motor_3508_param[4]; // 四个3508电机
 		MOTOR_RM_Param_t motor_6020_param[4]; // 四个6020电机
+		MOTOR_RM_Param_t motor_2006_param;	// 一个2006电机
 
-		MOTOR_RM_Param_t chassis_follow_gimbal_param; // 底盘跟随云台
+		// MOTOR_RM_Param_t chassis_follow_gimbal_param; // 底盘跟随云台
 		float mech_zero;/*云台6020的机械中点*/
 		float travel ;/*云台6020的机械行程*/
+
+		float chassis2006_setangle; // 变形舵轮2006目标角度  大概是48圈
+		float chassis3508_setangle; // 变形舵轮3508目标角度  大概是60度左右
+
+
 	} Chassis_Param_t;
 
 	typedef struct
@@ -106,6 +116,7 @@ typedef struct
 	{
 		float rotor3508_out[4];
 		float rotor6020_out[4];
+		float motor2006_out;
 	} Chassis_out_t;
 
 	typedef struct
@@ -134,6 +145,7 @@ typedef struct
 		{
 			fp32 final_6020out[4];
 			fp32 final_3508out[4];
+			fp32 final_2006out;
 			// fp32 final_pitchout;
 		} final_out;
 
@@ -141,10 +153,15 @@ typedef struct
 		{
 			fp32 rotor_rpm3508[4];
 			fp32 rotor_current3508[4];
+			fp32 rotor_angle3508[4];
 			fp32 rotor_rpm6020[4];
 			fp32 rotor_angle6020[4];
 			fp32 rotor_current6020[4];
 			fp32 rotor_temp6020[4];
+			fp32 motor2006_rpm;
+			fp32 motor2006_current;
+			fp32 motor2006_temp;
+			fp32 motor2006_angle;
 
 			float gimbal_yaw_encoder;
 		} motorfeedback;
@@ -155,6 +172,12 @@ typedef struct
 			KPID_t chassis_6020OmegaPid[4];
 			KPID_t chassis_3508VPID[4];
 			KPID_t chassis_follow_gimbal_pid;
+
+			KPID_t chassis_2006_anglepid;
+			KPID_t chassis_2006_OmegaPid;
+			KPID_t chassis_3508_anglepid[4];
+			KPID_t chassis_3508_OmegaPid[4];
+
 		} pid;
 
 		uint8_t keeping_angle_flag; // 是否处于保持角度模式
@@ -176,12 +199,15 @@ typedef struct
 		float wz_multi; /* 小陀螺模式旋转方向 */
 		float mech_zero;/*云台6020的机械中点*/
 		float travel ;/*云台6020的机械行程*/
+
+		float chassis2006_setangle; // 变形舵轮2006目标角度  大概是48圈
+		float chassis3508_setangle; // 变形舵轮3508目标角度  大概是60度左右
 	} Chassis_t;
 
 
 	int8_t chassis_init(Chassis_t *c,  Chassis_Param_t *param, float target_freq);
 	int8_t Chassis_update(Chassis_t *c);
-	int8_t Chassis_Control(Chassis_t *c,  Chassis_CMD_t *c_cmd);
+	int8_t Chassis_Control(Chassis_t *c,  Chassis_CMD_t *c_cmd,uint32_t now);
 	void Chassis_Setoutput(Chassis_t *c);
 #ifdef __cplusplus
 }

User/module/cmd/cmd.h

@@ -111,6 +111,7 @@ typedef struct CMD_Context {
     CMD_ChassisOutput_t chassis;
     CMD_GimbalOutput_t gimbal;
     CMD_ShootOutput_t shoot;
+		
     } output;
 } CMD_t;
 

User/module/cmd/cmd_adapter.c

@@ -130,12 +130,14 @@ int8_t CMD_ET16s_Init(void *data) {
 }
 
 int8_t CMD_ET16s_GetInput(void *data, CMD_RawInput_t *output) {
+	
     ET16s_t *et16s = (ET16s_t *)data;
     
-    memset(output, 0, sizeof(CMD_RawInput_RC_t));
+		memset(output, 0, sizeof(CMD_RawInput_RC_t));
-    
+//    memset(output, 0, sizeof(CMD_RawInput_RC_t));
+
     output->online[CMD_SRC_RC] = et16s->header.online;
-    
+
     /* TODO: 按照AT9S的数据格式进行映射 */
     output->rc.joy_left.x  = et16s->ET16s.ch_l_x;
     output->rc.joy_left.y  = et16s->ET16s.ch_l_y;
@@ -170,7 +172,7 @@ int8_t CMD_ET16s_GetInput(void *data, CMD_RawInput_t *output) {
         case ET16S_SW_UP:   output->rc.sw[4] = CMD_SW_UP;   break;
         case ET16S_SW_MID:  output->rc.sw[4] = CMD_SW_MID;  break;
         case ET16S_SW_DOWN: output->rc.sw[4] = CMD_SW_DOWN; break;
-//        default:           	output->rc.sw[4] = CMD_SW_ERR;  break;
+        default:           	output->rc.sw[4] = CMD_SW_ERR;  break;
     }
 		    switch (et16s->ET16s.key_F) {
         case ET16S_SW_UP:   output->rc.sw[5] = CMD_SW_UP;   break;
@@ -199,7 +201,7 @@ bool CMD_ET16s_IsOnline(void *data) {
     return et16s->header.online;
 }
 
-CMD_DEFINE_ADAPTER(ET16s, et16s, CMD_SRC_RC, CMD_ET16s_Init, CMD_ET16s_GetInput, CMD_ET16s_IsOnline)
+CMD_DEFINE_ADAPTER(ET16s, cmd_et16s, CMD_SRC_RC, CMD_ET16s_Init, CMD_ET16s_GetInput, CMD_ET16s_IsOnline)
 #endif /* CMD_RC_DEVICE_TYPE == 3 */
 
 
@@ -225,6 +227,9 @@ int8_t CMD_Adapter_InitAll(void) {
 #elif CMD_RC_DEVICE_TYPE == 1
     /* AT9S 目前只支持 RC 输入 */
     CMD_Adapter_Register(&g_adapter_AT9S);
+#elif CMD_RC_DEVICE_TYPE == 2
+    /*  目前只支持 RC 输入 */
+  	
 #elif CMD_RC_DEVICE_TYPE == 3
     /* ET16s 目前只支持 RC 输入 */
     CMD_Adapter_Register(&g_adapter_ET16s);	

User/module/cmd/cmd_adapter.h

@@ -90,7 +90,7 @@ typedef struct {
     #define CMD_RC_ADAPTER_VAR  vt13
 #elif CMD_RC_DEVICE_TYPE == 3
     #include "device/ET16s.h"
-    CMD_DECLARE_ADAPTER(ET16s, et16s, ET16s_t)
+    CMD_DECLARE_ADAPTER(ET16s, cmd_et16s, ET16s_t)
     #define CMD_RC_ADAPTER_NAME ET16s
     #define CMD_RC_ADAPTER_VAR  ET16s		
 #endif

User/module/config.c

@@ -114,25 +114,25 @@ Config_RobotParam_t robot_config = {
 				
 				.motor={
 					/*按自己需求选择电机*/
-					.major_yaw=DM,
+//					.major_yaw=DM,
 					.pit=DM,
-					.yaw=RM,
+					.yaw=DM,
 					/*是否开启限位*/
 					.limit_yaw=false,
 					.limit_pit=true,
 					.pit_rm_motor={},
 					.yaw_rm_motor={BSP_CAN_2,0x209,MOTOR_GM6020,false,false},	
 						/*达妙电机参数自己配*/
-					.major_yaw_dm_motor={
+					.yaw_dm_motor={
-					    .can=BSP_CAN_2,
+					    .can=BSP_CAN_1,
-							.can_id = 0x1,
+							.can_id = 0x01,
 							.master_id=0x11,
 							.module = MOTOR_DM_J4310,
 							.reverse=false
 					},
 					.pit_dm_motor={
 					    .can=BSP_CAN_2,
-							.can_id = 0x2,
+							.can_id = 0x02,
 							.master_id=0x12,
 							.module = MOTOR_DM_J4310,
 							.reverse=true,					

User/module/gimbal.h

@@ -38,6 +38,7 @@ typedef enum {
 } Gimbal_Control_Mode_t;
 
 typedef enum {
+	NULL_MOTOR,
   DM,    /*大妙*/
   RM, 		/*大疆 */
 } Gimbal_MOTOR_t;

User/module/step_motor.c

@@ -1,22 +0,0 @@
-/* 底盘固定模组,用步进 */
-#include "main.h"
-#include "step_motor.h"
-#include "bsp/gpio.h"
-#include "cmsis_os2.h"     
-
-int8_t Step_Motor_Ctrl(int pulse,float time){
-
-	/* 控制方向 */
-	BSP_GPIO_WritePin(BSP_GPIO_STEP__DIRECTION, true);
-	osDelay(1); // 方向稳定时间
-	
-	for(int i;i >= pulse;i++){
-	BSP_GPIO_WritePin(BSP_GPIO_STEP_MOTOR,true);
-	osDelay(time);
-	BSP_GPIO_WritePin(BSP_GPIO_STEP_MOTOR,false);	
-	osDelay(time);
-	}
-	return 0;
-}
-
-

User/task/chassis_ctrl.c

@@ -49,7 +49,7 @@ void Task_chassis_ctrl(void *argument) {
     // 或者什么都不做，让底盘保持上一帧的状态（取决于你的设计）
     // 一个安全的选择是让底盘停止
 	 Chassis_CMD_t safe_cmd = {.mode = STOP, .Vx = 0, .Vy = 0, .Vw = 0};
-    Chassis_Control(&chassis, &safe_cmd);
+    Chassis_Control(&chassis, &safe_cmd,tick);
 //}
     Chassis_Setoutput(&chassis);	
     /* USER CODE END */

User/task/cmd.c

@@ -63,19 +63,17 @@ void Task_cmd(void *argument) {
 		#elif CMD_RCTypeTable_Index == 2
 		osMessageQueueGet(task_runtime.msgq.rc.et16s, &cmd_et16s, NULL, 0);
     #endif		
-		
-
     CMD_Update(&cmd);
     /* 获取命令发送到各模块 */
     cmd_for_chassis = CMD_GetChassisCmd(&cmd);
     cmd_for_gimbal = CMD_GetGimbalCmd(&cmd);
-    cmd_for_shoot = CMD_GetShootCmd(&cmd);
+    cmd_for_shoot = CMD_GetShootCmd(&cmd);	
 		osMessageQueueReset(task_runtime.msgq.gimbal.cmd);
 		osMessageQueuePut(task_runtime.msgq.gimbal.cmd, cmd_for_gimbal, 0, 0);
 		osMessageQueueReset(task_runtime.msgq.shoot.cmd);
     osMessageQueuePut(task_runtime.msgq.shoot.cmd, cmd_for_shoot, 0, 0);
 		osMessageQueueReset(task_runtime.msgq.chassis.cmd);
-    osMessageQueuePut(task_runtime.msgq.chassis.cmd, cmd_for_chassis, 0, 0);
+    osMessageQueuePut(task_runtime.msgq.chassis.cmd, cmd_for_chassis, 0, 0);			
     /* USER CODE END */
     osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
   }

User/task/config.yaml

@@ -61,3 +61,10 @@
   function: Task_ET16s
   name: ET16s
   stack: 256
+- delay: 0
+  description: ''
+  freq_control: true
+  frequency: 500.0
+  function: Task_vofa
+  name: vofa
+  stack: 256

User/task/init.c

@@ -11,6 +11,7 @@
 #include "module/chassis.h"
 #include "device/dr16.h"
 #include "device/et16s.h"
+#include "device/step_motor.h"
 /* USER INCLUDE END */
 
 /* Private typedef ---------------------------------------------------------- */
@@ -42,14 +43,14 @@ void Task_Init(void *argument) {
   task_runtime.thread.cmd = osThreadNew(Task_cmd, NULL, &attr_cmd);
   task_runtime.thread.step_motor = osThreadNew(Task_step_motor, NULL, &attr_step_motor);
   task_runtime.thread.ET16s = osThreadNew(Task_ET16s, NULL, &attr_ET16s);
+  task_runtime.thread.vofa = osThreadNew(Task_vofa, NULL, &attr_vofa);
 
   // 创建消息队列
   /* USER MESSAGE BEGIN */
   task_runtime.msgq.user_msg= osMessageQueueNew(2u, 10, NULL);
 	task_runtime.msgq.gimbal.imu= osMessageQueueNew(2u, sizeof(Gimbal_IMU_t), NULL);
   task_runtime.msgq.gimbal.cmd= osMessageQueueNew(2u, sizeof(Gimbal_CMD_t), NULL);
-	task_runtime.msgq.chassis.cmd= osMessageQueueNew(2u, sizeof(Chassis_CMD_t), NULL);	
+	task_runtime.msgq.chassis.cmd= osMessageQueueNew(2u, sizeof(Chassis_CMD_t), NULL);		
-	task_runtime.msgq.chassis.state= osMessageQueueNew(2u, sizeof(Chassis_t), NULL);	
 	task_runtime.msgq.rc.dr16= osMessageQueueNew(2u, sizeof(DR16_t), NULL);
 	task_runtime.msgq.rc.et16s= osMessageQueueNew(2u, sizeof(ET16s_t), NULL);	
   /* USER MESSAGE END */

User/task/step_motor.c

@@ -6,7 +6,9 @@
 /* Includes ----------------------------------------------------------------- */
 #include "task/user_task.h"
 /* USER INCLUDE BEGIN */
-
+#include "device/step_motor.h"
+#include "device/ET16s.h"
+#include "bsp/gpio.h"
 /* USER INCLUDE END */
 
 /* Private typedef ---------------------------------------------------------- */
@@ -14,7 +16,15 @@
 /* Private macro ------------------------------------------------------------ */
 /* Private variables -------------------------------------------------------- */
 /* USER STRUCT BEGIN */
+STEP_MOTOR StepMotor_param={
+	.direction=true,
+	.pulse=900,
+	.time=500,
 
+};
+int key1=1;
+ET16S_SwitchPos_t Key_A;
+int prev_state ; // 初始为无状态
 /* USER STRUCT END */
 
 /* Private function --------------------------------------------------------- */
@@ -30,13 +40,26 @@ void Task_step_motor(void *argument) {
 
   uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
   /* USER CODE INIT BEGIN */
+  /* 当前状态变量 */
 
+//  StepMotor_param.state = 0; // 初始状态为停止
   /* USER CODE INIT END */
   
   while (1) {
     tick += delay_tick; /* 计算下一个唤醒时刻 */
     /* USER CODE BEGIN */
+		if(osMessageQueueGet(task_runtime.msgq.chassis.SetpMotor, &Key_A, NULL, 0)==osOK);
+//    /* 监听和更新拨杆状态 */
 
+    if (key1 == 0) {
+
+    } else if (key1 == 1) {
+
+			Step_Motor_Ctrl(&StepMotor_param);
+    }
+
+		
+	
     /* USER CODE END */
     osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
   }

User/task/user_task.c

@@ -53,4 +53,9 @@ const osThreadAttr_t attr_ET16s = {
     .name = "ET16s",
     .priority = osPriorityNormal,
     .stack_size = 256 * 4,
+};
+const osThreadAttr_t attr_vofa = {
+    .name = "vofa",
+    .priority = osPriorityNormal,
+    .stack_size = 256 * 4,
 };

User/task/user_task.h

@@ -22,6 +22,7 @@ extern "C" {
 #define CMD_FREQ (500.0)
 #define STEP_MOTOR_FREQ (500.0)
 #define ET16S_FREQ (500.0)
+#define VOFA_FREQ (500.0)
 
 /* 任务初始化延时ms */
 #define TASK_INIT_DELAY (100u)
@@ -34,6 +35,7 @@ extern "C" {
 #define CMD_INIT_DELAY (0)
 #define STEP_MOTOR_INIT_DELAY (0)
 #define ET16S_INIT_DELAY (0)
+#define VOFA_INIT_DELAY (0)
 
 /* Exported defines --------------------------------------------------------- */
 /* Exported macro ----------------------------------------------------------- */
@@ -52,6 +54,7 @@ typedef struct {
         osThreadId_t cmd;
         osThreadId_t step_motor;
         osThreadId_t ET16s;
+        osThreadId_t vofa;
     } thread;
 
     /* USER MESSAGE BEGIN */
@@ -68,17 +71,12 @@ typedef struct {
 						osMessageQueueId_t remote;
         }gimbal;
         struct {
+            osMessageQueueId_t SetpMotor;					
             osMessageQueueId_t state;
             osMessageQueueId_t cmd;
 						osMessageQueueId_t remote;
 					
         }chassis;
-        struct {
-            osMessageQueueId_t state;
-            osMessageQueueId_t cmd;
-						osMessageQueueId_t remote;
-					
-        }telescoping;
         struct {
             osMessageQueueId_t state;
             osMessageQueueId_t dr16;
@@ -109,6 +107,7 @@ typedef struct {
         UBaseType_t cmd;
         UBaseType_t step_motor;
         UBaseType_t ET16s;
+        UBaseType_t vofa;
     } stack_water_mark;
 
     /* 各任务运行频率 */
@@ -122,6 +121,7 @@ typedef struct {
         float cmd;
         float step_motor;
         float ET16s;
+        float vofa;
     } freq;
 
     /* 任务最近运行时间 */
@@ -135,6 +135,7 @@ typedef struct {
         float cmd;
         float step_motor;
         float ET16s;
+        float vofa;
     } last_up_time;
 
 } Task_Runtime_t;
@@ -153,6 +154,7 @@ extern const osThreadAttr_t attr_dr16;
 extern const osThreadAttr_t attr_cmd;
 extern const osThreadAttr_t attr_step_motor;
 extern const osThreadAttr_t attr_ET16s;
+extern const osThreadAttr_t attr_vofa;
 
 /* 任务函数声明 */
 void Task_Init(void *argument);
@@ -165,6 +167,7 @@ void Task_dr16(void *argument);
 void Task_cmd(void *argument);
 void Task_step_motor(void *argument);
 void Task_ET16s(void *argument);
+void Task_vofa(void *argument);
 
 #ifdef __cplusplus
 }

User/task/vofa.c

@@ -1,14 +1,12 @@
 /*
-    telecoping Task
+    vofa Task
     
 */
 
 /* Includes ----------------------------------------------------------------- */
 #include "task/user_task.h"
 /* USER INCLUDE BEGIN */
-#include "telescoping_gimal.h"
+#include "device/vofa.h"
-#include "main.h"
-#include "module/config.h"
 /* USER INCLUDE END */
 
 /* Private typedef ---------------------------------------------------------- */
@@ -16,36 +14,30 @@
 /* Private macro ------------------------------------------------------------ */
 /* Private variables -------------------------------------------------------- */
 /* USER STRUCT BEGIN */
-Telescoping_t telescoping;
+float channel[4];
-Telescoping_CMD_t telescoping_cmd;
 /* USER STRUCT END */
 
 /* Private function --------------------------------------------------------- */
 /* Exported functions ------------------------------------------------------- */
-void Task_telecoping(void *argument) {
+void Task_vofa(void *argument) {
   (void)argument; /* 未使用argument，消除警告 */
 
   
   /* 计算任务运行到指定频率需要等待的tick数 */
-  const uint32_t delay_tick = osKernelGetTickFreq() / TELECOPING_FREQ;
+  const uint32_t delay_tick = osKernelGetTickFreq() / VOFA_FREQ;
 
-  osDelay(TELECOPING_INIT_DELAY); /* 延时一段时间再开启任务 */
+  osDelay(VOFA_INIT_DELAY); /* 延时一段时间再开启任务 */
 
   uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
   /* USER CODE INIT BEGIN */
- Telescoping_Init(&telescoping,&Config_GetRobotParam()->telescoping,
+		VOFA_init(VOFA_PROTOCOL_JUSTFLOAT);
-                    TELECOPING_FREQ);
-		telescoping_cmd.mode=TELESCOPING_MODE_ANTI_CLOCKWISE ;
   /* USER CODE INIT END */
   
   while (1) {
     tick += delay_tick; /* 计算下一个唤醒时刻 */
     /* USER CODE BEGIN */
-
+		channel[1]=2;
-		Telescoping_UpdateFeedback(&telescoping);
+		VOFA_Send(channel, 2, false); 
-//		osMessageQueueGet(task_runtime.msgq.telescoping.cmd, &telescoping_cmd, NULL, 0);
-		Telescoping_Control(&telescoping,&telescoping_cmd); 		
-		Telescoping_Output(&telescoping);
     /* USER CODE END */
     osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
   }