加了视觉

Core/Src/gpio.c

@@ -76,7 +76,7 @@ void MX_GPIO_Init(void)
   /*Configure GPIO pin : PtPin */
   GPIO_InitStruct.Pin = up_ball_Pin;
   GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
-  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  GPIO_InitStruct.Pull = GPIO_PULLDOWN;
   HAL_GPIO_Init(up_ball_GPIO_Port, &GPIO_InitStruct);
 
   /*Configure GPIO pin : PtPin */
@@ -121,7 +121,7 @@ void MX_GPIO_Init(void)
   /*Configure GPIO pin : PtPin */
   GPIO_InitStruct.Pin = STOP_Pin;
   GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
-  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  GPIO_InitStruct.Pull = GPIO_PULLDOWN;
   HAL_GPIO_Init(STOP_GPIO_Port, &GPIO_InitStruct);
 
   /*Configure GPIO pin : PtPin */

Core/Src/usart.c

@@ -102,7 +102,7 @@ void MX_USART6_UART_Init(void)
 
   /* USER CODE END USART6_Init 1 */
   huart6.Instance = USART6;
-  huart6.Init.BaudRate = 4000000;
+  huart6.Init.BaudRate = 115200;
   huart6.Init.WordLength = UART_WORDLENGTH_8B;
   huart6.Init.StopBits = UART_STOPBITS_1;
   huart6.Init.Parity = UART_PARITY_NONE;

MDK-ARM/.vscode/c_cpp_properties.json

@@ -3,26 +3,26 @@
         {
             "name": "R1-shooter",
             "includePath": [
-                "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Core\\Inc",
+                "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Core\\Inc",
-                "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Drivers\\STM32F4xx_HAL_Driver\\Inc",
+                "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Drivers\\STM32F4xx_HAL_Driver\\Inc",
-                "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Drivers\\STM32F4xx_HAL_Driver\\Inc\\Legacy",
+                "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Drivers\\STM32F4xx_HAL_Driver\\Inc\\Legacy",
-                "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\include",
+                "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\include",
-                "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\CMSIS_RTOS_V2",
+                "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\CMSIS_RTOS_V2",
-                "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\RVDS\\ARM_CM4F",
+                "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\RVDS\\ARM_CM4F",
-                "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Drivers\\CMSIS\\Device\\ST\\STM32F4xx\\Include",
+                "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Drivers\\CMSIS\\Device\\ST\\STM32F4xx\\Include",
-                "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Drivers\\CMSIS\\Include",
+                "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Drivers\\CMSIS\\Include",
-                "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\User\\bsp",
+                "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\User\\bsp",
-                "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\User\\module",
+                "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\User\\module",
-                "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\User\\task",
+                "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\User\\task",
-                "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\User\\lib",
+                "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\User\\lib",
-                "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\User\\device",
+                "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\User\\device",
                 "D:\\keil\\ARM\\ARMCC\\include",
                 "D:\\keil\\ARM\\ARMCC\\include\\rw",
-                "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\MDK-ARM",
+                "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\MDK-ARM",
-                "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Core\\Src",
+                "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Core\\Src",
-                "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Drivers\\STM32F4xx_HAL_Driver\\Src",
+                "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Drivers\\STM32F4xx_HAL_Driver\\Src",
-                "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source",
+                "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source",
-                "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\MemMang"
+                "d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\MemMang"
             ],
             "defines": [
                 "USE_HAL_DRIVER",

MDK-ARM/.vscode/keil-assistant.log

@@ -54,3 +54,73 @@
 
 [info] Log at : 2025/4/26|16:18:23|GMT+0800
 
+[info] Log at : 2025/5/7|20:43:43|GMT+0800
+
+[info] Log at : 2025/5/8|16:11:00|GMT+0800
+
+[info] Log at : 2025/5/10|16:53:30|GMT+0800
+
+[info] Log at : 2025/5/10|20:53:12|GMT+0800
+
+[info] Log at : 2025/5/11|14:55:09|GMT+0800
+
+[info] Log at : 2025/5/11|15:23:30|GMT+0800
+
+[info] Log at : 2025/5/12|19:40:58|GMT+0800
+
+[info] Log at : 2025/5/13|20:50:37|GMT+0800
+
+[info] Log at : 2025/5/14|20:21:54|GMT+0800
+
+[info] Log at : 2025/5/15|14:31:29|GMT+0800
+
+[info] Log at : 2025/5/15|19:24:56|GMT+0800
+
+[info] Log at : 2025/5/17|21:50:24|GMT+0800
+
+[info] Log at : 2025/5/18|16:09:49|GMT+0800
+
+[info] Log at : 2025/5/18|19:22:14|GMT+0800
+
+[info] Log at : 2025/5/18|23:12:00|GMT+0800
+
+[info] Log at : 2025/5/19|21:08:52|GMT+0800
+
+[info] Log at : 2025/5/20|00:19:48|GMT+0800
+
+[info] Log at : 2025/5/20|19:15:21|GMT+0800
+
+[info] Log at : 2025/5/21|17:11:48|GMT+0800
+
+[info] Log at : 2025/5/21|19:21:48|GMT+0800
+
+<<<<<<< HEAD
+[info] Log at : 2025/5/22|18:05:58|GMT+0800
+
+[info] Log at : 2025/5/23|19:18:37|GMT+0800
+
+[info] Log at : 2025/5/23|19:32:28|GMT+0800
+=======
+[info] Log at : 2025/5/22|16:11:31|GMT+0800
+>>>>>>> 8fc2f50677ecabc32b37eac7a7df89d916fefef0
+
+[info] Log at : 2025/5/23|21:04:08|GMT+0800
+
+[info] Log at : 2025/5/25|14:26:08|GMT+0800
+
+[info] Log at : 2025/5/26|19:07:26|GMT+0800
+
+[info] Log at : 2025/5/26|20:16:52|GMT+0800
+
+[info] Log at : 2025/5/27|00:06:58|GMT+0800
+
+[info] Log at : 2025/5/27|19:35:30|GMT+0800
+
+[info] Log at : 2025/5/28|13:50:56|GMT+0800
+
+[info] Log at : 2025/5/29|19:03:54|GMT+0800
+
+[info] Log at : 2025/5/30|19:34:50|GMT+0800
+
+[info] Log at : 2025/5/31|14:39:23|GMT+0800
+

MDK-ARM/.vscode/settings.json

@@ -11,6 +11,24 @@
         "nuc.h": "c",
         "crc_ccitt.h": "c",
         "functional": "cpp",
-        "vofa.h": "c"
+        "vofa.h": "c",
+        "user_math.h": "c",
+        "queue": "cpp",
+        "cctype": "cpp",
+        "cerrno": "cpp",
+        "cfloat": "cpp",
+        "clocale": "cpp",
+        "cmath": "cpp",
+        "cstdarg": "cpp",
+        "cstddef": "cpp",
+        "cstdint": "cpp",
+        "cstdio": "cpp",
+        "cstdlib": "cpp",
+        "cwchar": "cpp",
+        "cwctype": "cpp",
+        "exception": "cpp",
+        "new": "cpp",
+        "typeinfo": "cpp",
+        "ostream": "cpp"
     }
 }

MDK-ARM/.vscode/uv4.log

@@ -1,8 +1,8 @@
 *** Using Compiler 'V5.06 update 7 (build 960)', folder: 'D:\keil\ARM\ARMCC\Bin'
 Build target 'R1-shooter'
-compiling ball.cpp...
+compiling usart.c...
 linking...
-Program Size: Code=26644 RO-data=1812 RW-data=240 ZI-data=23520  
+Program Size: Code=30588 RO-data=2144 RW-data=276 ZI-data=24012  
 FromELF: creating hex file...
 "R1-shooter\R1-shooter.axf" - 0 Error(s), 0 Warning(s).
-Build Time Elapsed:  00:00:03
+Build Time Elapsed:  00:00:05

MDK-ARM/.vscode/uv4.log.lock

@@ -1 +1 @@
-2025/4/26 16:32:18
+2025/5/31 16:57:56

MDK-ARM/R1-shooter.uvoptx

@@ -183,77 +183,92 @@
         <Ww>
           <count>5</count>
           <WinNumber>1</WinNumber>
-          <ItemText>angle1,0x0A</ItemText>
+          <ItemText>cnt1,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>6</count>
           <WinNumber>1</WinNumber>
-          <ItemText>k,0x0A</ItemText>
+          <ItemText>cmd_fromnuc</ItemText>
         </Ww>
         <Ww>
           <count>7</count>
           <WinNumber>1</WinNumber>
-          <ItemText>cnt1,0x0A</ItemText>
+          <ItemText>nucbuf</ItemText>
         </Ww>
         <Ww>
           <count>8</count>
           <WinNumber>1</WinNumber>
-          <ItemText>cmd_fromnuc</ItemText>
+          <ItemText>nucData</ItemText>
         </Ww>
         <Ww>
           <count>9</count>
           <WinNumber>1</WinNumber>
-          <ItemText>nucbuf</ItemText>
+          <ItemText>send,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>10</count>
           <WinNumber>1</WinNumber>
-          <ItemText>nucData</ItemText>
+          <ItemText>\\R1_shooter\../User/task/ballTask.cpp\ball,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>11</count>
           <WinNumber>1</WinNumber>
-          <ItemText>send,0x0A</ItemText>
+          <ItemText>abc,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>12</count>
           <WinNumber>1</WinNumber>
-          <ItemText>speed1,0x0A</ItemText>
+          <ItemText>shoot,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>13</count>
           <WinNumber>1</WinNumber>
-          <ItemText>\\R1_shooter\../User/task/ballTask.cpp\ball,0x0A</ItemText>
+          <ItemText>speedm,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>14</count>
           <WinNumber>1</WinNumber>
-          <ItemText>currentState1</ItemText>
+          <ItemText>shoot_flag,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>15</count>
           <WinNumber>1</WinNumber>
-          <ItemText>abc,0x0A</ItemText>
+          <ItemText>angle1,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>16</count>
           <WinNumber>1</WinNumber>
-          <ItemText>ball_state,0x0A</ItemText>
+          <ItemText>motor_5065</ItemText>
         </Ww>
         <Ww>
           <count>17</count>
           <WinNumber>1</WinNumber>
-          <ItemText>flag,0x0A</ItemText>
+          <ItemText>test_distance</ItemText>
         </Ww>
         <Ww>
           <count>18</count>
           <WinNumber>1</WinNumber>
-          <ItemText>triggerCount,0x0A</ItemText>
+          <ItemText>xxx</ItemText>
         </Ww>
         <Ww>
           <count>19</count>
           <WinNumber>1</WinNumber>
-          <ItemText>last_ball_state,0x0A</ItemText>
+          <ItemText>test_speed,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>20</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>speedm,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>21</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>Torque,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>22</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>triggerspeed,0x0A</ItemText>
         </Ww>
       </WatchWindow1>
       <WatchWindow2>
@@ -957,17 +972,41 @@
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
+    <File>
+      <GroupNumber>7</GroupNumber>
+      <FileNumber>50</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>..\User\lib\filter.c</PathWithFileName>
+      <FilenameWithoutPath>filter.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
+    <File>
+      <GroupNumber>7</GroupNumber>
+      <FileNumber>51</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>..\User\lib\kalman.c</PathWithFileName>
+      <FilenameWithoutPath>kalman.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
   </Group>
 
   <Group>
     <GroupName>User/device</GroupName>
-    <tvExp>0</tvExp>
+    <tvExp>1</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>50</FileNumber>
+      <FileNumber>52</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -979,7 +1018,7 @@
     </File>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>51</FileNumber>
+      <FileNumber>53</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -991,7 +1030,7 @@
     </File>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>52</FileNumber>
+      <FileNumber>54</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1003,7 +1042,7 @@
     </File>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>53</FileNumber>
+      <FileNumber>55</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1015,7 +1054,7 @@
     </File>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>54</FileNumber>
+      <FileNumber>56</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1027,7 +1066,7 @@
     </File>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>55</FileNumber>
+      <FileNumber>57</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1041,13 +1080,13 @@
 
   <Group>
     <GroupName>User/module</GroupName>
