Compare commits
11 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| f9b87f8f39 | |||
| a235d14661 | |||
| 9688eca871 | |||
| 46cc491963 | |||
| 1fdf3e7e38 | |||
| 8fc2f50677 | |||
| 3d482325fb | |||
| 95ea791d34 | |||
| 602a6a2728 | |||
| c362ffe265 | |||
| 793e40985b |
@ -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 */
|
||||
|
||||
@ -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;
|
||||
|
||||
36
MDK-ARM/.vscode/c_cpp_properties.json
vendored
36
MDK-ARM/.vscode/c_cpp_properties.json
vendored
@ -3,26 +3,26 @@
|
||||
{
|
||||
"name": "R1-shooter",
|
||||
"includePath": [
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Core\\Inc",
|
||||
"d:\\Desktop\\r1\\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\\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\\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\\r1upper-1\\Drivers\\CMSIS\\Include",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1upper-1\\User\\bsp",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1upper-1\\User\\module",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1upper-1\\User\\task",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1upper-1\\User\\lib",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1upper-1\\User\\device",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Core\\Inc",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Drivers\\STM32F4xx_HAL_Driver\\Inc",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Drivers\\STM32F4xx_HAL_Driver\\Inc\\Legacy",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\include",
|
||||
"d:\\Desktop\\r1\\r1_upper\\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\\portable\\RVDS\\ARM_CM4F",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Drivers\\CMSIS\\Device\\ST\\STM32F4xx\\Include",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Drivers\\CMSIS\\Include",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\User\\bsp",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\User\\module",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\User\\task",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\User\\lib",
|
||||
"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\\r1upper-1\\Core\\Src",
|
||||
"d:\\Desktop\\r1\\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\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\MemMang"
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\MDK-ARM",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Core\\Src",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Drivers\\STM32F4xx_HAL_Driver\\Src",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\MemMang"
|
||||
],
|
||||
"defines": [
|
||||
"USE_HAL_DRIVER",
|
||||
|
||||
70
MDK-ARM/.vscode/keil-assistant.log
vendored
70
MDK-ARM/.vscode/keil-assistant.log
vendored
@ -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
|
||||
|
||||
|
||||
20
MDK-ARM/.vscode/settings.json
vendored
20
MDK-ARM/.vscode/settings.json
vendored
@ -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"
|
||||
}
|
||||
}
|
||||
6
MDK-ARM/.vscode/uv4.log
vendored
6
MDK-ARM/.vscode/uv4.log
vendored
@ -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
|
||||
|
||||
2
MDK-ARM/.vscode/uv4.log.lock
vendored
2
MDK-ARM/.vscode/uv4.log.lock
vendored
@ -1 +1 @@
|
||||
2025/4/26 16:32:18
|
||||
2025/5/31 16:57:56
|
||||
@ -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>
|
||||
|
||||
@ -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>
|
||||
|
||||
11
README.md
11
README.md
@ -15,3 +15,14 @@ r1上层
|
||||
//PC6 接球光电
|
||||
|
||||
//PI6 运球光电
|
||||
|
||||
## 上层过程
|
||||
|
||||
+ 一个按键切换 运球 /发射
|
||||
+ 👈 运球 +
|
||||
+ .3
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -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_REFEREE_FAST_REFRESH_UI (1u << 11)
|
||||
#define SIGNAL_REFEREE_SLOW_REFRESH_UI (1u << 12)
|
||||
#define SIGNAL_R12DS_BUF0_REDY (1u << 7)
|
||||
#define SIGNAL_NUC_RAW_REDY (1u << 8)
|
||||
#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
|
||||
}
|
||||
|
||||
@ -45,6 +45,19 @@
|
||||
(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 0x202:
|
||||
case 0x203:
|
||||
case 0x204:
|
||||
case 0x205:
|
||||
case 0x206:
|
||||
case 0x207:
|
||||
case M3508_1:
|
||||
case M3508_2:
|
||||
case M3508_3:
|
||||
case M3508_4:
|
||||
case M2006_1:
|
||||
{
|
||||
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,16 +348,7 @@ 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);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 返回电机数据指针
|
||||
* @param[in] i: 电机编号
|
||||
* @retval 电机数据指针
|
||||
*/
|
||||
motor_measure_t *get_motor_point(uint8_t i)
|
||||
{
|
||||
return &motor_chassis[i];
|
||||
}
|
||||
|
||||
//控制四个vesc的发送
|
||||
int8_t CAN_VESC_Control(int id,int speed ,CAN_HandleTypeDef*hcan)
|
||||
{
|
||||
|
||||
@ -336,7 +390,6 @@ return DEVICE_OK;
|
||||
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief 限制vesc电机转速
|
||||
* @param[in/out] rpm: vesce电机转速
|
||||
@ -363,7 +416,7 @@ return DEVICE_OK;
|
||||
uint32_t send_mail_box;
|
||||
|
||||
id = controller_id | ((uint32_t)CAN_PACKET_SET_RPM << 8);
|
||||
assert_param_rpm(&RPM);
|
||||
//assert_param_rpm(&RPM);
|
||||
data = (int32_t)(RPM);
|
||||
|
||||
vesc_tx_message.ExtId = id;
|
||||
@ -398,3 +451,84 @@ return DEVICE_OK;
|
||||
|
||||
HAL_CAN_AddTxMessage(&hcan2, &vesc_tx_message, vesc_send_data, &send_mail_box);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 返回电机数据指针
|
||||
* @param[in] i: 电机编号
|
||||
* @retval 电机数据指针
|
||||
*/
|
||||
motor_measure_t *get_motor_point(uint8_t i)
|
||||
{
|
||||
return &motor_chassis[i];
|
||||
}
|
||||
|
||||
CAN_MotorFeedback_t *get_vesc_point(uint8_t i)
|
||||
{
|
||||
return &motor_5065[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. 解析速度
|
||||
int16_t speed_code = ((rx_data[3] << 4) | (rx_data[4] >> 4)) - 0x800;
|
||||
feedback->speed_rads = (speed_code / 2048.0f) * speed_full_scale;
|
||||
|
||||
// 3. 解析电流
|
||||
int16_t current_code = (((rx_data[4] & 0x0F) << 8) | rx_data[5]) - 0x800;
|
||||
feedback->current_A = (current_code / 2048.0f) * current_full_scale;
|
||||
|
||||
// 记录电机ID
|
||||
feedback->can_id = rx_data[0];
|
||||
}
|
||||
|
||||
|
||||
void CAN_XiaoMi(int id,JZ_xiaomi_t *JZ_xiaomi,CAN_HandleTypeDef*hcan)
|
||||
{
|
||||
uint32_t send_mail_box;
|
||||
static int is_open=0; //是否开启电机，根据手册，第一次使用需要ff开启
|
||||
tx_message.StdId = id;
|
||||
tx_message.IDE = CAN_ID_STD;
|
||||
tx_message.RTR = CAN_RTR_DATA;
|
||||
tx_message.DLC = 0x08;
|
||||
if(is_open){
|
||||
|
||||
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);
|
||||
|
||||
can_send_data[0] = (pos_code >> 8) & 0xFF; // 位置高8位
|
||||
can_send_data[1] = pos_code & 0xFF; // 位置低8位
|
||||
|
||||
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位
|
||||
|
||||
can_send_data[4] = JZ_xiaomi->K_P & 0xFF; // Kp低8位
|
||||
|
||||
can_send_data[5] = (JZ_xiaomi->K_D >> 4) & 0xFF; // Kd高8位（取12位中的高8）
|
||||
can_send_data[6] = ((JZ_xiaomi->K_D & 0x0F) << 4) | ((current_code >> 8) & 0x0F); // Kd低4位 + 电流高4位
|
||||
|
||||
can_send_data[7] = current_code & 0xFF; // 电流低8位
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
can_send_data[0] = 0xFF;
|
||||
can_send_data[1] = 0xFF;
|
||||
can_send_data[2] = 0xFF;
|
||||
can_send_data[3] = 0xFF;
|
||||
can_send_data[4] = 0xFF;
|
||||
can_send_data[5] = 0xFF;
|
||||
can_send_data[6] = 0xFF;
|
||||
can_send_data[7] = 0xFC;
|
||||
is_open=1;
|
||||
}
|
||||
|
||||
|
||||
HAL_CAN_AddTxMessage(hcan, &tx_message, can_send_data, &send_mail_box);
|
||||
}
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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);
|
||||
// detect_hook(NUC_TOE);
|
||||
osThreadFlagsSet(thread_alert,SIGNAL_NUC_RAW_REDY);
|
||||
|
||||
}
|
||||
|
||||
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) ==
|
||||
// SIGNAL_NUC_RAW_REDY);
|
||||
return 1;
|
||||
return (osThreadFlagsWait(SIGNAL_NUC_RAW_REDY, osFlagsWaitAll,500) ==
|
||||
SIGNAL_NUC_RAW_REDY);
|
||||
|
||||
}
|
||||
|
||||
int8_t NUC_SendPacket(void *data, uint16_t length) {
|
||||
@ -75,18 +77,11 @@ int8_t NUC_RawParse(NUC_t *n) {
|
||||
} 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) {
|
||||
|
||||
if(nucbuf[0]!=HEAD) goto error; //发送ID不是底盘
|
||||
else
|
||||
{
|
||||
n->status_fromnuc = nucbuf[1];
|
||||
n->ctrl_status = nucbuf[2];
|
||||
|
||||
switch (n->status_fromnuc) {
|
||||
case VISION:
|
||||
/* 协议格式
|
||||
@ -118,22 +113,83 @@ int8_t NUC_RawParse(NUC_t *n) {
|
||||
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_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;
|
||||
|
||||
@ -4,6 +4,7 @@
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include <cmsis_os2.h>
|
||||
#include "struct_typedef.h"
|
||||
#include "device.h"
|
||||
#include "uart_it.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 串口接收的最大数据长度
|
||||
|
||||
184
User/lib/filter.c
Normal file
184
User/lib/filter.c
Normal file
@ -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);
|
||||
}
|
||||
104
User/lib/filter.h
Normal file
104
User/lib/filter.h
Normal file
@ -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
|
||||
65
User/lib/kalman.c
Normal file
65
User/lib/kalman.c
Normal file
@ -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)
|
||||
}
|
||||
28
User/lib/kalman.h
Normal file
28
User/lib/kalman.h
Normal file
@ -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
|
||||
@ -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;
|
||||
|
||||
}
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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;
|
||||
|
||||
// 定义状态机变量
|
||||
BallState_t currentState1 = BALL_IDLE; // 当前状态
|
||||
uint32_t startTime = 0; // 用于记录延时的起始时间
|
||||
// C键 sw[5]👆 1800 中1000 👇200
|
||||
// D键 sw[6]👆 200 👇1800
|
||||
|
||||
#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);
|
||||
@ -35,11 +42,68 @@ Ball ::Ball()
|
||||
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,35 +112,60 @@ 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;
|
||||
int ball_state = 0;
|
||||
void Ball::ballDown(void)
|
||||
{
|
||||
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); // 确保闭合气缸
|
||||
if (key > 0) // 检测按键是否被按下
|
||||
// 只在拨杆从非200切到200时触发
|
||||
if ((rc_ctrl.sw[5] == 200 && last_sw5 != 200) || key > 0)
|
||||
{
|
||||
speedm=-4500;
|
||||
speedm1=-4500;
|
||||
speedm = -4000;
|
||||
currentState1 = BALL_FORWARD; // 切换到正转状态
|
||||
}
|
||||
break;
|
||||
|
||||
case BALL_FORWARD:
|
||||
|
||||
if ( hand_Motor[MOTOR_1]->speed_rpm >= 4480&&hand_Motor[MOTOR_1]->speed_rpm <= 4530 &&
|
||||
hand_Motor[MOTOR_2]->speed_rpm <= -4480&&hand_Motor[MOTOR_2]->speed_rpm >= -4530 &&
|
||||
hand_Motor[MOTOR_3]->speed_rpm <= -4480&&hand_Motor[MOTOR_3]->speed_rpm >= -4530 )
|
||||
if (hand_Motor[MOTOR_1]->speed_rpm >= 3980 && hand_Motor[MOTOR_1]->speed_rpm <= 4020 &&
|
||||
hand_Motor[MOTOR_2]->speed_rpm <= -3980 && hand_Motor[MOTOR_2]->speed_rpm >= -4020 &&
|
||||
hand_Motor[MOTOR_3]->speed_rpm <= -3980 && hand_Motor[MOTOR_3]->speed_rpm >= -4020)
|
||||
{
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET); // 打开气缸
|
||||
currentState1 = BALL_DROP; // 切换到球下落状态
|
||||
@ -84,55 +173,36 @@ void Ball::ballHadling(void)
|
||||
break;
|
||||
|
||||
case BALL_DROP:
|
||||
|
||||
if (ball_state == 1) //读光电 有球 0 无球 1
|
||||
if (ball_state == 0) // 读光电 有球 1 无球 0
|
||||
{
|
||||
osDelay(200); // 延时200ms
|
||||
speedm=4500;
|
||||
speedm1=4500;
|
||||
speedm = 3500;
|
||||
currentState1 = BALL_REVERSE; // 切换到反转状态
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case BALL_REVERSE:
|
||||
|
||||
if (ball_state == 0)
|
||||
if (ball_state == 1)
|
||||
{
|
||||
|
||||
//flag=2; //抽象的计次
|
||||
speedm = 0; // 停止电机
|
||||
speedm1=0;
|
||||
currentState1 = BALL_CLOSE; // 切换到完成状态
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case BALL_CLOSE:
|
||||
// osDelay(200); // 延时50ms
|
||||
if(ball_state == 0)
|
||||
if (ball_state == 1)
|
||||
{
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);
|
||||
currentState1 = BALL_FINISH; // 切换到完成状态
|
||||
}
|
||||
// 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; // 重置按键状态
|
||||
flag=0;
|
||||
speedm = 0;
|
||||
speedm1=0;
|
||||
currentState1 = BALL_IDLE; // 回到空闲状态
|
||||
osThreadFlagsSet(task_struct.thread.shoot, BALL_OK);
|
||||
currentState1 = BALL_IDLE; // 直接回到空闲状态
|
||||
break;
|
||||
|
||||
default:
|
||||
@ -140,72 +210,22 @@ void Ball::ballHadling(void)
|
||||
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:
|
||||
// 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)
|
||||
|
||||
@ -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"
|
||||
|
||||
#define HANDLING3 1
|
||||
#include "filter.h"
|
||||
|
||||
// 定义状态枚举
|
||||
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;//接收传回的线程通知
|
||||
|
||||
int up_ball_state; //上球状态 0:无球 1:有球
|
||||
private:
|
||||
|
||||
//三个摩擦轮
|
||||
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];
|
||||
};
|
||||
|
||||
|
||||
@ -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->type = GM6020;
|
||||
PID_init(&speed_pid,PID_POSITION,Gimbal_speed_PID,16000, 6000);
|
||||
PID_init(&angle_pid,PID_POSITION,Gimbal_angle_PID,5000, 2000);
|
||||
// GM6020_Motor = get_motor_point(6);
|
||||
// GM6020_Motor->type = GM6020;
|
||||
// PID_init(&speed_pid,PID_POSITION,Gimbal_speed_PID,16000, 6000);
|
||||
// PID_init(&angle_pid,PID_POSITION,Gimbal_angle_PID,5000, 2000);
|
||||
|
||||
result = 0;
|
||||
angleSet = 0;
|
||||
// result = 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]);
|
||||
|
||||
|
||||
@ -6,7 +6,6 @@
|
||||
#include "pid.h"
|
||||
#include "nuc.h"
|
||||
|
||||
#define GM6020ING 1
|
||||
class Gimbal
|
||||
{
|
||||
public:
|
||||
|
||||
@ -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
|
||||
#define SHOOT_SPEED_BACK -4000
|
||||
NUC_t nuc_v;
|
||||
|
||||
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
|
||||
|
||||
// 定义状态机变量
|
||||
ShootState_t currentState = SHOOT_IDLE;
|
||||
int Torque = -5000; // 扳机恒力
|
||||
#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);//获取电机数据指针
|
||||
CAN_VESC_RPM(1, STOP);
|
||||
CAN_VESC_RPM(2, STOP);
|
||||
//扳机初始化
|
||||
trigger_Motor= get_motor_point(4);
|
||||
trigger_Motor->type=M2006;
|
||||
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))
|
||||
{
|
||||
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);
|
||||
}
|
||||
|
||||
// result = PID_calc(&speed_pid,trigger_Motor->speed_rpm,t_speedSet);
|
||||
CAN_cmd_1FF(result,0,0,0,&hcan1);
|
||||
}
|
||||
|
||||
void Shoot::shootStop()
|
||||
{
|
||||
CAN_VESC_HEAD(1);
|
||||
CAN_VESC_HEAD(2);
|
||||
speed_5065=STOP;
|
||||
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) {
|
||||
currentState = SHOOT_FIRE; // 切换到发射状态
|
||||
if((rc_ctrl.sw[1]==200)) {
|
||||
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);
|
||||
CAN_VESC_HEAD(2);
|
||||
t_speedSet=STOP;
|
||||
speed_5065=STOP;
|
||||
|
||||
// 切换到返回状态
|
||||
currentState = SHOOT_RETURN;
|
||||
break;
|
||||
@ -109,11 +153,13 @@ void Shoot::shootStateMachine() {
|
||||
|
||||
case SHOOT_RETURN: {
|
||||
// 自动返回状态，控制电机反转
|
||||
CAN_VESC_RPM(1, SHOOT_SPEED_BACK);
|
||||
CAN_VESC_RPM(2, SHOOT_SPEED_BACK);
|
||||
speed_5065=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;
|
||||
}
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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);
|
||||
|
||||
abc=HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin);
|
||||
|
||||
|
||||
if(rc_ctrl.sw[6] == 200)
|
||||
{
|
||||
angle1=75;
|
||||
}
|
||||
if(rc_ctrl.sw[6]==1800)
|
||||
{
|
||||
angle1=0;
|
||||
|
||||
}
|
||||
|
||||
//运球
|
||||
if(rc_ctrl.sw[5]==200)
|
||||
{
|
||||
//👇
|
||||
ball.ballHadling(); // 处理摩擦轮转动
|
||||
ball.Spin(speedm,speedm1);
|
||||
}
|
||||
//下球
|
||||
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); // 发送数据到虚拟串口
|
||||
// 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);
|
||||
|
||||
@ -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)
|
||||
{
|
||||
// 使用接收到的视觉数据调整云台
|
||||
gimbal.gimbalVision(nucData);
|
||||
}
|
||||
// if (osMessageQueueGet(task_struct.msgq.nuc, &nucData, NULL, 0) == osOK)
|
||||
// {
|
||||
// // 使用接收到的视觉数据调整云台
|
||||
// //gimbal.gimbalVision(nucData);
|
||||
// }
|
||||
|
||||
osDelay(1);
|
||||
|
||||
|
||||
@ -22,7 +22,6 @@ void Function_nuc(void *argument)
|
||||
|
||||
NUC_Init(&cmd_fromnuc);
|
||||
|
||||
|
||||
uint32_t tick = osKernelGetTickCount();
|
||||
|
||||
while(1)
|
||||
|
||||
@ -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);
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user