-    <tvExp>0</tvExp>
+    <tvExp>1</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>56</FileNumber>
+      <FileNumber>58</FileNumber>
       <FileType>8</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1059,7 +1098,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>57</FileNumber>
+      <FileNumber>59</FileNumber>
       <FileType>8</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1071,7 +1110,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>58</FileNumber>
+      <FileNumber>60</FileNumber>
       <FileType>8</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1083,7 +1122,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>59</FileNumber>
+      <FileNumber>61</FileNumber>
       <FileType>8</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1103,7 +1142,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>60</FileNumber>
+      <FileNumber>62</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1115,7 +1154,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>61</FileNumber>
+      <FileNumber>63</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1127,7 +1166,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>62</FileNumber>
+      <FileNumber>64</FileNumber>
       <FileType>8</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1139,7 +1178,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>63</FileNumber>
+      <FileNumber>65</FileNumber>
       <FileType>8</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1151,7 +1190,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>64</FileNumber>
+      <FileNumber>66</FileNumber>
       <FileType>8</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1163,7 +1202,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>65</FileNumber>
+      <FileNumber>67</FileNumber>
       <FileType>8</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1175,7 +1214,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>66</FileNumber>
+      <FileNumber>68</FileNumber>
       <FileType>8</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>

MDK-ARM/R1-shooter.uvprojx

@@ -658,6 +658,16 @@
               <FileType>1</FileType>
               <FilePath>..\User\lib\user_math.c</FilePath>
             </File>
+            <File>
+              <FileName>filter.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\lib\filter.c</FilePath>
+            </File>
+            <File>
+              <FileName>kalman.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\lib\kalman.c</FilePath>
+            </File>
           </Files>
         </Group>
         <Group>

README.md

@@ -14,4 +14,15 @@ r1上层
 
 //PC6  接球光电
 
-//PI6  运球光电
+//PI6  运球光电
+
+## 上层过程
+
++ 一个按键切换 运球 /发射
++ 👈 运球 +
++ .3
+
+
+
+
+

User/bsp/TopDefine.h

@@ -1,10 +1,63 @@
 #ifndef TOP_DEFINE_H
 #define TOP_DEFINE_H
-#define RTOS 1
-#define DEBUG 1
-#define AUTO 0
 
+//===================用户配置===================
+//是否使用freertos
+#ifndef _FREERTOS
+#define _FREERTOS 1
+#endif
+//是否开启调试
+#ifndef DEBUG
+#define DEBUG 1
+#endif
+//是否使用自动
+#ifndef _AUTO
+#define _AUTO 0
+#endif
+//是否使用大疆DT7遥控器
+#ifndef DT7
+#define DT7 0
+#endif
+//是否使用三摩擦
+#ifndef HANDLING3
+#define HANDLING3 1
+#endif
+//云台使用类型
+#ifndef GM6020ING
+#define GM6020ING 1
+#endif
+//=============================================
+//================任务通知,时间组================//
 //事件组
 #define EVENT_RC    (1<<1)
 #define EVENT_CAN   (1<<2)
+
+//================任务通知================//
+//运球
+#define BALL_OK	(1<<1)
+//接到放球命令
+#define PUT_CMD	(1<<2)
+//运球结束
+#define PUT_OK	(1<<2)
+
+
+//take任务要等待上面两个标志位
+#define TAKE_WAIT (0x0C)
+//要发送ok了
+#define BALL_SEND (1<<6)
+//能够处理放球命令
+#define PUT_ENABLE (1<<7)
+
+
+//userTask里定义机器人状态
+typedef enum
+{
+    ROBOT_IN_ONE = 1,
+    // ROBOT_IN_TWO,
+    ROBOT_FIND_BALL,
+    ROBOT_PUT_BALL,
+    ROBOT_ERROR
+}robot_status_e;
+
+
 #endif

User/bsp/uart_it.c

@@ -13,7 +13,7 @@ static BSP_UART_t UART_Get(UART_HandleTypeDef *huart) {
     return BSP_UART_DR16;
   else if (huart->Instance == USART1)
     return BSP_UART_REF;
-  else if (huart->Instance == USART1)
+  else if (huart->Instance == USART6)
     return BSP_UART_AI;
   /*
   else if (huart->Instance == USARTX)
@@ -112,7 +112,7 @@ UART_HandleTypeDef *BSP_UART_GetHandle(BSP_UART_t uart) {
     case BSP_UART_REF:
       return &huart1;
     case BSP_UART_AI:
-      return &huart1;
+      return &huart6;
     /*
     case BSP_UART_XXX:
             return &huartX;

User/device/GO_M8010_6_Driver.c

@@ -32,7 +32,7 @@ void GO_M8010_init (){
         
     }
     // HAL_UART_RegisterCallback(&huart1, HAL_UART_TX_COMPLETE_CB_ID, uartTxCB);
-       HAL_UART_RegisterCallback(&huart6, HAL_UART_TX_COMPLETE_CB_ID, uartTxCB);
+    //   HAL_UART_RegisterCallback(&huart6, HAL_UART_TX_COMPLETE_CB_ID, uartTxCB);
 }
 
 

User/device/device.h

@@ -29,11 +29,15 @@ DEVICE_ERR_OVERFLOW = -5,
 #define SIGNAL_IST8310_MAGN_NEW_DATA (1u << 8)
 #define SIGNAL_IST8310_MAGN_RAW_REDY (1u << 9)
 
-#define SIGNAL_REFEREE_RAW_REDY (1u << 10)
+#define SIGNAL_R12DS_BUF0_REDY (1u << 7)
-#define SIGNAL_REFEREE_FAST_REFRESH_UI (1u << 11)
+#define SIGNAL_NUC_RAW_REDY (1u << 8)
-#define SIGNAL_REFEREE_SLOW_REFRESH_UI (1u << 12)
+#define SIGNAL_IST8310_MAGN_RAW_REDY (1u << 9)
+#define SIGNAL_ACTION_RAW_REDY (1u << 10)
+#define SIGNAL_OPSTIMER_REDY  (1u << 11)
+#define SIGNAL_R12DS_BUF1_REDY (1u << 12)
 
 #define SIGNAL_AI_RAW_REDY (1u << 14)
+#define SIGNAL_KEY_REDY (1u << 15)
 
 #ifdef __cplusplus
 }

User/device/djiMotor.c

@@ -41,10 +41,23 @@
 				else if((ptr)->type == M2006)																\
 					(ptr)->total_angle = (fp64)((ptr)->round_cnt * 8192 + (ptr)->ecd - (ptr)->offset_ecd ) * MOTOR_ECD_TO_ANGLE_2006;      \
 				else																											\
-					(ptr)->total_angle = (fp64)((ptr)->round_cnt * 8192 + (ptr)->ecd - (ptr)->offset_ecd ) * MOTOR_ECD_TO_ANGLE_6020;      \
+				(ptr)->total_angle = (fp64)((ptr)->round_cnt * 8192 + (ptr)->ecd - (ptr)->offset_ecd ) * MOTOR_ECD_TO_ANGLE_6020;      \
 				(ptr)->real_round = (ptr)->total_angle / 360;													\
 				(ptr)->real_angle = (ptr)->total_angle % 360;													\
     }
+ //单圈绝对值式解析   
+ #define get_6020_motor_measure(ptr, data)                     								\
+    {                                                                 	\
+        (ptr)->last_ecd = (ptr)->ecd;                              			\
+        (ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]);            \
+        (ptr)->speed_rpm = (uint16_t)((data)[2] << 8 | (data)[3]);      \
+        (ptr)->given_current = (uint16_t)((data)[4] << 8 | (data)[5]);  \
+        (ptr)->temperate = (data)[6]; 																	\
+        (ptr)->real_round=(ptr)->ecd/ (float)CAN_MOTOR_ENC_RES * 360.0f; \
+				(ptr)->ecd - (ptr)->last_ecd > 4096 ? ((ptr)->round_cnt--) : ((ptr)->round_cnt=(ptr)->round_cnt);																	\
+				(ptr)->ecd - (ptr)->last_ecd < -4096 ? ((ptr)->round_cnt++) : ((ptr)->round_cnt=(ptr)->round_cnt);																\
+				(ptr)->total_angle = (fp64)((ptr)->round_cnt * 8192 + (ptr)->ecd - (ptr)->offset_ecd ) * MOTOR_ECD_TO_ANGLE_6020;														\
+    } 
 
 /*(ptr)->real_angle = (ptr)->real_angle % 360;	*/
 #define get_motor_offset(ptr, data)																			\
@@ -65,11 +78,18 @@
 
   }
 
+//小米
+#define ID_xiaomi 1		
+#define Pmax 1		
+#define speed_full_scale 200.0f
+#define current_full_scale 4.0f
+
+
 #if DEBUG == 1
 
 //电机回传数据结构体
 motor_measure_t motor_chassis[10];
-CAN_MotorFeedback_t motor_5065[2];
+CAN_MotorFeedback_t motor_5065[3];
 #else
 static motor_measure_t motor_chassis[5];
 #endif
@@ -80,16 +100,25 @@ static uint8_t              can_send_data_2ff[8];
 static CAN_TxHeaderTypeDef  tx_message_200;
 static uint8_t              can_send_data_200[8];
 
+//can1 dji
 CAN_RxHeaderTypeDef dji_rx_header;
 uint8_t dji_rx_data[8];
 
+CAN_RawTx_t raw_tx;
+
+//can2 vesc xiaomi
 CAN_RxHeaderTypeDef rx_header;
 uint8_t rx_data[8];
 
 static CAN_TxHeaderTypeDef vesc_tx_message;
 static uint8_t  vesc_send_data[4];
+//小米
+uint8_t can_send_data[8];
+// 定义 CAN 传输头部信息
+static CAN_TxHeaderTypeDef tx_message;
+
+MotorFeedback_t MotorFeedback;
 
-CAN_RawTx_t raw_tx;
 /**
  * @brief           数据处理函数
  * @param[in]       none
@@ -99,13 +128,11 @@ void djiMotorEncode()
 {
     switch (dji_rx_header.StdId)
     {
-    case 0x201:
+    case M3508_1:
-    case 0x202:
+    case M3508_2:
-    case 0x203:
+    case M3508_3:
-    case 0x204:
+    case M3508_4:
-    case 0x205:
+    case M2006_1:
-    case 0x206:
-    case 0x207:
     {
         static uint8_t i = 0;
         //get motor id
@@ -119,6 +146,29 @@ void djiMotorEncode()
         }
         break;
     }
+    case GM6020_1:
+    {
+      if(motor_chassis[6].msg_cnt<=50)
+      {
+        motor_chassis[6].msg_cnt++;
+        get_motor_offset(&motor_chassis[6], dji_rx_data);
+      }else{
+        get_6020_motor_measure(&motor_chassis[6], dji_rx_data);
+      }
+        break;
+    }
+    case GM6020_2:
+    {
+      if(motor_chassis[7].msg_cnt<=50)
+      {
+        motor_chassis[7].msg_cnt++;
+        get_motor_offset(&motor_chassis[7], dji_rx_data);
+      }else{
+        get_6020_motor_measure(&motor_chassis[7], dji_rx_data);
+      }
+        break;
+    }
+
     default:
     {
         break;
@@ -134,15 +184,28 @@ void can2MotorEncode()
       // 存储消息到对应的电机结构体中
       VescMotor_Decode(&motor_5065[0], &rx_header,rx_data);
       break;
-
     case CAN_VESC5065_M2_MSG1:
       // 存储消息到对应的电机结构体中
       VescMotor_Decode(&motor_5065[1], &rx_header,rx_data);
+     case CAN_VESC5065_M3_MSG1:
+      // 存储消息到对应的电机结构体中
+      VescMotor_Decode(&motor_5065[2], &rx_header,rx_data);
       break;
-      
+
     default:
       break;
   }
+
+    switch (rx_data[0]) 
+  {
+				 case ID_xiaomi:
+				 Parse_CAN_Response (rx_data,&MotorFeedback);
+				 break ;
+  
+   default:
+    break;
+  }
+
 }
 
 #if FREERTOS_DJI == 0
@@ -159,7 +222,7 @@ void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
     
 }
 #else
-static osEventFlagsId_t eventReceive;
+//static osEventFlagsId_t eventReceive;
 static osThreadId_t thread_alert;
 /**
   * @brief          自定义大疆电机回调函数
@@ -285,6 +348,110 @@ void CAN_cmd_2FF(int16_t motor1, int16_t motor2, int16_t motor3, CAN_HandleTypeD
     HAL_CAN_AddTxMessage(hcan, &tx_meessage_2ff, can_send_data_2ff, &send_mail_box);
 }
 
+//控制四个vesc的发送 
+int8_t CAN_VESC_Control(int id,int speed ,CAN_HandleTypeDef*hcan)
+{
+ 
+  uint32_t send_mail_box;
+  int Byte[4];
+  Vesc_ByteGet raw[4];
+
+    //将期望的四个电机输出值分别对应到四个联合体 为了下面的拆分字节
+   raw[0].as_array = speed;
+   raw[1].as_array = speed;
+
+  for(int i=0 ; i < 2 ; i ++)
+  {			 
+
+    //将单个电机的期望输出值通过联合体拆分
+   Byte[0] = raw[i].byte.byte1;
+   Byte[1] = raw[i].byte.byte2;
+   Byte[2] = raw[i].byte.byte3;
+   Byte[3] = raw[i].byte.byte4;
+    
+   raw_tx.tx_header.ExtId = id+i+CAN_VESC_CTRL_ID_BASE;
+   raw_tx.tx_header.IDE = CAN_ID_EXT;
+   raw_tx.tx_header.RTR = CAN_RTR_DATA;
+   raw_tx.tx_header.DLC = CAN_MOTOR_TX_BUF_SIZE;
+
+   raw_tx.tx_data[0] = Byte[3];
+   raw_tx.tx_data[1] = Byte[2];
+   raw_tx.tx_data[2] = Byte[1];
+   raw_tx.tx_data[3] = Byte[0];
+   raw_tx.tx_data[4] = 0;
+   raw_tx.tx_data[5] = 0;
+   raw_tx.tx_data[6] = 0;
+   raw_tx.tx_data[7] = 0;
+
+   HAL_CAN_AddTxMessage(hcan, &raw_tx.tx_header, raw_tx.tx_data, &send_mail_box);
+  }
+
+return DEVICE_OK;
+
+}
+
+/**
+   * @brief          限制vesc电机转速
+   * @param[in/out]  rpm:  vesce电机转速
+   * @retval         none
+   *
+   * 如果rpm的绝对值大于wtrcfg_VESC_COMMAND_ERPM_MAX,
+   * 则将rpm的值设置为wtrcfg_VESC_COMMAND_ERPM_MAX或-wtrcfg_VESC_COMMAND_ERPM_MAX
+   */
+  void assert_param_rpm(float *rpm){
+    if( fabsf(*rpm) > wtrcfg_VESC_COMMAND_ERPM_MAX )
+      *rpm = *rpm > 0 ? wtrcfg_VESC_COMMAND_ERPM_MAX : - wtrcfg_VESC_COMMAND_ERPM_MAX ;
+  }
+  
+  /**
+    * @brief          使vesc电机进入转速模式
+    * @param[in]      controller_id: vesc电机控制器id
+    * @param[in]      RPM: 电机转速
+    * @retval         RPM（1000-50000之间的数）
+    */
+  void  CAN_VESC_RPM(uint8_t controller_id, float RPM)
+  {
+        uint32_t id;
+        int32_t data;
+        uint32_t send_mail_box;
+    
+        id = controller_id | ((uint32_t)CAN_PACKET_SET_RPM << 8);
+        //assert_param_rpm(&RPM);
+        data = (int32_t)(RPM);
+    
+        vesc_tx_message.ExtId = id;
+        vesc_tx_message.IDE = CAN_ID_EXT;
+        vesc_tx_message.RTR = CAN_RTR_DATA;
+        vesc_tx_message.DLC = 0x04;
+        vesc_send_data[0] = data >> 24 ;
+        vesc_send_data[1] = data >> 16 ;
+        vesc_send_data[2] = data >> 8 ;
+        vesc_send_data[3] = data ;
+    
+        HAL_CAN_AddTxMessage(&hcan2, &vesc_tx_message, vesc_send_data, &send_mail_box);
+  }
+  
+  
+  /**
+    * @brief          使vesc电机进入制动模式
+    * @param[in]      controller_id: vesc电机控制器id
+    * @retval         none
+    */
+  void  CAN_VESC_HEAD(uint8_t controller_id)
+  {
+        uint32_t id;
+        uint32_t send_mail_box;
+    
+        id = controller_id | ((uint32_t)CAN_PACKET_SET_CURRENT_BRAKE << 8);
+    
+        vesc_tx_message.ExtId = id;
+        vesc_tx_message.IDE = CAN_ID_EXT;
+        vesc_tx_message.RTR = CAN_RTR_DATA;
+        vesc_tx_message.DLC = 0x04;
+    
+        HAL_CAN_AddTxMessage(&hcan2, &vesc_tx_message, vesc_send_data, &send_mail_box);
+}
+
 /**
   * @brief          返回电机数据指针
   * @param[in]      i: 电机编号
@@ -295,106 +462,73 @@ void CAN_cmd_2FF(int16_t motor1, int16_t motor2, int16_t motor3, CAN_HandleTypeD
      return &motor_chassis[i];
  }
 
- int8_t CAN_VESC_Control(int id,int speed ,CAN_HandleTypeDef*hcan)
+ CAN_MotorFeedback_t *get_vesc_point(uint8_t i)
  {
-  
+    return &motor_5065[i];
-   uint32_t send_mail_box;
+ }
-   int Byte[4];
-   Vesc_ByteGet raw[4];
 
-     //将期望的四个电机输出值分别对应到四个联合体 为了下面的拆分字节
-    raw[0].as_array = speed;
-    raw[1].as_array = speed;
 
-   for(int i=0 ; i < 2 ; i ++)
+ //小米
-   {			 
+ void Parse_CAN_Response(uint8_t* rx_data, MotorFeedback_t* feedback) {
+  // 1. 解析位置
+  int16_t pos_code = (rx_data[1] << 8) | rx_data[2];
+  feedback->position_deg = ((int16_t)(pos_code - 0x8000) / 32768.0f) * 360.0f * Pmax;
 
-     //将单个电机的期望输出值通过联合体拆分
+  // 2. 解析速度
-    Byte[0] = raw[i].byte.byte1;
+  int16_t speed_code = ((rx_data[3] << 4) | (rx_data[4] >> 4)) - 0x800;
-    Byte[1] = raw[i].byte.byte2;
+  feedback->speed_rads = (speed_code / 2048.0f) * speed_full_scale;
-    Byte[2] = raw[i].byte.byte3;
-    Byte[3] = raw[i].byte.byte4;
-     
-    raw_tx.tx_header.ExtId = id+i+CAN_VESC_CTRL_ID_BASE;
-    raw_tx.tx_header.IDE = CAN_ID_EXT;
-    raw_tx.tx_header.RTR = CAN_RTR_DATA;
-    raw_tx.tx_header.DLC = CAN_MOTOR_TX_BUF_SIZE;
 
-    raw_tx.tx_data[0] = Byte[3];
+  // 3. 解析电流
-    raw_tx.tx_data[1] = Byte[2];
+  int16_t current_code = (((rx_data[4] & 0x0F) << 8) | rx_data[5]) - 0x800;
-    raw_tx.tx_data[2] = Byte[1];
+  feedback->current_A = (current_code / 2048.0f) * current_full_scale;
-    raw_tx.tx_data[3] = Byte[0];
-    raw_tx.tx_data[4] = 0;
-    raw_tx.tx_data[5] = 0;
-    raw_tx.tx_data[6] = 0;
-    raw_tx.tx_data[7] = 0;
-
-    HAL_CAN_AddTxMessage(hcan, &raw_tx.tx_header, raw_tx.tx_data, &send_mail_box);
-   }
-
-return DEVICE_OK;
 
+  // 记录电机ID
+  feedback->can_id = rx_data[0];
 }
 
 
- /**
+void CAN_XiaoMi(int id,JZ_xiaomi_t *JZ_xiaomi,CAN_HandleTypeDef*hcan)
-    * @brief          限制vesc电机转速
+{
-    * @param[in/out]  rpm:  vesce电机转速
+    uint32_t send_mail_box;
-    * @retval         none
+	  static int is_open=0;  //是否开启电机，根据手册，第一次使用需要ff开启
-    *
+    tx_message.StdId = id;
-    * 如果rpm的绝对值大于wtrcfg_VESC_COMMAND_ERPM_MAX,
+    tx_message.IDE = CAN_ID_STD;
-    * 则将rpm的值设置为wtrcfg_VESC_COMMAND_ERPM_MAX或-wtrcfg_VESC_COMMAND_ERPM_MAX
+    tx_message.RTR = CAN_RTR_DATA;
-    */
+    tx_message.DLC = 0x08;
-   void assert_param_rpm(float *rpm){
+	if(is_open){
-     if( fabsf(*rpm) > wtrcfg_VESC_COMMAND_ERPM_MAX )
+
-       *rpm = *rpm > 0 ? wtrcfg_VESC_COMMAND_ERPM_MAX : - wtrcfg_VESC_COMMAND_ERPM_MAX ;
+		uint16_t pos_code = (uint16_t)((JZ_xiaomi->position  / (Pmax * 360.0f)) * 32768.0f + 32768.0f);
-   }
+		uint16_t speed_code = (uint16_t)((JZ_xiaomi->speed  / 200.0f) * 2048.0f + 2048.0f);
-   
+		uint16_t current_code = (uint16_t)((JZ_xiaomi->K_C  / 4.0f) * 2048.0f + 2048.0f);
-   /**
+
-     * @brief          使vesc电机进入转速模式
+		can_send_data[0] = (pos_code >> 8) & 0xFF;    // 位置高8位
-     * @param[in]      controller_id: vesc电机控制器id
+		can_send_data[1] = pos_code & 0xFF;            // 位置低8位
-     * @param[in]      RPM: 电机转速
+
-     * @retval         RPM（1000-50000之间的数）
+		can_send_data[2] = (speed_code >> 4) & 0xFF;  // 速度高8位（取12位中的高8）
-     */
+		can_send_data[3] = ((speed_code & 0x0F) << 4) | ((JZ_xiaomi->K_P >> 8) & 0x0F); // 速度低4位 + Kp高4位
-   void  CAN_VESC_RPM(uint8_t controller_id, float RPM)
+
-   {
+		can_send_data[4] = JZ_xiaomi->K_P  & 0xFF;                 // Kp低8位
-         uint32_t id;
+
-         int32_t data;
+		can_send_data[5] = (JZ_xiaomi->K_D  >> 4) & 0xFF;           // Kd高8位（取12位中的高8）
-         uint32_t send_mail_box;
+		can_send_data[6] = ((JZ_xiaomi->K_D & 0x0F) << 4) | ((current_code >> 8) & 0x0F); // Kd低4位 + 电流高4位
-     
+
-         id = controller_id | ((uint32_t)CAN_PACKET_SET_RPM << 8);
+		can_send_data[7] = current_code & 0xFF;       // 电流低8位	
-         assert_param_rpm(&RPM);
+			
-         data = (int32_t)(RPM);
+	}
-     
+	else
-         vesc_tx_message.ExtId = id;
+	{
-         vesc_tx_message.IDE = CAN_ID_EXT;
+		can_send_data[0] = 0xFF;
-         vesc_tx_message.RTR = CAN_RTR_DATA;
+		can_send_data[1] = 0xFF;
-         vesc_tx_message.DLC = 0x04;
+		can_send_data[2] = 0xFF;
-         vesc_send_data[0] = data >> 24 ;
+		can_send_data[3] = 0xFF;
-         vesc_send_data[1] = data >> 16 ;
+		can_send_data[4] = 0xFF;
-         vesc_send_data[2] = data >> 8 ;
+		can_send_data[5] = 0xFF;
-         vesc_send_data[3] = data ;
+		can_send_data[6] = 0xFF;
-     
+		can_send_data[7] = 0xFC;	
-         HAL_CAN_AddTxMessage(&hcan2, &vesc_tx_message, vesc_send_data, &send_mail_box);
+		is_open=1;
-   }
+		}
-   
+
-   
+	
-   /**
+  	HAL_CAN_AddTxMessage(hcan, &tx_message, can_send_data, &send_mail_box);
-     * @brief          使vesc电机进入制动模式
+}
-     * @param[in]      controller_id: vesc电机控制器id
+
-     * @retval         none
-     */
-   void  CAN_VESC_HEAD(uint8_t controller_id)
-   {
-         uint32_t id;
-         uint32_t send_mail_box;
-     
-         id = controller_id | ((uint32_t)CAN_PACKET_SET_CURRENT_BRAKE << 8);
-     
-         vesc_tx_message.ExtId = id;
-         vesc_tx_message.IDE = CAN_ID_EXT;
-         vesc_tx_message.RTR = CAN_RTR_DATA;
-         vesc_tx_message.DLC = 0x04;
-     
-         HAL_CAN_AddTxMessage(&hcan2, &vesc_tx_message, vesc_send_data, &send_mail_box);
- }

User/device/djiMotor.h

@@ -10,12 +10,12 @@ extern "C"{
 #include "device.h"
 #include "can.h"
 #include <math.h>
-
+#include "string.h"
 
 typedef enum{
   GM6020 = 0,
   M3508 = 1,
-  M2006 = 2
+  M2006 = 2,
 }motor_type_e;
 
 typedef union{
@@ -44,6 +44,7 @@ enum{
   GM6020_2 = 0x208,
   CAN_VESC5065_M1_MSG1 =0x901,           //vesc的数据回传使用了扩展id，[0:7]为驱动器id，[8:15]为帧类型
 	CAN_VESC5065_M2_MSG1 =0x902,
+  CAN_VESC5065_M3_MSG1 =0x903,
 };
 
 //vesc指令
@@ -102,6 +103,8 @@ typedef struct
 #define MOTOR_ECD_TO_ANGLE_3508  (360.0 / 8191.0 / (3591.0/187.0))
 #define MOTOR_ECD_TO_ANGLE_6020  (360.0 / 8191.0 )
 #define MOTOR_ECD_TO_ANGLE_2006  (360.0 / 8191.0 / 36)
+#define CAN_MOTOR_ENC_RES (8191)  /* 电机编码器分辨率 */
+#define M_2PI 6.28318530717958647692f
 #if FREERTOS_DJI == 1
 
 /**
@@ -152,6 +155,7 @@ extern void CAN_cmd_2FF(int16_t motor1, int16_t motor2, int16_t motor3, CAN_Hand
   */
  extern motor_measure_t *get_motor_point(uint8_t i);
 
+ extern CAN_MotorFeedback_t *get_vesc_point(uint8_t i);
 /**
  * @brief           数据处理函数
  * @param[in]       none
@@ -167,6 +171,38 @@ void  CAN_VESC_HEAD(uint8_t controller_id);
 
 int8_t CAN_VESC_Control(int id,int speed ,CAN_HandleTypeDef*hcan);
 
+
+//小米
+
+typedef struct 
+{
+	int16_t position;
+	int16_t    speed;    
+	int16_t    K_P;
+	int16_t    K_D;
+	int16_t    K_C;
+	
+	int16_t    Pmax;//决定最大角度值，+-1 -> 最大+-360°
+	
+}JZ_xiaomi_t;
+
+typedef struct {
+	
+  uint8_t can_id;
+  float position_deg;
+  float speed_rads;
+  float current_A;
+	
+} MotorFeedback_t;
+
+
+
+void Parse_CAN_Response(uint8_t* rx_data, MotorFeedback_t* feedback) ;
+
+
+void CAN_XiaoMi(int id,JZ_xiaomi_t *JZ_xiaomi,CAN_HandleTypeDef*hcan);
+
+
 #ifdef __cplusplus
 }
 #endif

User/device/nuc.c

@@ -2,17 +2,19 @@
 #include <string.h>
 #include "crc_ccitt.h"
 
+static osThreadId_t thread_alert;
+
 uint8_t nucbuf[32];
 uint8_t packet[32]; // 假设最大数据包长度为 32 字节
 
 static void NUC_IdleCallback(void) {	
-	// osThreadFlagsSet(thread_alert,SIGNAL_NUC_RAW_REDY);
+	osThreadFlagsSet(thread_alert,SIGNAL_NUC_RAW_REDY);
-	// detect_hook(NUC_TOE);
+
 }
 
 int8_t NUC_Init(NUC_t *nuc){
     if(nuc == NULL) return DEVICE_ERR_NULL;
-  //信号量的东西 // if((thread_alert = osThreadGetId()) == NULL ) return DEVICE_ERR_NULL;
+    if((thread_alert = osThreadGetId()) == NULL ) return DEVICE_ERR_NULL;
    BSP_UART_RegisterCallback(BSP_UART_AI,BSP_UART_IDLE_LINE_CB,
                             NUC_IdleCallback);
 	
@@ -35,9 +37,9 @@ int8_t NUC_Restart(void) {
 
 
 bool_t NUC_WaitDmaCplt(void) {
-  // return (osThreadFlagsWait(SIGNAL_NUC_RAW_REDY, osFlagsWaitAll,500) ==
+  return (osThreadFlagsWait(SIGNAL_NUC_RAW_REDY, osFlagsWaitAll,500) ==
-  //         SIGNAL_NUC_RAW_REDY);
+          SIGNAL_NUC_RAW_REDY);
-  return 1;
+
 }
 
 int8_t NUC_SendPacket(void *data, uint16_t length) {
@@ -75,19 +77,12 @@ int8_t NUC_RawParse(NUC_t *n) {
   } instance; // 方便在浮点数和字符数组之间进行数据转换
 
   // 校验数据包头
-  if (nucbuf[0] != HEAD) {
+  if(nucbuf[0]!=HEAD) goto error;  //发送ID不是底盘
-      return DEVICE_ERR;
+  else
-  }
+  {
-  // 提取数据包中的 CRC 值（假设 CRC 值在数据包的最后两个字节）
-  uint16_t received_crc = (nucbuf[16] << 8) | nucbuf[17]; // 假设 CRC 在第 16 和 17 字节
-  uint16_t calculated_crc = do_crc_table(nucbuf, 16);     // 计算前 16 字节的 CRC
-  // 校验 CRC
-  if (received_crc == calculated_crc) {
-    
     n->status_fromnuc = nucbuf[1];
-  n->ctrl_status = nucbuf[2];
+    n->ctrl_status = nucbuf[2];
-
+ switch (n->status_fromnuc) {
-  switch (n->status_fromnuc) {
       case VISION:
           /* 协议格式 
              0xFF  HEAD
@@ -98,42 +93,103 @@ int8_t NUC_RawParse(NUC_t *n) {
              z    fp32
              0xFE  TAIL
           */
-          if (nucbuf[15] != TAIL) goto error;
+      if (nucbuf[15] != TAIL) goto error;
 
-          instance.data[3] = nucbuf[3];
+      instance.data[3] = nucbuf[3];
-          instance.data[2] = nucbuf[4];
+      instance.data[2] = nucbuf[4];
-          instance.data[1] = nucbuf[5];
+      instance.data[1] = nucbuf[5];
-          instance.data[0] = nucbuf[6];
+      instance.data[0] = nucbuf[6];
-          n->vision.x = instance.x[0];
+      n->vision.x = instance.x[0];
 
-          instance.data[7] = nucbuf[7];
+      instance.data[7] = nucbuf[7];
-          instance.data[6] = nucbuf[8];
+      instance.data[6] = nucbuf[8];
-          instance.data[5] = nucbuf[9];
+      instance.data[5] = nucbuf[9];
-          instance.data[4] = nucbuf[10];
+      instance.data[4] = nucbuf[10];
-          n->vision.y = instance.x[1];
+      n->vision.y = instance.x[1];
 
-          instance.data[11] = nucbuf[11];
+      instance.data[11] = nucbuf[11];
-          instance.data[10] = nucbuf[12];
+      instance.data[10] = nucbuf[12];
-          instance.data[9] = nucbuf[13];
+      instance.data[9] = nucbuf[13];
-          instance.data[8] = nucbuf[14];
+      instance.data[8] = nucbuf[14];
-          n->vision.z = instance.x[2];
+      n->vision.z = instance.x[2];
           break;
-
-
   }
-  return DEVICE_OK;
+   return DEVICE_OK;
- }
-  else 
-  {
-    return DEVICE_ERR;
-  
   }
 
+
 error:
   return DEVICE_ERR;
 }
 
 
+// int8_t NUC_RawParse(NUC_t *n) {
+//   if (n == NULL) return DEVICE_ERR_NULL;
+//   union {
+//       float x[3];
+//       char data[12];
+//   } instance; // 方便在浮点数和字符数组之间进行数据转换
+
+//   // 校验数据包头
+//   if (nucbuf[0] != HEAD) {
+//       return DEVICE_ERR;
+//   }
+//   // 提取数据包中的 CRC 值（假设 CRC 值在数据包的最后两个字节）
+//   uint16_t received_crc = (nucbuf[16] << 8) | nucbuf[17]; // 假设 CRC 在第 16 和 17 字节
+//   uint16_t calculated_crc = do_crc_table(nucbuf, 16);     // 计算前 16 字节的 CRC
+//   // 校验 CRC
+//   if (received_crc == calculated_crc) {
+    
+//     n->status_fromnuc = nucbuf[1];
+//   n->ctrl_status = nucbuf[2];
+
+//   switch (n->status_fromnuc) {
+//       case VISION:
+//           /* 协议格式 
+//              0xFF  HEAD
+//              0x02  控制帧
+//              0x01  相机帧
+//              x    fp32
+//              y    fp32
+//              z    fp32
+//              0xFE  TAIL
+//           */
+//           if (nucbuf[15] != TAIL) goto error;
+
+//           instance.data[3] = nucbuf[3];
+//           instance.data[2] = nucbuf[4];
+//           instance.data[1] = nucbuf[5];
+//           instance.data[0] = nucbuf[6];
+//           n->vision.x = instance.x[0];
+
+//           instance.data[7] = nucbuf[7];
+//           instance.data[6] = nucbuf[8];
+//           instance.data[5] = nucbuf[9];
+//           instance.data[4] = nucbuf[10];
+//           n->vision.y = instance.x[1];
+
+//           instance.data[11] = nucbuf[11];
+//           instance.data[10] = nucbuf[12];
+//           instance.data[9] = nucbuf[13];
+//           instance.data[8] = nucbuf[14];
+//           n->vision.z = instance.x[2];
+//           break;
+
+
+//   }
+//   return DEVICE_OK;
+//  }
+//   else 
+//   {
+//     return DEVICE_ERR;
+  
+//   }
+
+// error:
+//   return DEVICE_ERR;
+// }
+
+
 int8_t NUC_HandleOffline(NUC_t *cmd)
 {
 	if(cmd == NULL) return DEVICE_ERR_NULL;

User/device/nuc.h

@@ -4,6 +4,7 @@
 extern "C" {
 #endif
 
+#include <cmsis_os2.h>
 #include "struct_typedef.h"
 #include "device.h"
 #include "uart_it.h"

User/device/remote_control.h

@@ -22,13 +22,12 @@
 extern "C"{
 #endif
 
+#include "TopDefine.h"
 #include "struct_typedef.h"
 #include "usart.h"
 #include <string.h>
 #include <stdlib.h>
 
-#define DT7 0
-
 #if DT7==1
 
 #define DBUS_MAX_LEN     (50)//通过 UART 串口接收的最大数据长度

User/lib/filter.c

@@ -0,0 +1,184 @@
+/*
+  各类滤波器。
+*/
+
+#include "filter.h"
+
+#include "user_math.h"
+
+/**
+ * @brief 初始化滤波器
+ *
+ * @param f 滤波器
+ * @param sample_freq 采样频率
+ * @param cutoff_freq 截止频率
+ */
+void LowPassFilter2p_Init(LowPassFilter2p_t *f, float sample_freq,
+                          float cutoff_freq) {
+  if (f == NULL) return;
+
+  f->cutoff_freq = cutoff_freq;
+
+  f->delay_element_1 = 0.0f;
+  f->delay_element_2 = 0.0f;
+
+  if (f->cutoff_freq <= 0.0f) {
+    /* no filtering */
+    f->b0 = 1.0f;
+    f->b1 = 0.0f;
+    f->b2 = 0.0f;
+
+    f->a1 = 0.0f;
+    f->a2 = 0.0f;
+
+    return;
+  }
+  const float fr = sample_freq / f->cutoff_freq;
+  const float ohm = tanf(M_PI / fr);
+  const float c = 1.0f + 2.0f * cosf(M_PI / 4.0f) * ohm + ohm * ohm;
+
+  f->b0 = ohm * ohm / c;
+  f->b1 = 2.0f * f->b0;
+  f->b2 = f->b0;
+
+  f->a1 = 2.0f * (ohm * ohm - 1.0f) / c;
+  f->a2 = (1.0f - 2.0f * cosf(M_PI / 4.0f) * ohm + ohm * ohm) / c;
+}
+
+/**
+ * @brief 施加一次滤波计算
+ *
+ * @param f 滤波器
+ * @param sample 采样的值
+ * @return float 滤波后的值
+ */
+float LowPassFilter2p_Apply(LowPassFilter2p_t *f, float sample) {
+  if (f == NULL) return 0.0f;
+
+  /* do the filtering */
+  float delay_element_0 =
+      sample - f->delay_element_1 * f->a1 - f->delay_element_2 * f->a2;
+
+  if (isinf(delay_element_0)) {
+    /* don't allow bad values to propagate via the filter */
+    delay_element_0 = sample;
+  }
+
+  const float output = delay_element_0 * f->b0 + f->delay_element_1 * f->b1 +
+                       f->delay_element_2 * f->b2;
+
+  f->delay_element_2 = f->delay_element_1;
+  f->delay_element_1 = delay_element_0;
+
+  /* return the value. Should be no need to check limits */
+  return output;
+}
+
+/**
+ * @brief 重置滤波器
+ *
+ * @param f 滤波器
+ * @param sample 采样的值
+ * @return float 滤波后的值
+ */
+float LowPassFilter2p_Reset(LowPassFilter2p_t *f, float sample) {
+  if (f == NULL) return 0.0f;
+
+  const float dval = sample / (f->b0 + f->b1 + f->b2);
+
+  if (isfinite(dval)) {
+    f->delay_element_1 = dval;
+    f->delay_element_2 = dval;
+
+  } else {
+    f->delay_element_1 = sample;
+    f->delay_element_2 = sample;
+  }
+
+  return LowPassFilter2p_Apply(f, sample);
+}
+
+/**
+ * @brief 初始化滤波器
+ *
+ * @param f 滤波器
+ * @param sample_freq 采样频率
+ * @param notch_freq 中心频率
+ * @param bandwidth 带宽
+ */
+void NotchFilter_Init(NotchFilter_t *f, float sample_freq, float notch_freq,float bandwidth) 
+{
+  if (f == NULL) return;
+
+  f->notch_freq = notch_freq;
+  f->bandwidth = bandwidth;
+
+  f->delay_element_1 = 0.0f;
+  f->delay_element_2 = 0.0f;
+
+  if (notch_freq <= 0.0f) {
+    /* no filtering */
+    f->b0 = 1.0f;
+    f->b1 = 0.0f;
+    f->b2 = 0.0f;
+
+    f->a1 = 0.0f;
+    f->a2 = 0.0f;
+
+    return;
+  }
+
+  const float alpha = tanf(M_PI * bandwidth / sample_freq);
+  const float beta = -cosf(M_2PI * notch_freq / sample_freq);
+  const float a0_inv = 1.0f / (alpha + 1.0f);
+
+  f->b0 = a0_inv;
+  f->b1 = 2.0f * beta * a0_inv;
+  f->b2 = a0_inv;
+
+  f->a1 = f->b1;
+  f->a2 = (1.0f - alpha) * a0_inv;
+}
+
+/**
+ * @brief 施加一次滤波计算
+ *
+ * @param f 滤波器
+ * @param sample 采样的值
+ * @return float 滤波后的值
+ */
+inline float NotchFilter_Apply(NotchFilter_t *f, float sample) {
+  if (f == NULL) return 0.0f;
+
+  /* Direct Form II implementation */
+  const float delay_element_0 = sample - f->delay_element_1 * f->a1 - f->delay_element_2 * f->a2;
+  const float output = delay_element_0 * f->b0 + f->delay_element_1 * f->b1 +
+                       f->delay_element_2 * f->b2;
+
+  f->delay_element_2 = f->delay_element_1;
+  f->delay_element_1 = delay_element_0;
+
+  return output;
+}
+
+/**
+ * @brief 重置滤波器
+ *
+ * @param f 滤波器
+ * @param sample 采样的值
+ * @return float 滤波后的值
+ */
+float NotchFilter_Reset(NotchFilter_t *f, float sample) {
+  if (f == NULL) return 0.0f;
+
+  float dval = sample;
+
+  if (fabsf(f->b0 + f->b1 + f->b2) > FLT_EPSILON) {
+    dval = dval / (f->b0 + f->b1 + f->b2);
+  }
+
+  f->delay_element_1 = dval;
+  f->delay_element_2 = dval;
+
+  return NotchFilter_Apply(f, sample);
+}

User/lib/filter.h

@@ -0,0 +1,104 @@
+/*
+  各类滤波器。
+*/
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "user_math.h"
+
+/* 二阶低通滤波器 */
+typedef struct {
+  float cutoff_freq; /* 截止频率 */
+
+  float a1;
+  float a2;
+
+  float b0;
+  float b1;
+  float b2;
+
+  float delay_element_1;
+  float delay_element_2;
+
+} LowPassFilter2p_t;
+
+/* 带阻滤波器 */
+typedef struct {
+  float notch_freq; /* 阻止频率 */
+  float bandwidth;  /* 带宽 */
+
+  float a1;
+  float a2;
+
+  float b0;
+  float b1;
+  float b2;
+  float delay_element_1;
+  float delay_element_2;
+
+} NotchFilter_t;
+
+/**
+ * @brief 初始化滤波器
+ *
+ * @param f 滤波器
+ * @param sample_freq 采样频率
+ * @param cutoff_freq 截止频率
+ */
+void LowPassFilter2p_Init(LowPassFilter2p_t *f, float sample_freq,
+                          float cutoff_freq);
+
+/**
+ * @brief 施加一次滤波计算
+ *
+ * @param f 滤波器
+ * @param sample 采样的值
+ * @return float 滤波后的值
+ */
+float LowPassFilter2p_Apply(LowPassFilter2p_t *f, float sample);
+
+/**
+ * @brief 重置滤波器
+ *
+ * @param f 滤波器
+ * @param sample 采样的值
+ * @return float 滤波后的值
+ */
+float LowPassFilter2p_Reset(LowPassFilter2p_t *f, float sample);
+
+/**
+ * @brief 初始化滤波器
+ *
+ * @param f 滤波器
+ * @param sample_freq 采样频率
+ * @param notch_freq 中心频率
+ * @param bandwidth 带宽
+ */
+void NotchFilter_Init(NotchFilter_t *f, float sample_freq, float notch_freq,
+                      float bandwidth);
+
+/**
+ * @brief 施加一次滤波计算
+ *
+ * @param f 滤波器
+ * @param sample 采样的值
+ * @return float 滤波后的值
+ */
+float NotchFilter_Apply(NotchFilter_t *f, float sample);
+
+/**
+ * @brief 重置滤波器
+ *
+ * @param f 滤波器
+ * @param sample 采样的值
+ * @return float 滤波后的值
+ */
+float NotchFilter_Reset(NotchFilter_t *f, float sample);
+
+#ifdef __cplusplus
+}
+#endif

User/lib/kalman.c

@@ -0,0 +1,65 @@
+/**    
+  * @author  Liu heng
+  * 一阶卡尔曼滤波器来自RoboMaster论坛  
+  *   一维卡尔曼滤波器                     
+  *   使用时先定义一个kalman指针，然后调用kalmanCreate()创建一个滤波器 
+  *   每次读取到传感器数据后即可调用KalmanFilter()来对数据进行滤波
+  *          使用示例                                             
+  *          extKalman p;                  //定义一个卡尔曼滤波器结构体                                                 
+  *          float SersorData;             //需要进行滤波的数据                                          
+  *          KalmanCreate(&p,20,200);      //初始化该滤波器的Q=20 R=200参数                                                  
+  *          while(1)                                                                
+  *          {                                                                            
+  *             SersorData = sersor();                     //获取数据                                           
+  *             SersorData = KalmanFilter(&p,SersorData);  //对数据进行滤波                                                                            
+  *          }                                                                            
+  */
+
+#include "kalman.h"
+
+/**
+  * @name   kalmanCreate
+  * @brief  创建一个卡尔曼滤波器
+  * @param  p:  滤波器
+  *         T_Q:系统噪声协方差
+  *         T_R:测量噪声协方差
+  *         
+  * @retval none
+  * @attention R固定，Q越大，代表越信任侧量值，Q无穷代表只用测量值
+  *		       	反之，Q越小代表越信任模型预测值，Q为零则是只用模型预测
+  */
+void KalmanCreate(extKalman_t *p,float T_Q,float T_R)
+{
+    p->X_last = (float)0;
+    p->P_last = 0;
+    p->Q = T_Q;
+    p->R = T_R;
+    p->A = 1;
+	  p->B = 0;
+    p->H = 1;
+    p->X_mid = p->X_last;
+}
+
+/**
+  * @name   KalmanFilter
+  * @brief  卡尔曼滤波器
+  * @param  p:  滤波器
+  *         dat:待滤波数据
+  * @retval 滤波后的数据
+  * @attention Z(k)是系统输入,即测量值   X(k|k)是卡尔曼滤波后的值,即最终输出
+  *            A=1 B=0 H=1 I=1  W(K)  V(k)是高斯白噪声,叠加在测量值上了,可以不用管
+  *            以下是卡尔曼的5个核心公式
+  *            一阶H'即为它本身,否则为转置矩阵
+  */
+
+float KalmanFilter(extKalman_t* p,float dat)
+{
+    p->X_mid =p->A*p->X_last;                     //百度对应公式(1)    x(k|k-1) = A*X(k-1|k-1)+B*U(k)+W(K)
+    p->P_mid = p->A*p->P_last+p->Q;               //百度对应公式(2)    p(k|k-1) = A*p(k-1|k-1)*A'+Q
+    p->kg = p->P_mid/(p->P_mid+p->R);             //百度对应公式(4)    kg(k) = p(k|k-1)*H'/(H*p(k|k-1)*H'+R)
+    p->X_now = p->X_mid+p->kg*(dat-p->X_mid);     //百度对应公式(3)    x(k|k) = X(k|k-1)+kg(k)*(Z(k)-H*X(k|k-1))
+    p->P_now = (1-p->kg)*p->P_mid;                //百度对应公式(5)    p(k|k) = (I-kg(k)*H)*P(k|k-1)
+    p->P_last = p->P_now;                         //状态更新
+    p->X_last = p->X_now;
+    return p->X_now;							  //输出预测结果x(k|k)
+}

User/lib/kalman.h

@@ -0,0 +1,28 @@
+/**
+  * @author  Liu heng
+  * 卡尔曼滤波器来自RoboMaster论坛  
+  */
+  
+#ifndef _KALMAN_H
+#define _KALMAN_H
+
+
+typedef struct {
+    float X_last; //上一时刻的最优结果  X(k-|k-1)
+    float X_mid;  //当前时刻的预测结果  X(k|k-1)
+    float X_now;  //当前时刻的最优结果  X(k|k)
+    float P_mid;  //当前时刻预测结果的协方差  P(k|k-1)
+    float P_now;  //当前时刻最优结果的协方差  P(k|k)
+    float P_last; //上一时刻最优结果的协方差  P(k-1|k-1)
+    float kg;     //kalman增益
+    float A;      //系统参数
+	  float B;
+    float Q;
+    float R;
+    float H;
+}extKalman_t;
+
+void KalmanCreate(extKalman_t *p,float T_Q,float T_R);
+float KalmanFilter(extKalman_t* p,float dat);
+
+#endif

User/lib/user_math.c

@@ -112,3 +112,24 @@ inline float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm) {
   /* 不为裁判系统设定值时,计算转速 */
   return 60.0f * (float)bullet_speed / (M_2PI * fric_radius);
 }
+
+float motor_angle_change(uint32_t angle, uint16_t offset_angle)
+{
+    int32_t relative_angle = angle - offset_angle;
+    if (relative_angle > 180)
+    {
+      while(relative_angle>180)
+			{
+				relative_angle -= 360;
+			}
+    }
+    else if (relative_angle < -180)
+    { 
+			while(relative_angle<-180)
+			{
+				relative_angle += 360;
+			}
+    }
+    return relative_angle;
+		
+}

User/lib/user_math.h

@@ -15,6 +15,7 @@ extern "C" {
 #include <math.h>
 #include <stdbool.h>
 
+
 #define M_DEG2RAD_MULT (0.01745329251f)
 #define M_RAD2DEG_MULT (57.2957795131f)
 
@@ -101,6 +102,8 @@ void CircleReverse(float *origin);
  */
 float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm);
 
+float motor_angle_change(uint32_t ecd, uint16_t offset_ecd);
+
 #ifdef __cplusplus
 }
 #endif

User/module/ball.cpp

@@ -2,27 +2,34 @@
 #include "bsp_delay.h"
 #include "remote_control.h"
 #include "detect.h"
-
+#include "userTask.h"
+#include "user_math.h"
 extern RC_ctrl_t rc_ctrl;
 extern int key;
 
-// 定义状态机变量
+// C键 sw[5]👆 1800   中1000  👇200
-BallState_t currentState1 = BALL_IDLE; // 当前状态
+// D键 sw[6]👆 200   👇1800
-uint32_t startTime = 0;             // 用于记录延时的起始时间
+
+#define M_SPEED 4000
+
+#if HANDLING3 == 1
+//三摩擦轮运球！！！
+//添加平移3508 得跑位置吧
+
+const fp32 Ball:: M3508_speed_PID[3] = {15, 0.03, 0};
+const fp32 Ball:: Extend_speed_PID[3] = { 25, 0, 0.};
+const fp32 Ball:: Extend_angle_PID[3]=  { 60, 0, 0.1};
+
+//摩擦轮转速
 int speedm=0;
-int speedm1=0;
-
-#define MOTOR_SPEED 1000
-
-const fp32 Ball:: M3508_speed_PID[3] = {5, 0.01, 0};
 
 //PE11 气缸
 
-//三摩擦轮运球！！！
-
 Ball ::Ball()
 {  
    detect_init();
+
+ //三摩擦轮
    for(int i = 0;i < MOTOR_NUM;i ++)
  {
     hand_Motor[i] = get_motor_point(i);  
@@ -34,12 +41,69 @@ Ball ::Ball()
     result[i] = 0;
     speedSet[i] = 0;    
  }
-     
+
+  //两个伸缩6020
+   Extern_Motor[0] = get_motor_point(6);
+   Extern_Motor[1] = get_motor_point(7);
+
+   Extern_Motor[0]->type = GM6020;
+   Extern_Motor[1]->type = GM6020;
+   PID_init(&extend_speed_pid[0],PID_POSITION,Extend_speed_PID,25000, 2000);
+   PID_init(&extend_angle_pid[0],PID_POSITION,Extend_angle_PID,25000, 1000);
+
+   PID_init(&extend_speed_pid[1],PID_POSITION,Extend_speed_PID,25000, 2000);
+   PID_init(&extend_angle_pid[1],PID_POSITION,Extend_angle_PID,25000, 1000);
+
+   e_result[0] = 0;
+   e_result[1] = 0;
+   angleSet[0] = 0;
+   angleSet[1] = 0;
+
+    //小米电机初始化
+    JZ_xiaomi.position = 0;    // 
+    JZ_xiaomi.speed    = 20;   // 
+    JZ_xiaomi.K_P      = 100;  // 
+    JZ_xiaomi.K_D      =20;    // 
+    JZ_xiaomi.K_C      = 2 ;  
+    JZ_xiaomi.Pmax     =1;
+
+  //状态机状态初始化
+  currentState1= BALL_IDLE;
+
+
 }
 
-#if HANDLING3 == 1
 
-void Ball ::Spin(float speed,float speed2)
+void Ball ::Extend_control(int angle)
+{
+    int16_t delta[2];
+    angleSet[0] = angle;
+    angleSet[1] = -angle;
+
+    delta[0] = PID_calc(&extend_angle_pid[0],Extern_Motor[0]->total_angle , angleSet[0]);
+    e_result[0] = PID_calc(&extend_speed_pid[0], Extern_Motor[0]->speed_rpm, delta[0]);
+
+    delta[1] = PID_calc(&extend_angle_pid[1], Extern_Motor[1]->total_angle , angleSet[1]);
+    e_result[1] = PID_calc(&extend_speed_pid[1], Extern_Motor[1]->speed_rpm, delta[1]);
+
+    // delta[1] = PID_calc(&extend_angle_pid, angle_get[1], angleSet[1]);
+    // e_result[1] = PID_calc(&extend_speed_pid, Extern_Motor[1]->speed_rpm, delta[1]);
+
+    // angle_get[0]=motor_angle_change(Extern_Motor[0]->real_round, angleSet[0]);
+    // angle_get[1]=motor_angle_change(Extern_Motor[1]->real_round, angleSet[1]);
+
+
+    // delta[0] = PID_calc(&extend_angle_pid, angle_get[0], angleSet[0]);
+    // e_result[0] = PID_calc(&extend_speed_pid, Extern_Motor[0]->speed_rpm, delta[0]);
+
+    // delta[1] = PID_calc(&extend_angle_pid, angle_get[1], angleSet[1]);
+    // e_result[1] = PID_calc(&extend_speed_pid, Extern_Motor[1]->speed_rpm, delta[1]);
+
+
+}
+
+
+void Ball ::Spin(float speed)
 {
 
     speedSet[MOTOR_1] = -speed;
@@ -48,164 +112,120 @@ void Ball ::Spin(float speed,float speed2)
     speedSet[MOTOR_2] = speed;
     result[MOTOR_2] = PID_calc(&speed_pid[MOTOR_2],hand_Motor[MOTOR_2]->speed_rpm,speedSet[MOTOR_2]);
 
-    speedSet[MOTOR_3] = speed2;
+    speedSet[MOTOR_3] = speed;
     result[MOTOR_3] = PID_calc(&speed_pid[MOTOR_3],hand_Motor[MOTOR_3]->speed_rpm,speedSet[MOTOR_3]);
 
 }
 
-	int flag =0;
+void Ball::ballDown(void)
-	int ball_state = 0;
+{
-   
+     HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET);//确保闭合气缸
+     speedm=-500;
+
+}
+
+void Ball::ballStop(void)
+{
+    HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保闭合气缸
+     speedm=0;
+
+}
+
+int ball_have = 0;
+void Ball::ballTake(void)
+{
+    ball_have=HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin);//有球 1 无球 0
+      HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET);//确保闭合气缸
+      speedm=2000;
+      if(ball_have==1)
+      {
+          speedm=0;
+          HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);//确保闭合气缸
+      }
+}
+
+ 
 void Ball::ballHadling(void)
 {
+    static int last_sw5 = 0; // 保存上一次拨杆状态
+    ball_state = HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin); // 有球 1 无球 0
 
-    ball_state =HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin);//有球 0 无球 1
     switch (currentState1)
     {
     case BALL_IDLE:
-       HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);//确保闭合气缸
+        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保闭合气缸
-        if (key > 0) // 检测按键是否被按下
+        // 只在拨杆从非200切到200时触发
+        if ((rc_ctrl.sw[5] == 200 && last_sw5 != 200) || key > 0)
         {
-            speedm=-4500;
+            speedm = -4000;
-            speedm1=-4500;
             currentState1 = BALL_FORWARD; // 切换到正转状态
         }
         break;
 
     case BALL_FORWARD:
-
+        if (hand_Motor[MOTOR_1]->speed_rpm >= 3980 && hand_Motor[MOTOR_1]->speed_rpm <= 4020 &&
-        if (  hand_Motor[MOTOR_1]->speed_rpm >= 4480&&hand_Motor[MOTOR_1]->speed_rpm <= 4530 &&
+            hand_Motor[MOTOR_2]->speed_rpm <= -3980 && hand_Motor[MOTOR_2]->speed_rpm >= -4020 &&
-              hand_Motor[MOTOR_2]->speed_rpm <= -4480&&hand_Motor[MOTOR_2]->speed_rpm >= -4530 &&          
+            hand_Motor[MOTOR_3]->speed_rpm <= -3980 && hand_Motor[MOTOR_3]->speed_rpm >= -4020)
-              hand_Motor[MOTOR_3]->speed_rpm <= -4480&&hand_Motor[MOTOR_3]->speed_rpm >= -4530 )
         {
-            HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET);// 打开气缸
+            HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET); // 打开气缸
             currentState1 = BALL_DROP; // 切换到球下落状态
         }
         break;
 
     case BALL_DROP:
-
+        if (ball_state == 0) // 读光电 有球 1  无球 0
-        if (ball_state == 1) //读光电 有球 0  无球 1
         {
             osDelay(200); // 延时200ms
-            speedm=4500;
+            speedm = 3500;
-            speedm1=4500;
             currentState1 = BALL_REVERSE; // 切换到反转状态
         }
-        
         break;
 
     case BALL_REVERSE:
-       
+        if (ball_state == 1)
-        if (ball_state == 0) 
         {
-            
+            speedm = 0; // 停止电机
-            //flag=2;   //抽象的计次
-            speedm=0; // 停止电机
-            speedm1=0;
             currentState1 = BALL_CLOSE; // 切换到完成状态
         }
-     
+        break;
-     break;
 
     case BALL_CLOSE:
-       // osDelay(200); // 延时50ms
+        if (ball_state == 1)
-       if(ball_state == 0)
+        {
-       {
+            HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);
-           HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);
+            currentState1 = BALL_FINISH; // 切换到完成状态
-           currentState1 = BALL_FINISH; // 切换到完成状态
+        }
-       }
+        break;
-        // if (flag == 2) 
-        // {
-        //     if(ball_state == 0)
-        //     {
-        //         HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);
-        //         currentState1 = BALL_FINISH; // 切换到完成状态
-        //     }
-          
-        // }
-        break;    
 
     case BALL_FINISH:
-        osDelay(200); // 延时50ms
+        osDelay(200); // 延时200ms
-        key=0; // 重置按键状态
+        key = 0; // 重置按键状态
-        flag=0;
+        speedm = 0;
-        speedm=0;
+        osThreadFlagsSet(task_struct.thread.shoot, BALL_OK);
-        speedm1=0;
+        currentState1 = BALL_IDLE; // 直接回到空闲状态
-        currentState1 = BALL_IDLE; // 回到空闲状态
         break;
 
     default:
-       currentState1 = BALL_IDLE; // 默认回到空闲状态
+        currentState1 = BALL_IDLE; // 默认回到空闲状态
         break;
     }
 
+    last_sw5 = rc_ctrl.sw[5]; // 更新上一次拨杆状态
 }
 
 void Ball::Send_control()
 {
     CAN_cmd_200(result[MOTOR_1],result[MOTOR_2],result[MOTOR_3],0,&hcan1);
+    osDelay(1);
+
+    CAN_cmd_1FF(0,0,e_result[0],e_result[1],&hcan1);
     osDelay(2);
 
 }
 
 #else
-int flag =0;
-int ball_state = 0;
-// void Ball::ballHadling(void)
-// {
-//     ball_state =HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin);//有球 0 无球 1
-//     switch (currentState1)
-//     {
-//     case BALL_IDLE:
-    
-//     if (key > 0) // 检测按键是否被按下
-//      {
-//         currentState1 = BALL_FORWARD; // 切换到正转状态
-        
-//      }
-//         break;
 
-//     case BALL_FORWARD:
+ int ball_state = 0;
-//     HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_SET);
-//     osDelay(50); 
-//     HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_SET);
-
-//     currentState1 = BALL_DROP; // 切换到球下落状态
-//         break;
-
-//     case BALL_DROP:
-//     osDelay(100); // 
-//     HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);
-//     currentState1 = BALL_CLOSE; // 切换到反转状态
-//      break;
-
-//     case BALL_CLOSE:
-//         osDelay(100);
-//         if(ball_state == 0)
-//         {
-//            HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET); 
-//            currentState1 = BALL_REVERSE; // 切换到反转状态 
-//         }
-     
-     
-
-//     break;
-
-//     case BALL_REVERSE:
-//      osDelay(50); // 延时200ms
-//      HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET);
-//      HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);
-//      key=0; // 重置按键状态
-//      currentState1 = BALL_IDLE; // 回到空闲状态
-//      break;
-    
-//      default:
-//       currentState1 = BALL_IDLE; // 默认回到空闲状态
-//       break;
-//   }
-    
-// }    
  int triggerCount = 0; // 光电传感器触发计数
  int last_ball_state = 1; // 上一次的光电状态
 void Ball::ballHadling(void)

User/module/ball.hpp

@@ -1,13 +1,14 @@
 #ifndef BALL_HPP
 #define BALL_HPP
 #include "main.h"
+#include "TopDefine.h"
 #include "cmsis_os.h"
 #include "FreeRTOS.h"
+#include <cmsis_os2.h>
 #include "bsp_delay.h"
 #include "djiMotor.h"
 #include "pid.h"
-
+#include "filter.h"
-#define HANDLING3  1
 
 // 定义状态枚举
 typedef enum {
@@ -17,7 +18,12 @@ typedef enum {
     BALL_REVERSE,      // 反转状态
     BALL_FLAG,
     BALL_CLOSE,         // 关闭状态
-    BALL_FINISH        // 完成状态
+    BALL_FINISH,        // 完成状态
+
+  //持球状态
+    BALL_TAKE,
+    BALL_LOSE
+
 } BallState_t;
 
 typedef enum
@@ -29,6 +35,12 @@ typedef enum
     MOTOR_NUM
 }motorhangd_e;
 
+  /* 滤波器 */
+  typedef struct {
+    LowPassFilter2p_t *in;  /* 反馈值滤波器 */
+    LowPassFilter2p_t *out; /* 输出值滤波器 */
+  } Filter;
+
 // 定义光电传感器检测宏
 #define IS_PHOTOELECTRIC_BALL() (HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin) == GPIO_PIN_RESET)
 
@@ -36,27 +48,50 @@ class Ball
 {
 public:
     Ball();
-    void Spin(float speed,float speed2);
+    void Spin(float speed);
     void ballHadling(void);
+    void ballDown(void);
     void Send_control(void);
-    void ballStateMachine(void);
+    void ballStop(void);
+    void ballTake(void);
+    void Extend_control(int angle);
+  
+    BallState_t currentState1; // 当前状态
+    int flag;//暂时还没用到
+	  int ball_state ;//光电检测
+    
+    //小米电机伸缩
+    JZ_xiaomi_t JZ_xiaomi;
+
+    //伸缩6020
+    int16_t e_result[2];
+    motor_measure_t *  Extern_Motor[2];
 
     int16_t result[MOTOR_NUM];
     motor_measure_t *hand_Motor[MOTOR_NUM];
-private:
+//==========================公共变量==========================//
+    //用于传接球,运球后通知
+    volatile BallState_t ballStatus;//是否有球
+    volatile uint32_t flag_thread;//接收传回的线程通知
+
+private:  
 
-    int up_ball_state; //上球状态 0:无球 1:有球
-  
     //三个摩擦轮
     static const float M3508_speed_PID[3];
     static const float M3508_angle_PID[3];
 
+    static const float Extend_speed_PID[3];
+    static const float Extend_angle_PID[3]; 
+
     //电机速度pid结构体
     pid_type_def speed_pid[MOTOR_NUM];
     //位置环pid
     pid_type_def angle_pid[MOTOR_NUM];
 
-    float angleSet;
+    pid_type_def extend_speed_pid[2];
+    pid_type_def extend_angle_pid[2];
+
+    float angleSet[2];
     float speedSet[MOTOR_NUM];
 };
 

User/module/gimbal.cpp

@@ -10,7 +10,6 @@
 //可活动角度
 #define ANGLE_ALLOW 1.0f
 extern RC_ctrl_t rc_ctrl;
-int angle1 = 0;
 NUC_t nuc;
 
 const fp32 Gimbal:: Gimbal_speed_PID[3] = {50, 0.1,  0};
@@ -19,20 +18,20 @@ const fp32 Gimbal:: Gimbal_angle_PID[3]=  { 5, 0.01, 0};
 #if GM6020ING ==1
 Gimbal::Gimbal()
 {
-  GM6020_Motor = get_motor_point(6);
+  // GM6020_Motor = get_motor_point(6);
-  GM6020_Motor->type = GM6020;
+  // GM6020_Motor->type = GM6020;
-  PID_init(&speed_pid,PID_POSITION,Gimbal_speed_PID,16000, 6000);
+  // PID_init(&speed_pid,PID_POSITION,Gimbal_speed_PID,16000, 6000);
-  PID_init(&angle_pid,PID_POSITION,Gimbal_angle_PID,5000, 2000);
+  // PID_init(&angle_pid,PID_POSITION,Gimbal_angle_PID,5000, 2000);
 
-  result = 0;
+  // result = 0;
-  angleSet = 0;
+  // angleSet = 0;
 
 }
 
 void Gimbal::gimbalFlow()
 {
   int16_t delta[1];
-  angleSet = angle1;
+  //angleSet = angle1;
   delta[0] = PID_calc(&angle_pid,GM6020_Motor->total_angle,angleSet);
   result = PID_calc(&speed_pid, GM6020_Motor->speed_rpm, delta[0]);
 

User/module/gimbal.hpp

@@ -6,7 +6,6 @@
 #include "pid.h"
 #include "nuc.h"
 
-#define GM6020ING 1
 class Gimbal
 {
 public:

User/module/shoot.cpp

@@ -5,103 +5,147 @@
 #include "FreeRTOS.h"
 #include <cmsis_os2.h>
 #include "calc_lib.h"
+#include "vofa.h"
+
 extern RC_ctrl_t rc_ctrl;
-#define SHOOT_SPEED 40000
+NUC_t nuc_v;
-#define SHOOT_SPEED_BACK -4000
+
+float vofa[8];
+#define SHOOT_SPEED 30500
+#define SHOOT_SPEED_BACK -2500
+#define Error 50
+
+int triggerspeed = -5000; // 扳机速度
+int test_speed =35200;
+float test_distance =1;
+float  xxx;
+
 #define STOP 0
 
-// 定义状态机变量
+int Torque = -5000; // 扳机恒力
-ShootState_t currentState = SHOOT_IDLE;
+#define Trigger_Torque -5000
+#define Trigger_Slow 2000//扳机慢速 不用加负号
+
+//sw[7]👆 1694  中 1000 👇306
+//sw[2]👆 1694   👇306
+
+//E键 sw[1]👆 200 shoot  中 1000 stop 👇1800  error
+//F键 sw[0]👆 1800 开   👇200 关
+//B键 sw[3]👆 200 开   👇1800 关
+
+const fp32 Shoot:: M2006_speed_PID[3] = {50, 0, 0};
+int t=0;
 
 Shoot::Shoot()
 {
-    // motor_5065[0] = get_5065_motor_point(0);//获取电机数据指针
+    //扳机初始化
-    // motor_5065[1] = get_5065_motor_point(1);//获取电机数据指针
+    trigger_Motor= get_motor_point(4);
-    CAN_VESC_RPM(1, STOP); 
+    trigger_Motor->type=M2006;
-    CAN_VESC_RPM(2, STOP); 
+    PID_init(&speed_pid,PID_POSITION,M2006_speed_PID,8000, 1000);//pid初始化
-	
+    t_speedSet = 0; 
+    result = 0;
+   
+    //发射摩擦轮
+    motor_5065[0] = get_vesc_point(0);//获取电机数据指针
+    motor_5065[1] = get_vesc_point(1);//获取电机数据指针
+    speed_5065=0;
+    currentState= SHOOT_IDLE;
 }
 
-void Shoot::shootThree()
+void Shoot::trigger_control()
 {
-    if((rc_ctrl.sw[1]>500))
+   // result = PID_calc(&speed_pid,trigger_Motor->speed_rpm,t_speedSet);
-    {
+    CAN_cmd_1FF(result,0,0,0,&hcan1);
-        speed_5065 = map((float)rc_ctrl.sw[1],500,1694,30000,45000);
-        
-        // CAN_VESC_RPM(1, speed_5065); 
-        // CAN_VESC_RPM(2, speed_5065); 
-    }
-    else
-    {
-        speed_5065=STOP;
-       // CAN_VESC_Control(1,STOP,&hcan2);
-        // CAN_VESC_RPM(1, STOP); 
-        // CAN_VESC_RPM(2, STOP); 
-    }
-    if(rc_ctrl.sw[5]==1694)
-    {
-        speed_5065=SHOOT_SPEED_BACK;
-        // CAN_VESC_Control(1,SHOOT_SPEED_BACK,&hcan2);
-        // CAN_VESC_RPM(1, SHOOT_SPEED_BACK); 
-        // CAN_VESC_RPM(2, SHOOT_SPEED_BACK);
-        
-    }
-    //CAN_VESC_Control(1,speed_5065,&hcan2);
-    CAN_VESC_RPM(1, speed_5065); 
-    CAN_VESC_RPM(2, speed_5065); 
-}
-
-void Shoot::shootBack()
-{
-    if(rc_ctrl.sw[5]==1694)
-    {
-        
-        CAN_VESC_RPM(1, SHOOT_SPEED_BACK); 
-        CAN_VESC_RPM(2, SHOOT_SPEED_BACK);
-    }
-
 }
 
 void Shoot::shootStop()
 {
-    CAN_VESC_HEAD(1); 
+      speed_5065=STOP;   
-    CAN_VESC_HEAD(2); 
+      result=STOP;//扳机慢速归位   
-  
+    
+
 }
 
-void Shoot::shootControl()
+void Shoot::errorControl()
 {
-        shootThree();
+        result=Trigger_Slow;//扳机慢速归位
-  
+        speed_5065=SHOOT_SPEED_BACK; //摩擦归位     
+       
+    
 }
 
+
+void Shoot :: distanceGet(const NUC_t &nuc_v)
+{
+
+    test_distance=nuc_v.vision.x; // 获取自瞄数据
+
+}
+
+void Shoot::vesc_send()
+{
+    CAN_VESC_Control(1, speed_5065, &hcan2);
+}
+
+void Shoot::vofaWatch()
+{
+    vofa[0] = motor_5065[1]->rotor_speed;
+    vofa[1] = motor_5065[0]->rotor_speed; // 发送电机速度数据
+    vofa[2] = speed_5065; // 发送电机速度数据
+    vofa[4] = trigger_Motor->speed_rpm; // 发送扳机电机速度数据
+    vofa_tx_main(vofa); // 发送数据到虚拟串口
+}
+
+
 void Shoot::shootStateMachine() {
+    
+     xxx=polynomial(test_distance);
+     distance =test_distance;
     switch (currentState) {
         case SHOOT_IDLE: {
+            speed_5065=STOP;
+            t_speedSet=STOP;
             // 空闲状态，等待遥控器输入
-            if (rc_ctrl.sw[1] > 500) {
+            if((rc_ctrl.sw[1]==200)) {
-                currentState = SHOOT_FIRE; // 切换到发射状态
+                currentState = SHOOT_READY; // 切换到发射状态
             }
             break;
         }
 
+        case SHOOT_READY:
+        {
+           // 发射状态，控制电机发射
+           //speed_5065 = test_speed;
+            speed_5065 =polynomial(distance);
+        if(motor_5065[0]->rotor_speed>=speed_5065-Error && motor_5065[0]->rotor_speed<=speed_5065+Error &&
+		   motor_5065[1]->rotor_speed>=speed_5065-Error && motor_5065[1]->rotor_speed<=speed_5065+Error)
+		{
+             if((rc_ctrl.sw[3]==1800)) {
+                currentState = SHOOT_FIRE; // 切换到发射状态
+            }
+		
+		}
+        break;
+
+        }
+
         case SHOOT_FIRE: {
-            // 发射状态，控制电机发射
-            speed_5065 = map((float)rc_ctrl.sw[1], 500, 1694, 30000, 45000);
-            CAN_VESC_RPM(1, speed_5065); 
-            CAN_VESC_RPM(2, speed_5065); 
 
+            t_speedSet=triggerspeed;// 扳机速度影响？
             // 检测光电传感器是否触发
             if (IS_PHOTOELECTRIC_TRIGGERED()) {
-                currentState = SHOOT_TRIGGERED; // 切换到光电触发状态
+                currentState = SHOOT_BACK; // 切换到光电触发状态
             }
             break;
         }
 
-        case SHOOT_TRIGGERED: {
+
+
+        case SHOOT_BACK: {
             // 光电触发状态，停止发射
-            CAN_VESC_HEAD(1); 
+             t_speedSet=STOP;
-            CAN_VESC_HEAD(2);
+             speed_5065=STOP;
+            
             // 切换到返回状态
             currentState = SHOOT_RETURN;
             break;
@@ -109,11 +153,13 @@ void Shoot::shootStateMachine() {
 
         case SHOOT_RETURN: {
             // 自动返回状态，控制电机反转
-            CAN_VESC_RPM(1, SHOOT_SPEED_BACK); 
+            speed_5065=SHOOT_SPEED_BACK;
-            CAN_VESC_RPM(2, SHOOT_SPEED_BACK);
+            t_speedSet=2000;
 
             // 检测返回完成（可以通过时间或其他传感器判断）
-            if (rc_ctrl.sw[1] < 500) { // 假设遥控器开关控制返回完成
+            if (rc_ctrl.sw[1]==1000) { // 假设遥控器开关控制返回完成
+                speed_5065=SHOOT_SPEED_BACK;
+                t_speedSet=STOP;
                 currentState = SHOOT_IDLE; // 切换回空闲状态
             }
             break;
@@ -125,4 +171,17 @@ void Shoot::shootStateMachine() {
             break;
         }
     }
+      result = PID_calc(&speed_pid,trigger_Motor->speed_rpm,t_speedSet);
+   //  CAN_VESC_Control(1,speed_5065,&hcan2);
+          
+}
+
+//MATLAB拟合函数
+float polynomial(float x) {
+    return 6348.2377f * powf(x, 5.0f)
+        + -128275.8296f * powf(x, 4.0f)
+        + 1030651.0231f * powf(x, 3.0f)
+        + -4115199.9362f * powf(x, 2.0f)
+        + 8167973.1254f * x
+        + -6420648.3555f;
 }

User/module/shoot.hpp

@@ -1,15 +1,27 @@
 #ifndef SHOOT_HPP
 #define SHOOT_HPP
 #include "djiMotor.h"
+#include "pid.h"
+#include "nuc.h"
 
 // 定义状态枚举
 typedef enum {
     SHOOT_IDLE,        // 空闲状态
+    SHOOT_READY,
     SHOOT_FIRE,        // 发射状态
-    SHOOT_TRIGGERED,   // 光电触发状态
+    SHOOT_BACK,   // 光电触发状态
     SHOOT_RETURN       // 自动返回状态
 } ShootState_t;
 
+// 定义状态枚举
+typedef enum {
+    BALL_IDLE,         // 空闲状态
+    BALL_FINISH,        // 完成状态
+//持球状态
+    BALL_TAKE,
+    BALL_LOSE
+
+} BallState_t;
 
 // 光电传感器读取宏
 #define IS_PHOTOELECTRIC_TRIGGERED() (HAL_GPIO_ReadPin(STOP_GPIO_Port, STOP_Pin) == GPIO_PIN_RESET)
@@ -18,16 +30,45 @@ class Shoot
 {
 public:
     Shoot();
-    void shootThree(void);
     void shootStop(void);
-    void shootBack(void);
-    void shootControl(void);
     void shootStateMachine(void);
+    void trigger_control(void);
+    void errorControl(void);
+    void vofaWatch(void);
+    void vesc_send(void);
+    void distanceGet(const NUC_t &nuc);
     float speed_5065;
+    float speed_trigger;
+    ShootState_t currentState;
+
+    int16_t result;
+    float t_speedSet;
+    //暂时放在公共,task里使用
+     CAN_MotorFeedback_t *motor_5065[2];
+    //==========================公共变量==========================
+    volatile BallState_t ballStatus;//是否有球
+    volatile uint32_t flag_thread;//接收传回的线程通知
+
+    //距离
+    float distance;
+
 private:
 
-   CAN_MotorFeedback_t *motor_5065[2];
+    //扳机2006
+    static const float M2006_speed_PID[3];
+    //电机速度pid结构体
+    pid_type_def speed_pid;
+    motor_measure_t *trigger_Motor;
+
+
+
 };
 
 
+float distanceToSpeed(float x);
+
+float polynomial(float x);
+
+float polynomial2(float x);
+
 #endif

User/task/ballTask.cpp

@@ -8,11 +8,25 @@
 #include "vofa.h"
 extern RC_ctrl_t rc_ctrl;
 Ball ball;
-float vofa[8];
+//float vofa[8];
+
+
+// C键 sw[5]👆 1800   中1000  👇200
+// D键 sw[6]👆 200 外伸   👇1800 归位  //三摩擦架子
+// H键 sw[7]👆 200   👇1800  //取球
+
+int angle1=34;
+int angle2=23;
 
 int abc=0;
+
+int a1=0;
+
+int speed_set=0;
+int speed_feedback=0;
+
 extern int speedm;
-extern int speedm1;
+
 
 void FunctionBall(void *argument)
 {
@@ -26,19 +40,59 @@ void FunctionBall(void *argument)
 #ifdef DEBUG
      task_struct.stack_water_mark.ball = osThreadGetStackSpace(osThreadGetId());  
 #endif          
-       abc=HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin);
-       //abc=HAL_GPIO_ReadPin(STOP_GPIO_Port, STOP_Pin);
 
-       ball.ballHadling(); // 处理摩擦轮转动
+  abc=HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin);
-      ball.Spin(speedm,speedm1);
-      ball.Send_control();
 
-      vofa[0] = speedm; // 发送电机角度数据
-      vofa[1] = -ball.hand_Motor[0]->speed_rpm; // 发送电机角度数据
-      vofa[2] = ball.hand_Motor[1]->speed_rpm; // 发送电机速度数据
-      vofa[3] = ball.hand_Motor[2]->speed_rpm; // 发送电机速度数据
 
-      vofa_tx_main(vofa); // 发送数据到虚拟串口
+   if(rc_ctrl.sw[6] == 200)
+    {
+        angle1=75;
+    }
+    if(rc_ctrl.sw[6]==1800)
+    {
+        angle1=0;
+      
+    }
+
+     //运球
+    if(rc_ctrl.sw[5]==200)
+    {
+        //👇
+        ball.ballHadling(); // 处理摩擦轮转动
+    }
+     //下球     
+    if(rc_ctrl.sw[5]==1800)
+    {
+        //👆
+        ball.ballDown();
+    }
+    //中间
+    if(rc_ctrl.sw[7]==200)
+    {
+         if(rc_ctrl.sw[5]==1000)
+        {
+            //中
+            ball.ballStop();
+            ball.ballHadling(); // 处理摩擦轮转动
+        }
+    }
+
+    if(rc_ctrl.sw[7]==1800)
+    {
+        //ball.ballTake();
+
+    }
+
+	// CAN_XiaoMi(1,&ball.JZ_xiaomi,&hcan2);
+
+    ball.Spin(speedm);
+    ball.Send_control();
+
+      // vofa[0] = -speedm; // 发送电机角度数据
+      // vofa[1] = ball.hand_Motor[0]->speed_rpm; // 发送电机角度数据
+      // vofa[2] = -ball.hand_Motor[1]->speed_rpm; // 发送电机速度数据
+      // vofa[3] = -ball.hand_Motor[2]->speed_rpm; // 发送电机速度数据
+      // vofa_tx_main(vofa); // 发送数据到虚拟串口
 
         tick += delay_tick; /* 计算下一个唤醒时刻 */
         osDelayUntil(tick);

User/task/gimbalTask.cpp

@@ -8,7 +8,7 @@
 #include "remote_control.h"
 #include "nuc.h"
 Gimbal gimbal;
-NUC_t nucData; // 用于存储从队列接收的数据
+// NUC_t nucData; // 用于存储从队列接收的数据
 extern RC_ctrl_t rc_ctrl;
 int cnt1=0;
 
@@ -28,15 +28,15 @@ void FunctionGimbal(void *argument)
 		task_struct.stack_water_mark.gimbal = osThreadGetStackSpace(osThreadGetId());  
    #endif 
 
-		cnt1++;
+		//cnt1++;
 
     //  gimbal.gimbalFlow();
 		// 从消息队列接收视觉数据
-		if (osMessageQueueGet(task_struct.msgq.nuc, &nucData, NULL, 0) == osOK)
+		// if (osMessageQueueGet(task_struct.msgq.nuc, &nucData, NULL, 0) == osOK)
-		{
+		// {
-			// 使用接收到的视觉数据调整云台
+		// 	// 使用接收到的视觉数据调整云台
-			gimbal.gimbalVision(nucData);
+		// 	//gimbal.gimbalVision(nucData);
-		}
+		// }
 			
 		osDelay(1);
 

User/task/nucTask.cpp

@@ -22,7 +22,6 @@ void Function_nuc(void *argument)
 
     NUC_Init(&cmd_fromnuc);
     
-
    uint32_t tick = osKernelGetTickCount();
 
     while(1)

User/task/shootTask.cpp

@@ -6,10 +6,17 @@
 #include "shoot.hpp"
 #include "motor.hpp"
 #include "remote_control.h"
+#include "nuc.h"
 extern RC_ctrl_t rc_ctrl;
 Shoot shoot;
+NUC_t nucData; // 自瞄
 
-// sw[0]2 下306上1694 sw[5]3前306后1694 sw[4]4前1694后306 sw[1]xuan1 sw[3]xuan2 
+int shoot_flag = 0;
+
+int a2;
+
+//E键 sw[0]👆 200  中 1000 👇1800
+//F键 sw[1]👆 1800   👇200
 
 void FunctionShoot(void *argument)
 {
@@ -25,8 +32,51 @@ while(1)
 	task_struct.stack_water_mark.shoot = osThreadGetStackSpace(osThreadGetId());  
 #endif
 
-	shoot.shootThree();
+	//我放的任务通知 运球成功放置过来后
-	//shoot.shootBack();
+	shoot.flag_thread=osThreadFlagsGet();
+
+	if(shoot.flag_thread & BALL_OK)
+	{
+		a2=2;
+	}
+
+
+	shoot_flag=HAL_GPIO_ReadPin(STOP_GPIO_Port, STOP_Pin);
+
+	if (osMessageQueueGet(task_struct.msgq.nuc, &nucData, NULL, 0) == osOK)
+	{
+		shoot.distanceGet(nucData);
+	}
+
+
+	if(rc_ctrl.sw[0]>1000)
+	{
+
+		if(rc_ctrl.sw[1]==1800)
+		{
+			shoot.errorControl();
+		}
+		if(rc_ctrl.sw[1]==1000)
+		{
+			shoot.shootStop();
+			shoot.shootStateMachine();//shoot
+		}
+		if(rc_ctrl.sw[1]==200)
+		{
+			shoot.shootStateMachine();//shoot
+		}
+	}
+	if(rc_ctrl.sw[0]==200)
+	{
+		shoot.shootStop();
+
+	}
+
+	shoot.vesc_send(); // 发送电机速度数据
+
+	shoot.trigger_control();
+
+	shoot.vofaWatch();
 
 	osDelay(2);