LYL/R2_NEW
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Compare commits

...

22 Commits
Cooperatio ... main

Author SHA1 Message Date
ZHAISHUI04
2a15ca25ed 投篮赛完赛 2025-07-13 17:55:54 +08:00
ZHAISHUI04
bea23c3860 修改参数 2025-07-13 01:42:10 +08:00
ZHAISHUI04
6cf2fb649b 参数 2025-07-11 04:47:49 +08:00
ZHAISHUI04
04a3906b2c 底盘pid 2025-07-11 01:10:03 +08:00
ZHAISHUI04
c4bc8288c7 参数修改 2025-07-09 07:33:59 +08:00
ZHAISHUI04
de7321138f 协议修改 2025-07-08 07:44:04 +08:00
ZHAISHUI04
2403b3bf52 频率 2025-07-08 07:39:48 +08:00
ZHAISHUI04
2e854ade02 解决nuc控制数据不清空问题 2025-07-07 23:41:32 +08:00
ZHAISHUI04
bd16e41962 Merge branch 'main' of ssh://gitea.qutrobot.top:222/LYL/R2_NEW 2025-07-06 23:16:29 +08:00
ZHAISHUI04
4bc5997670 7.6nuc通信正常 2025-07-06 23:04:45 +08:00
ZHAISHUI04
6179339e82 nuc协议更改，上层参数修改 2025-07-05 02:55:37 +08:00
Robofish
b7e4d04550 更新imu数据 2025-07-03 22:47:46 +08:00
ZHAISHUI04
40ca2de038 sick改pid，远距离抖 2025-07-02 20:42:48 +08:00
ZHAISHUI04
67960d2805 比较完善的，手动，雷达，运球 2025-06-29 01:48:25 +08:00
ZHAISHUI04
866e4de877 优化 2025-06-27 23:05:03 +08:00
ZHAISHUI04
d0efaf8dab up_utils 2025-06-26 00:48:19 +08:00
ZHAISHUI04
4859c69fad sick 2025-06-24 21:01:44 +08:00
ZHAISHUI04
8570b03ca2 Merge branch 'cooperation_mid360' to main 2025-06-23 01:22:12 +08:00
ZHAISHUI04
1b5eb756e9 改大疆遥控 2025-06-04 19:50:56 +08:00
ZHAISHUI04
8ef2a0a348 雷达增加发送，增加视觉备用，增加go电机限位 2025-06-04 00:25:05 +08:00
ZHAISHUI04
7499e75a50 增加配合，总过程 2025-05-24 14:56:15 +08:00
ZHAISHUI04
79f6f5f9c0 go和nuc串口更换，根据位置自动发go的位置 2025-05-24 14:19:31 +08:00
35 changed files with 1600 additions and 1092 deletions

2
.gitignore vendored
View File

@ -17,7 +17,7 @@
*.dbg* *.dbg*
*.uvguix.* *.uvguix.*
.mxproject .mxproject
*.axf
RTE/ RTE/
Templates/ Templates/
Examples/ Examples/

1
Core/Src/main.c
View File

@ -81,6 +81,7 @@ int main(void)
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */ /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
HAL_Init(); HAL_Init();
/* USER CODE BEGIN Init */ /* USER CODE BEGIN Init */

16
MDK-ARM/.vscode/keil-assistant.log vendored
View File

@ -471,3 +471,19 @@
[info] Log at : 2025/6/22|14:26:08|GMT+0800 [info] Log at : 2025/6/22|14:26:08|GMT+0800
[info] Log at : 2025/6/23|20:59:34|GMT+0800
[info] Log at : 2025/6/24|03:28:50|GMT+0800
[info] Log at : 2025/6/26|19:47:59|GMT+0800
[info] Log at : 2025/6/27|16:40:58|GMT+0800
[info] Log at : 2025/6/28|01:22:24|GMT+0800
[info] Log at : 2025/6/30|18:03:06|GMT+0800
[info] Log at : 2025/7/1|14:01:22|GMT+0800
[info] Log at : 2025/7/7|02:36:22|GMT+0800

3
MDK-ARM/.vscode/settings.json vendored
View File

@ -97,6 +97,7 @@
"gimbal_task.h": "c", "gimbal_task.h": "c",
"gimgal.h": "c", "gimgal.h": "c",
"navi.h": "c", "navi.h": "c",
"up.h": "c" "up.h": "c",
"bsp_usart.h": "c"
} }
} }

164
MDK-ARM/R2.uvoptx
View File

@ -26,7 +26,7 @@
<ToolsetNumber>0x4</ToolsetNumber> <ToolsetNumber>0x4</ToolsetNumber>
<ToolsetName>ARM-ADS</ToolsetName> <ToolsetName>ARM-ADS</ToolsetName>
<TargetOption> <TargetOption>
<CLKADS>12000000</CLKADS> <CLKADS>25000000</CLKADS>
<OPTTT> <OPTTT>
<gFlags>1</gFlags> <gFlags>1</gFlags>
<BeepAtEnd>1</BeepAtEnd> <BeepAtEnd>1</BeepAtEnd>
@ -117,11 +117,6 @@
<pMon>BIN\CMSIS_AGDI.dll</pMon> <pMon>BIN\CMSIS_AGDI.dll</pMon>
</DebugOpt> </DebugOpt>
<TargetDriverDllRegistry> <TargetDriverDllRegistry>
<SetRegEntry>
<Number>0</Number>
<Key>ST-LINKIII-KEIL_SWO</Key>
<Name>-U00260035480000034E575152 -O206 -SF5000 -C0 -A0 -I0 -HNlocalhost -HP7184 -P1 -N00("ARM CoreSight SW-DP (ARM Core") -D00(2BA01477) -L00(0) -TO131090 -TC10000000 -TT10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)</Name>
</SetRegEntry>
<SetRegEntry> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
<Key>ARMRTXEVENTFLAGS</Key> <Key>ARMRTXEVENTFLAGS</Key>
@ -139,13 +134,13 @@
</SetRegEntry> </SetRegEntry>
<SetRegEntry> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
<Key>DLGUARM</Key> <Key>CMSIS_AGDI</Key>
<Name></Name> <Name>-X"Any" -UAny -O206 -S8 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO65554 -TC10000000 -TT10000000 -TP20 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)</Name>
</SetRegEntry> </SetRegEntry>
<SetRegEntry> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
<Key>CMSIS_AGDI</Key> <Key>DLGUARM</Key>
<Name>-X"Any" -UAny -O206 -S9 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO65554 -TC10000000 -TT10000000 -TP20 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)</Name> <Name></Name>
</SetRegEntry> </SetRegEntry>
<SetRegEntry> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
@ -158,72 +153,57 @@
<Ww> <Ww>
<count>0</count> <count>0</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>can,0x0A</ItemText> <ItemText>tar_angle</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>1</count> <count>1</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>chassis,0x0A</ItemText> <ItemText>huart-&gt;ErrorCode</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>2</count> <count>2</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>UP,0x0A</ItemText> <ItemText>error_code</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>3</count> <count>3</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>NUC_send,0x0A</ItemText> <ItemText>posss</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>4</count> <count>4</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>pid,0x0A</ItemText> <ItemText>UP,0x0A</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>5</count> <count>5</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>pid_param,0x0A</ItemText> <ItemText>chassis,0x0A</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>6</count> <count>6</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>cmd_rc,0x0A</ItemText> <ItemText>nuc_raw</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>7</count> <count>7</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>cbuf,0x10</ItemText> <ItemText>cmd_fromnuc,0x0A</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>8</count> <count>8</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>cmd</ItemText> <ItemText>drop_message,0x0A</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>9</count> <count>9</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>rc_ctrl,0x0A</ItemText> <ItemText>\\R2\../User/task/chassis_task.c\chassis.param-&gt;M3508_param.d</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>10</count> <count>10</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>nuc_raw,0x0A</ItemText> <ItemText>\\R2\../User/task/nuc_task.c\cmd_fromnuc</ItemText>
</Ww>
<Ww>
<count>11</count>
<WinNumber>1</WinNumber>
<ItemText>cmd_fromnuc,0x0A</ItemText>
</Ww>
<Ww>
<count>12</count>
<WinNumber>1</WinNumber>
<ItemText>nucbuf,0x0A</ItemText>
</Ww>
<Ww>
<count>13</count>
<WinNumber>1</WinNumber>
<ItemText>LD_raw,0x0A</ItemText>
</Ww> </Ww>
</WatchWindow1> </WatchWindow1>
<Tracepoint> <Tracepoint>
@ -270,10 +250,10 @@
<pMultCmdsp></pMultCmdsp> <pMultCmdsp></pMultCmdsp>
<DebugDescription> <DebugDescription>
<Enable>1</Enable> <Enable>1</Enable>
<EnableFlashSeq>0</EnableFlashSeq> <EnableFlashSeq>1</EnableFlashSeq>
<EnableLog>0</EnableLog> <EnableLog>0</EnableLog>
<Protocol>2</Protocol> <Protocol>2</Protocol>
<DbgClock>5000000</DbgClock> <DbgClock>10000000</DbgClock>
</DebugDescription> </DebugDescription>
</TargetOption> </TargetOption>
</Target> </Target>
@ -312,7 +292,7 @@
<Group> <Group>
<GroupName>Application/User/Core</GroupName> <GroupName>Application/User/Core</GroupName>
<tvExp>1</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel> <cbSel>0</cbSel>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
@ -452,7 +432,7 @@
<Group> <Group>
<GroupName>Drivers/STM32F4xx_HAL_Driver</GroupName> <GroupName>Drivers/STM32F4xx_HAL_Driver</GroupName>
<tvExp>1</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel> <cbSel>0</cbSel>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
@ -724,7 +704,7 @@
<Group> <Group>
<GroupName>Drivers/CMSIS</GroupName> <GroupName>Drivers/CMSIS</GroupName>
<tvExp>1</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel> <cbSel>0</cbSel>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
@ -744,7 +724,7 @@
<Group> <Group>
<GroupName>Middlewares/FreeRTOS</GroupName> <GroupName>Middlewares/FreeRTOS</GroupName>
<tvExp>1</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel> <cbSel>0</cbSel>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
@ -912,6 +892,18 @@
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<bShared>0</bShared> <bShared>0</bShared>
</File> </File>
<File>
<GroupNumber>6</GroupNumber>
<FileNumber>50</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>..\User\Module\up_utils.c</PathWithFileName>
<FilenameWithoutPath>up_utils.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
</Group> </Group>
<Group> <Group>
@ -922,7 +914,7 @@
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<File> <File>
<GroupNumber>7</GroupNumber> <GroupNumber>7</GroupNumber>
<FileNumber>50</FileNumber> <FileNumber>51</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -934,7 +926,7 @@
</File> </File>
<File> <File>
<GroupNumber>7</GroupNumber> <GroupNumber>7</GroupNumber>
<FileNumber>51</FileNumber> <FileNumber>52</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -946,7 +938,7 @@
</File> </File>
<File> <File>
<GroupNumber>7</GroupNumber> <GroupNumber>7</GroupNumber>
<FileNumber>52</FileNumber> <FileNumber>53</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -958,7 +950,7 @@
</File> </File>
<File> <File>
<GroupNumber>7</GroupNumber> <GroupNumber>7</GroupNumber>
<FileNumber>53</FileNumber> <FileNumber>54</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -970,7 +962,7 @@
</File> </File>
<File> <File>
<GroupNumber>7</GroupNumber> <GroupNumber>7</GroupNumber>
<FileNumber>54</FileNumber> <FileNumber>55</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -982,7 +974,7 @@
</File> </File>
<File> <File>
<GroupNumber>7</GroupNumber> <GroupNumber>7</GroupNumber>
<FileNumber>55</FileNumber> <FileNumber>56</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -994,7 +986,7 @@
</File> </File>
<File> <File>
<GroupNumber>7</GroupNumber> <GroupNumber>7</GroupNumber>
<FileNumber>56</FileNumber> <FileNumber>57</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1006,7 +998,7 @@
</File> </File>
<File> <File>
<GroupNumber>7</GroupNumber> <GroupNumber>7</GroupNumber>
<FileNumber>57</FileNumber> <FileNumber>58</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1018,7 +1010,7 @@
</File> </File>
<File> <File>
<GroupNumber>7</GroupNumber> <GroupNumber>7</GroupNumber>
<FileNumber>58</FileNumber> <FileNumber>59</FileNumber>
<FileType>5</FileType> <FileType>5</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1030,7 +1022,7 @@
</File> </File>
<File> <File>
<GroupNumber>7</GroupNumber> <GroupNumber>7</GroupNumber>
<FileNumber>59</FileNumber> <FileNumber>60</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1042,7 +1034,7 @@
</File> </File>
<File> <File>
<GroupNumber>7</GroupNumber> <GroupNumber>7</GroupNumber>
<FileNumber>60</FileNumber> <FileNumber>61</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1054,7 +1046,7 @@
</File> </File>
<File> <File>
<GroupNumber>7</GroupNumber> <GroupNumber>7</GroupNumber>
<FileNumber>61</FileNumber> <FileNumber>62</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1074,7 +1066,7 @@
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<File> <File>
<GroupNumber>8</GroupNumber> <GroupNumber>8</GroupNumber>
<FileNumber>62</FileNumber> <FileNumber>63</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1086,7 +1078,7 @@
</File> </File>
<File> <File>
<GroupNumber>8</GroupNumber> <GroupNumber>8</GroupNumber>
<FileNumber>63</FileNumber> <FileNumber>64</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1098,7 +1090,7 @@
</File> </File>
<File> <File>
<GroupNumber>8</GroupNumber> <GroupNumber>8</GroupNumber>
<FileNumber>64</FileNumber> <FileNumber>65</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1110,7 +1102,7 @@
</File> </File>
<File> <File>
<GroupNumber>8</GroupNumber> <GroupNumber>8</GroupNumber>
<FileNumber>65</FileNumber> <FileNumber>66</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1122,7 +1114,7 @@
</File> </File>
<File> <File>
<GroupNumber>8</GroupNumber> <GroupNumber>8</GroupNumber>
<FileNumber>66</FileNumber> <FileNumber>67</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1134,7 +1126,7 @@
</File> </File>
<File> <File>
<GroupNumber>8</GroupNumber> <GroupNumber>8</GroupNumber>
<FileNumber>67</FileNumber> <FileNumber>68</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1146,7 +1138,7 @@
</File> </File>
<File> <File>
<GroupNumber>8</GroupNumber> <GroupNumber>8</GroupNumber>
<FileNumber>68</FileNumber> <FileNumber>69</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1158,7 +1150,7 @@
</File> </File>
<File> <File>
<GroupNumber>8</GroupNumber> <GroupNumber>8</GroupNumber>
<FileNumber>69</FileNumber> <FileNumber>70</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1170,7 +1162,7 @@
</File> </File>
<File> <File>
<GroupNumber>8</GroupNumber> <GroupNumber>8</GroupNumber>
<FileNumber>70</FileNumber> <FileNumber>71</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1182,7 +1174,7 @@
</File> </File>
<File> <File>
<GroupNumber>8</GroupNumber> <GroupNumber>8</GroupNumber>
<FileNumber>71</FileNumber> <FileNumber>72</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1202,7 +1194,7 @@
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<File> <File>
<GroupNumber>9</GroupNumber> <GroupNumber>9</GroupNumber>
<FileNumber>72</FileNumber> <FileNumber>73</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1214,7 +1206,7 @@
</File> </File>
<File> <File>
<GroupNumber>9</GroupNumber> <GroupNumber>9</GroupNumber>
<FileNumber>73</FileNumber> <FileNumber>74</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1226,7 +1218,7 @@
</File> </File>
<File> <File>
<GroupNumber>9</GroupNumber> <GroupNumber>9</GroupNumber>
<FileNumber>74</FileNumber> <FileNumber>75</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1238,7 +1230,7 @@
</File> </File>
<File> <File>
<GroupNumber>9</GroupNumber> <GroupNumber>9</GroupNumber>
<FileNumber>75</FileNumber> <FileNumber>76</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1250,7 +1242,7 @@
</File> </File>
<File> <File>
<GroupNumber>9</GroupNumber> <GroupNumber>9</GroupNumber>
<FileNumber>76</FileNumber> <FileNumber>77</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1262,7 +1254,7 @@
</File> </File>
<File> <File>
<GroupNumber>9</GroupNumber> <GroupNumber>9</GroupNumber>
<FileNumber>77</FileNumber> <FileNumber>78</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1282,7 +1274,7 @@
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<File> <File>
<GroupNumber>10</GroupNumber> <GroupNumber>10</GroupNumber>
<FileNumber>78</FileNumber> <FileNumber>79</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1294,7 +1286,7 @@
</File> </File>
<File> <File>
<GroupNumber>10</GroupNumber> <GroupNumber>10</GroupNumber>
<FileNumber>79</FileNumber> <FileNumber>80</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1306,7 +1298,7 @@
</File> </File>
<File> <File>
<GroupNumber>10</GroupNumber> <GroupNumber>10</GroupNumber>
<FileNumber>80</FileNumber> <FileNumber>81</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1318,7 +1310,7 @@
</File> </File>
<File> <File>
<GroupNumber>10</GroupNumber> <GroupNumber>10</GroupNumber>
<FileNumber>81</FileNumber> <FileNumber>82</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1330,7 +1322,7 @@
</File> </File>
<File> <File>
<GroupNumber>10</GroupNumber> <GroupNumber>10</GroupNumber>
<FileNumber>82</FileNumber> <FileNumber>83</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1342,7 +1334,7 @@
</File> </File>
<File> <File>
<GroupNumber>10</GroupNumber> <GroupNumber>10</GroupNumber>
<FileNumber>83</FileNumber> <FileNumber>84</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1354,7 +1346,7 @@
</File> </File>
<File> <File>
<GroupNumber>10</GroupNumber> <GroupNumber>10</GroupNumber>
<FileNumber>84</FileNumber> <FileNumber>85</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1374,7 +1366,7 @@
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<File> <File>
<GroupNumber>11</GroupNumber> <GroupNumber>11</GroupNumber>
<FileNumber>85</FileNumber> <FileNumber>86</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1386,7 +1378,7 @@
</File> </File>
<File> <File>
<GroupNumber>11</GroupNumber> <GroupNumber>11</GroupNumber>
<FileNumber>86</FileNumber> <FileNumber>87</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1398,7 +1390,7 @@
</File> </File>
<File> <File>
<GroupNumber>11</GroupNumber> <GroupNumber>11</GroupNumber>
<FileNumber>87</FileNumber> <FileNumber>88</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1418,7 +1410,7 @@
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<File> <File>
<GroupNumber>12</GroupNumber> <GroupNumber>12</GroupNumber>
<FileNumber>88</FileNumber> <FileNumber>89</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1432,13 +1424,13 @@
<Group> <Group>
<GroupName>Middlewares/USB_Device_Library</GroupName> <GroupName>Middlewares/USB_Device_Library</GroupName>
<tvExp>1</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel> <cbSel>0</cbSel>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<File> <File>
<GroupNumber>13</GroupNumber> <GroupNumber>13</GroupNumber>
<FileNumber>89</FileNumber> <FileNumber>90</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1450,7 +1442,7 @@
</File> </File>
<File> <File>
<GroupNumber>13</GroupNumber> <GroupNumber>13</GroupNumber>
<FileNumber>90</FileNumber> <FileNumber>91</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1462,7 +1454,7 @@
</File> </File>
<File> <File>
<GroupNumber>13</GroupNumber> <GroupNumber>13</GroupNumber>
<FileNumber>91</FileNumber> <FileNumber>92</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1474,7 +1466,7 @@
</File> </File>
<File> <File>
<GroupNumber>13</GroupNumber> <GroupNumber>13</GroupNumber>
<FileNumber>92</FileNumber> <FileNumber>93</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>

5
MDK-ARM/R2.uvprojx
View File

@ -1113,6 +1113,11 @@
<FileType>1</FileType> <FileType>1</FileType>
<FilePath>..\User\Module\up.c</FilePath> <FilePath>..\User\Module\up.c</FilePath>
</File> </File>
<File>
<FileName>up_utils.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\Module\up_utils.c</FilePath>
</File>
</Files> </Files>
</Group> </Group>
<Group> <Group>

BIN
MDK-ARM/R2/R2.axf
View File

Binary file not shown.

100
Middlewares/Third_Party/Protocol/protocol.h vendored Normal file
View File

@ -0,0 +1,100 @@
/*
视觉与电控通信协议
*/
#pragma once
#include <stdint.h>
#include <stdbool.h>
#ifdef __cplusplus
extern "C"
{
#endif
#define NUC_HEAD (0xFF)
#define NUC_END (0xFE)
#define NUC_ID (0x09)
#define NUC_CTRL (0x01)
#define IMU_ID (0x01)
#define CMD_ID (0x02)
/* 电控 -> 视觉 IMU数据结构体*/
typedef struct __attribute__((packed))
{
struct __attribute__((packed))
{
float x;
float y;
float z;
} gyro; /* 陀螺仪数据 */
struct __attribute__((packed))
{
float x;
float y;
float z;
} accl; /* 四元数 */
struct __attribute__((packed))
{
float q0;
float q1;
float q2;
float q3;
} quat; /* 四元数 */
} Protocol_UpDataIMU_t;
/* 电控 -> 视觉 控制数据结构体*/
typedef struct __attribute__((packed))
{
int8_t cmd; /* 控制命令 */
} Protocol_UpDataCMD_t;
/* 视觉 -> 电控 数据结构体*/
typedef struct __attribute__((packed))
{
struct __attribute__((packed))
{
float vx; /* x轴移动速度 */
float vy; /* y轴移动速度 */
float wz; /* z轴转动速度 */
} chassis_move_vec; /* 底盘移动向量 */
float distance; /* 距离（单位：米） */
float angle; /* 角度（单位：弧度） */
bool notice; /* 控制命令 */
} Protocol_DownData_t;
typedef struct __attribute__((packed))
{
uint8_t head;
uint8_t id; /* 数据包ID */
uint8_t ctrl; /* 控制字 */
Protocol_UpDataIMU_t data;
uint8_t end; /* 数据包结束符 */
} Protocol_UpPackageIMU_t;
typedef struct __attribute__((packed))
{
uint8_t head;
uint8_t id;
uint8_t ctrl;
Protocol_UpDataCMD_t data;
uint8_t end;
} Protocol_UpPackageCMD_t;
typedef struct __attribute__((packed))
{
Protocol_DownData_t data;
uint16_t crc16;
} Protocol_DownPackage_t;
#ifdef __cplusplus
}
#endif

2
User/Algorithm/error_detect.h
View File

@ -1,6 +1,4 @@
#ifndef ERROR_DETECT_H #ifndef ERROR_DETECT_H
#define ERROR_DETECT_H #define ERROR_DETECT_H
#include "struct_typedef.h" #include "struct_typedef.h"

15
User/Algorithm/user_math.c
View File

@ -405,4 +405,19 @@ bool is_reached_multiple(float current1, float current2, float current3, float t
count++; // 所有条件都满足，计数加 1 count++; // 所有条件都满足，计数加 1
} }
return false; // 未满足条件达到阈值，返回 0 return false; // 未满足条件达到阈值，返回 0
}
/**
* @brief 底盘无头模式转换函数
* @param vx 输入 X 方向速度（全局坐标系），输出转换后的底盘 X 速度
* @param vy 输入 Y 方向速度（全局坐标系），输出转换后的底盘 Y 速度
* @param vw 旋转速度（直接控制，不受影响）
* @param yaw 当前陀螺仪 yaw 角度（弧度制）
*/
void ChassisHeadlessMode(double *vx, double *vy, double yaw) {
double vx_global = *vx;
double vy_global = *vy;
// 标准二维旋转变换（逆时针为正）
*vx = vx_global * cosf(yaw) - vy_global * sinf(yaw);
*vy = vx_global * sinf(yaw) + vy_global * cosf(yaw);
} }

1
User/Algorithm/user_math.h
View File

@ -165,5 +165,6 @@ void normalize_vector(double x, double y, double *out_x, double *out_y) ;
bool is_reached(float current, float target, float mistake) ; bool is_reached(float current, float target, float mistake) ;
bool is_reached_multiple(float current1, float current2, float current3, float target1, float target2, float target3, float mistake, int threshold) ; bool is_reached_multiple(float current1, float current2, float current3, float target1, float target2, float target3, float mistake, int threshold) ;
fp32 abs_limit_min_max_fp(fp32 *num, fp32 Limit_min,fp32 Limit_max); fp32 abs_limit_min_max_fp(fp32 *num, fp32 Limit_min,fp32 Limit_max);
void ChassisHeadlessMode(double *vx, double *vy, double yaw) ;
#endif #endif

222
User/Module/Chassis.c
View File

@ -3,7 +3,6 @@
#include "user_math.h" #include "user_math.h"
#include "bsp_buzzer.h" #include "bsp_buzzer.h"
#include "bsp_delay.h" #include "bsp_delay.h"
/* /*
机器人坐标系，向前x，右y，上yaw 机器人坐标系，向前x，右y，上yaw
不同于nuc (前x，左y，上yaw) 不同于nuc (前x，左y，上yaw)
@ -28,10 +27,13 @@ int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can) {
c->motorfeedback.rotor_rpm3508[i] = can->motor.chassis_motor3508.as_array[i].rotor_speed; c->motorfeedback.rotor_rpm3508[i] = can->motor.chassis_motor3508.as_array[i].rotor_speed;
c->motorfeedback.rotor_current3508[i] = can->motor.chassis_motor3508.as_array[i].torque_current; c->motorfeedback.rotor_current3508[i] = can->motor.chassis_motor3508.as_array[i].torque_current;
} }
for (uint8_t i = 0; i < 4; i++) { // for (uint8_t i = 0; i < 4; i++) {
//
c->sick_cali.sick_dis[i]=can->sickfed.raw_dis[i] / 10; //没使用原值 // c->sick_cali.sick_dis[i]=can->sickfed.raw_dis[i] / 10; //没使用原值
} // }
c->sick_cali.sick_dis[0]=(fp32)(can->sickfed.raw_dis[0]) ;
c->sick_cali.sick_dis[1]=(fp32)(can->sickfed.raw_dis[1]+30) ; //有点误差，手动补偿
c->sick_cali.sick_dis[2]=(fp32)(can->sickfed.raw_dis[2] );
return CHASSIS_OK; return CHASSIS_OK;
} }
@ -40,37 +42,65 @@ int8_t Chassis_init(Chassis_t *c, const Chassis_Param_t *param, float target_fre
if (c == NULL) return CHASSIS_ERR_NULL; if (c == NULL) return CHASSIS_ERR_NULL;
c->param = param; c->param = param;
for (int i = 0; i < 4; i++) { for (int i = 0; i < 3; i++) {
PID_init(&(c->pid.chassis_3508VecPID[i]), PID_POSITION_D, &(c->param->M3508_param)); PID_init(&(c->pid.chassis_3508VecPID[i]), PID_POSITION_D, &(c->param->M3508_param));
} }
PID_init(&(c->pid.chassis_3508VecPID[3]), PID_POSITION_D, &(c->param->M3508_param_other));
PID_init(&(c->pid.chassis_PICKWzPID_HIGN), PID_POSITION, &(c->param->chassis_PICKWzPID_HIGN_param)); PID_init(&(c->pid.chassis_PICKWzPID_HIGN), PID_POSITION, &(c->param->chassis_PICKWzPID_HIGN_param));
PID_init(&(c->pid.chassis_PICKWzPID_LOW), PID_POSITION, &(c->param->chassis_PICKWzPID_LOW_param)); PID_init(&(c->pid.chassis_PICKWzPID_LOW), PID_POSITION, &(c->param->chassis_PICKWzPID_LOW_param));
PID_init(&(c->pid.SickCaliWzPID), PID_POSITION, &(c->param->Sick_CaliWparam)); /*sick*/
PID_init(&(c->pid.SickCaliVxPID), PID_POSITION, &(c->param->Sick_CaliXparam)); PID_init(&(c->pid.SickCaliWzInPID), PID_POSITION, &(c->param->SickCali_WInparam));
PID_init(&(c->pid.SickCaliVyPID), PID_POSITION, &(c->param->Sick_CaliYparam)); PID_init(&(c->pid.SickCaliWzOutPID), PID_POSITION, &(c->param->SickCali_WOutparam));
PID_init(&(c->pid.SickCaliVxInPID), PID_POSITION, &(c->param->SickCali_XInparam));
PID_init(&(c->pid.SickCaliVxOutPID), PID_POSITION, &(c->param->SickCali_XOutparam));
PID_init(&(c->pid.SickCaliVyInPID), PID_POSITION, &(c->param->SickCali_YInparam));
PID_init(&(c->pid.SickCaliVyOutPID), PID_POSITION, &(c->param->SickCali_YOutparam));
PID_init(&(c->pid.Mid360AngleInPID), PID_POSITION, &(c->param->Mid360AngleInPID_param));
PID_init(&(c->pid.Mid360AngleOutPID), PID_POSITION, &(c->param->Mid360AngleOutPID_param));
/*修正 */
PID_init(&(c->pid.Chassis_AngleAdjust), PID_POSITION, &(c->param->Chassis_AngleAdjust_param));
LowPassFilter2p_Init(&(c->filled[0]), target_freq, 80.0f); // 角加速度滤波 LowPassFilter2p_Init(&(c->filled[0]), target_freq, 80.0f); // 角加速度滤波
LowPassFilter2p_Init(&(c->filled[1]), target_freq, 80.0f); // w滤波 LowPassFilter2p_Init(&(c->filled[1]), target_freq, 50.0f);
LowPassFilter2p_Init(&(c->filled[2]), target_freq, 80.0f); // y滤波 LowPassFilter2p_Init(&(c->filled[2]), target_freq, 80.0f);
LowPassFilter2p_Init(&(c->filled[3]), target_freq, 80.0f); // x滤波 LowPassFilter2p_Init(&(c->filled[3]), target_freq, 80.0f);
KalmanCreate(&c->extKalman[0],10,1);
KalmanCreate(&c->extKalman[1],10,1);
KalmanCreate(&c->extKalman[2],10,1);
c->sick_cali .sickparam=c->param ->sickparam ; c->sick_cali .sickparam=c->param ->sickparam ;
c->ang_cail.is_open=1;
return CHASSIS_OK; return CHASSIS_OK;
} }
fp32 gyro_kp = 1.0f; //void Chassis_speed_calculate(Chassis_t *c, fp32 Vx, fp32 Vy, fp32 Vw) {
fp32 PIAN = 0; // fp64 Nor_Vx, Nor_Vy;
fp32 pian_yaw; // normalize_vector(Vx, Vy, &Nor_Vx, &Nor_Vy);
//
// c->hopemotorout.OmniSpeedOut[0] = -Nor_Vx + Nor_Vy + Vw+c->ang_cail.out; // 右前
// c->hopemotorout.OmniSpeedOut[1] = -Nor_Vx - Nor_Vy + Vw+c->ang_cail.out; // 右后
// c->hopemotorout.OmniSpeedOut[2] = Nor_Vx - Nor_Vy + Vw +c->ang_cail.out; // 左后
// c->hopemotorout.OmniSpeedOut[3] = Nor_Vx + Nor_Vy + Vw +c->ang_cail.out; // 左前
//
// Chassis_AngleCompensate(c);
//}
void Chassis_speed_calculate(Chassis_t *c, fp32 Vx, fp32 Vy, fp32 Vw) { void Chassis_speed_calculate(Chassis_t *c, fp32 Vx, fp32 Vy, fp32 Vw) {
fp64 Nor_Vx, Nor_Vy; fp64 Nor_Vx, Nor_Vy;
normalize_vector(Vx, Vy, &Nor_Vx, &Nor_Vy); normalize_vector(Vx, Vy, &Nor_Vx, &Nor_Vy);
ChassisHeadlessMode(&Nor_Vx,&Nor_Vy,c->pos088.imu_eulr.yaw);
c->hopemotorout.OmniSpeedOut[0] = -Nor_Vx + Nor_Vy + Vw+pian_yaw; // 右前 c->hopemotorout.OmniSpeedOut[0] = -Nor_Vx + Nor_Vy + Vw+c->ang_cail.out; // 右前
c->hopemotorout.OmniSpeedOut[1] = -Nor_Vx - Nor_Vy + Vw+pian_yaw; // 右后 c->hopemotorout.OmniSpeedOut[1] = -Nor_Vx - Nor_Vy + Vw+c->ang_cail.out; // 右后
c->hopemotorout.OmniSpeedOut[2] = Nor_Vx - Nor_Vy + Vw +pian_yaw; // 左后 c->hopemotorout.OmniSpeedOut[2] = Nor_Vx - Nor_Vy + Vw +c->ang_cail.out; // 左后
c->hopemotorout.OmniSpeedOut[3] = Nor_Vx + Nor_Vy + Vw +pian_yaw; // 左前 c->hopemotorout.OmniSpeedOut[3] = Nor_Vx + Nor_Vy + Vw +c->ang_cail.out; // 左前
Chassis_AngleCompensate(c); Chassis_AngleCompensate(c);
@ -84,14 +114,20 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
c->pos088.bmi088.filtered_gyro.z = LowPassFilter2p_Apply(&(c->filled[0]), c->pos088.bmi088.gyro.z); c->pos088.bmi088.filtered_gyro.z = LowPassFilter2p_Apply(&(c->filled[0]), c->pos088.bmi088.gyro.z);
/*初始数据*/ /*初始数据*/
c->move_vec.Vw = 0;
c->to_nuc.send = 0;
c->sick_cali.is_reach = 0;
c->move_vec.Vw=0;
c->move_vec.Vx = 0; c->move_vec.Vx = 0;
c->move_vec.Vy = 0; c->move_vec.Vy = 0;
c->NUC_send.send[0] = 0;
c->sick_cali.is_reach = 0;
switch (c->chassis_ctrl.ctrl) { switch (c->chassis_ctrl.ctrl) {
case RCcontrol: case RCcontrol:
c->to_nuc.send = 0;
switch (c->chassis_ctrl.mode) { switch (c->chassis_ctrl.mode) {
case Normal: case Normal:
c->move_vec.Vw = ctrl->Vw * 8000; c->move_vec.Vw = ctrl->Vw * 8000;
@ -107,41 +143,71 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
c->move_vec.Vw = ctrl->Vw * 6000; c->move_vec.Vw = ctrl->Vw * 6000;
break; break;
case Chassis_Adjust: case Chassis_Adjust_Sick:
sick_calibration(c, ctrl, out); if(abs(ctrl->cmd_MID360.angle)<1.0f){
c->NUC_send.send[0] = (c->sick_cali.is_reach == 1) ? 1 : 0; c->move_vec.Vx = ctrl->cmd_MID360.dis * 3000;
break; }
default: if(-ctrl->cmd_MID360.dis>0.02f){
c->move_vec.Vw = -ctrl->cmd_MID360.angle * 1000;
}
// sick_calibration(c, ctrl, out);
// c->to_nuc.send = (c->sick_cali.is_reach == 1) ? 1 : 0;
break; break;
} }
break; break;
case AUTO: case AUTO:
switch (c->chassis_ctrl.mode) { switch (c->chassis_ctrl.mode) {
case AUTO_MID360: case AUTO_MID360:
c->move_vec.Vw = ctrl->cmd_MID360.posw * 1000; c->move_vec.Vw = ctrl->Vw * 8000;
c->move_vec.Vy = -ctrl->cmd_MID360.posy * 1000; c->move_vec.Vx = ctrl->Vy * 8000;
c->move_vec.Vx = -ctrl->cmd_MID360.posx * 1000; c->move_vec.Vy = ctrl->Vx * 8000;
// c->move_vec.Vw = -ctrl->cmd_MID360.angle * 3000;
// abs_limit_fp(&c->move_vec.Vw, 5000.0f);
break;
case AUTO_MID360_Pitch:
c->move_vec.Vw = -ctrl->cmd_MID360.angle * 3000;
c->move_vec.Vx = ctrl->Vy * 8000;
c->move_vec.Vy = ctrl->Vx * 8000;
abs_limit_fp(&c->move_vec.Vw, 5000.0f);
break;
case REPOSITION:
c->to_nuc.send = 1;
break;
default:
break;
}
case PASS_BALL:
switch (c->chassis_ctrl.mode)
{
case PB_UP:
case PB_MID:
case PB_DOWN:
c->move_vec.Vw = -ctrl->cmd_MID360.angle * 1000;
abs_limit_fp(&c->move_vec.Vx, 5000.0f); abs_limit_fp(&c->move_vec.Vx, 5000.0f);
abs_limit_fp(&c->move_vec.Vy, 5000.0f); abs_limit_fp(&c->move_vec.Vy, 5000.0f);
abs_limit_fp(&c->move_vec.Vw, 5000.0f); abs_limit_fp(&c->move_vec.Vw, 5000.0f);
c->NUC_send.send[0] = 1; break ;
break;
case AUTO_MID360_Pitch:
break;
case AUTO_MID360_Adjust:
c->move_vec.Vw = ctrl->Vw * 6000;
c->move_vec.Vx = ctrl->Vy * 6000;
c->move_vec.Vy = ctrl->Vx * 6000;
break;
default:
break;
} }
break;
case RELAXED:
c->move_vec.Vw = 0;
c->move_vec.Vx = 0;
c->move_vec.Vy = 0;
break ;
break;
break; break;
default: default:
@ -149,6 +215,7 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
break; break;
} }
// 电机速度限幅 // 电机速度限幅
abs_limit_fp(&c->move_vec.Vx, 6000.0f); abs_limit_fp(&c->move_vec.Vx, 6000.0f);
abs_limit_fp(&c->move_vec.Vy, 6000.0f); abs_limit_fp(&c->move_vec.Vy, 6000.0f);
@ -163,7 +230,7 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
out->motor_CHASSIS3508.as_array[i] = c->final_out.final_3508out[i]; out->motor_CHASSIS3508.as_array[i] = c->final_out.final_3508out[i];
} }
c->vofa_send[0] = (fp32)c->sick_cali.sick_dis[1] - c->sick_cali.sick_dis[2];
return CHASSIS_OK; return CHASSIS_OK;
@ -180,33 +247,50 @@ sick2,左1
*/ */
fp32 diff_yaw,diff_y,diff_x;
int8_t sick_calibration(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) int8_t sick_calibration(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out)
{ {
if (c == NULL) return CHASSIS_ERR_NULL; if (c == NULL) return CHASSIS_ERR_NULL;
if (ctrl == NULL) return CHASSIS_ERR_NULL; if (ctrl == NULL) return CHASSIS_ERR_NULL;
fp32 delta_w,delta_x,delta_y;
fp32 sick0 = c->sick_cali.sick_dis[0];
fp32 sick0 = c->sick_cali.sick_dis[0] ;
fp32 sick1 = c->sick_cali.sick_dis[1]; fp32 sick1 = c->sick_cali.sick_dis[1];
fp32 sick2 = c->sick_cali.sick_dis[2]; fp32 sick2 = c->sick_cali.sick_dis[2];
const sickparam_t *param = &c->sick_cali.sickparam; const sickparam_t *param = &c->sick_cali.sickparam;
fp32 diff_yaw = -(fp32)(sick1 - sick2);
fp32 diff_y = -(fp32)(sick1 - c->sick_cali.sickparam.y_set);
fp32 diff_x = (fp32)(sick0 - c->sick_cali.sickparam.x_set);
diff_yaw =LowPassFilter2p_Apply(&(c->filled[1]), -(fp32)(sick1 +10 - sick2));
diff_y = LowPassFilter2p_Apply(&(c->filled[2]),-(fp32)(sick1 - c->sick_cali.sickparam.y_set));
diff_x = LowPassFilter2p_Apply(&(c->filled[3]),(fp32)(sick0 - c->sick_cali.sickparam.x_set));
delta_w=PID_calc(&c->pid.SickCaliWzOutPID, diff_yaw, 0);
delta_x=PID_calc(&c->pid.SickCaliVxOutPID, diff_x, 0);
delta_y=PID_calc(&c->pid.SickCaliVyOutPID, diff_y,0);
if(fabs(diff_yaw)>param->w_error){
c->move_vec.Vw=PID_calc(&c->pid.SickCaliWzInPID,-delta_w,0);
}
// if(fabs(diff_y)>param->xy_error){
//
// c->move_vec.Vy=PID_calc(&c->pid.SickCaliVyInPID,delta_y,0);
// }
// if(fabs(diff_x)>param->xy_error){
//
// c->move_vec.Vx=PID_calc(&c->pid.SickCaliVxInPID,-delta_x,0);
// }
c->move_vec.Vw = (fabsf(diff_yaw) > param->w_error) ? PID_calc(&(c->pid.SickCaliWzPID), diff_yaw, 0) : 0;
c->move_vec.Vx = (fabsf(diff_x) > param->xy_error) ? PID_calc(&(c->pid.SickCaliVxPID), diff_x, 0) : 0;
c->move_vec.Vy = (fabsf(diff_y) > param->xy_error) ? PID_calc(&(c->pid.SickCaliVyPID), diff_y, 0) : 0;
static uint16_t reach_cnt = 0; static uint16_t reach_cnt = 0;
if (fabsf(diff_yaw) <= param->w_error && if (fabsf(diff_yaw) <= param->w_error &&
fabsf(diff_x) <= param->xy_error && fabsf(diff_x) <= param->xy_error &&
fabsf(diff_y) <= param->xy_error) { fabsf(diff_y) <= param->xy_error) {
reach_cnt++; reach_cnt++;
if (reach_cnt >= 50) { if (reach_cnt >= 20) {
c->sick_cali.is_reach = 1; c->sick_cali.is_reach = 1;
@ -219,37 +303,25 @@ int8_t sick_calibration(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out)
return CHASSIS_OK; return CHASSIS_OK;
} }
pid_type_def pid;
pid_param_t pid_param={
.p = 10.0f,
.i = 0.00f,
.d = 0.00f,
.i_limit = 2000.0f,
.out_limit =5000.0f,
};
fp32 cur_angle,pian_angel;
int8_t Chassis_AngleCompensate(Chassis_t *c) int8_t Chassis_AngleCompensate(Chassis_t *c)
{ {
if (c == NULL) return CHASSIS_ERR_NULL; if (c == NULL) return CHASSIS_ERR_NULL;
// int is_in;//是否初始化 if(c->ang_cail.is_open==0) return 0;
// if(is_in==0){
PID_init(&pid,PID_POSITION,&pid_param);
// is_in=1;
// }
static fp32 pian_angel,cur_angle;
if((c->move_vec.Vx || c->move_vec.Vy) && (c->move_vec.Vw== 0)) if((c->move_vec.Vx || c->move_vec.Vy) && (c->move_vec.Vw== 0))
{ {
pian_angel=cur_angle-AngleChange(DEGREE,c->pos088.imu_eulr.yaw); c->ang_cail.ang_error=cur_angle-AngleChange(DEGREE,c->pos088.imu_eulr.yaw);
} }
else { else {
cur_angle=AngleChange(DEGREE,c->pos088.imu_eulr.yaw); c->ang_cail.ang_cur=AngleChange(DEGREE,c->pos088.imu_eulr.yaw);
pian_angel=0; c->ang_cail.ang_error=0;
} }
pian_yaw = PID_calc(&pid,pian_angel,0); c->ang_cail.out = PID_calc(&c->pid.Chassis_AngleAdjust,pian_angel,0);
} }

58
User/Module/Chassis.h
View File

@ -31,6 +31,7 @@
#include "can_use.h" #include "can_use.h"
#include "cmd.h" #include "cmd.h"
#include "filter.h" #include "filter.h"
#include "kalman.h"
#define CHASSIS_OK (0) #define CHASSIS_OK (0)
#define CHASSIS_ERR (-1) #define CHASSIS_ERR (-1)
@ -72,6 +73,8 @@ typedef struct
/*该部分决定PID的参数整定在config中修改*/ /*该部分决定PID的参数整定在config中修改*/
pid_param_t M3508_param; pid_param_t M3508_param;
pid_param_t M3508_param_other;
pid_param_t chassis_PICKWzPID_HIGN_param; pid_param_t chassis_PICKWzPID_HIGN_param;
pid_param_t chassis_PICKWzPID_LOW_param; pid_param_t chassis_PICKWzPID_LOW_param;
@ -80,9 +83,19 @@ typedef struct
pid_param_t NaviVy_param; pid_param_t NaviVy_param;
pid_param_t NaviVw_param; pid_param_t NaviVw_param;
pid_param_t Sick_CaliWparam; pid_param_t SickCali_WInparam;
pid_param_t Sick_CaliYparam; pid_param_t SickCali_WOutparam;
pid_param_t Sick_CaliXparam; pid_param_t SickCali_YInparam;
pid_param_t SickCali_YOutparam;
pid_param_t SickCali_XInparam;
pid_param_t SickCali_XOutparam;
pid_param_t Chassis_AngleAdjust_param;
pid_param_t Mid360AngleInPID_param;
pid_param_t Mid360AngleOutPID_param;
sickparam_t sickparam; sickparam_t sickparam;
@ -98,11 +111,6 @@ typedef struct
fp32 Vw; fp32 Vw;
fp32 mul;//油门倍率 fp32 mul;//油门倍率
}ChassisMove_Vec; }ChassisMove_Vec;
typedef struct
{
fp32 send[4];
}NUC_send_t;
/** /**
* @brief * @brief
@ -153,27 +161,49 @@ typedef struct{
pid_type_def chassis_PICKWzPID_HIGN; pid_type_def chassis_PICKWzPID_HIGN;
/*存在较低误差*/ /*存在较低误差*/
pid_type_def chassis_PICKWzPID_LOW; pid_type_def chassis_PICKWzPID_LOW;
pid_type_def Chassis_AngleAdjust;
/*sick */
pid_type_def SickCaliWzInPID;
pid_type_def SickCaliWzOutPID;
pid_type_def SickCaliVxInPID;
pid_type_def SickCaliVxOutPID;
pid_type_def SickCaliVyInPID;
pid_type_def SickCaliVyOutPID;
pid_type_def Mid360AngleInPID;
pid_type_def Mid360AngleOutPID;
pid_type_def SickCaliWzPID;
pid_type_def SickCaliVxPID;
pid_type_def SickCaliVyPID;
}pid; }pid;
fp32 vofa_send[8]; fp32 vofa_send[8];
/*卡尔曼滤波*/
extKalman_t extKalman[3];
LowPassFilter2p_t filled[8]; /* 输出滤波器滤波器数组 */
LowPassFilter2p_t filled[6]; /* 输出滤波器滤波器数组 */ /*角度偏转修正 */
struct{
int is_open;
fp32 ang_error;
fp32 ang_cur;
fp32 out;
}ang_cail;
struct { struct {
int32_t sick_dis[4]; //获取到的sick激光值 fp32 sick_dis[4]; //获取到的sick激光值
sickparam_t sickparam; sickparam_t sickparam;
int is_reach; int is_reach;
}sick_cali; }sick_cali;
NUC_send_t NUC_send; struct {
fp32 send;
}to_nuc;
}Chassis_t; }Chassis_t;

245
User/Module/config.c
View File

@ -37,18 +37,18 @@ static const ConfigParam_t param ={
.out_limit = 3000.0f, .out_limit = 3000.0f,
}, },
.Pitch_M2006_angle_param = { .Pitch_M2006_angle_param = {
.p = 600.0f, .p = 1600.0f,
.i = 0.0f, .i = 0.0f,
.d = 3.0f, .d = 3.0f,
.i_limit = 2000.0f, .i_limit = 4000.0f,
.out_limit = 3000.0f, .out_limit = 20000.0f,
}, },
.Pitch_M2006_speed_param={ .Pitch_M2006_speed_param={
.p = 5.0f, .p = 8.5f,
.i = 0.3f, .i = 0.03f,
.d = 0.0f, .d = 0.0f,
.i_limit = 2000.0f, .i_limit = 2000.0f,
.out_limit = 3000.0f, .out_limit = 15000.0f,
}, },
.Pitch_Angle_M2006_speed_param={ //俯仰跑速度环 .Pitch_Angle_M2006_speed_param={ //俯仰跑速度环
.p = 20.0f, .p = 20.0f,
@ -58,28 +58,6 @@ static const ConfigParam_t param ={
.out_limit = 3000.0f, .out_limit = 3000.0f,
}, },
.Dribble_M3508_speed_param={ //三摩擦速度环
.p = 30.0f,
.i = 0.45f,
.d = 0.0f,
.i_limit = 3000.0f,
.out_limit = 5000.0f,
},
.Dribble_translate_M3508_speed_param={//平移用3508速度环
.p = 5.0f,
.i = 0.3f,
.d = 0.0f,
.i_limit = 2000.0f,
.out_limit = 3000.0f,
},
.Dribble_translate_M3508_angle_param= { //平移用3508角度环
.p = 25.0f,
.i = 0.0f,
.d = 1.5f,
.i_limit = 1000.0f,
.out_limit = 3000.0f,
},
.go_cmd={ .go_cmd={
.id =0, .id =0,
@ -93,43 +71,69 @@ static const ConfigParam_t param ={
}, },
/*上层其他参数*/ /*上层其他参数,一些只是初始值，在运行过程中会被更改*/
/*运球*/
.DribbleCfg = {
.dribble_flag=0,
.m3508_init_ang = -5,
.m3508_translate_angle = 1000,
.rev_spd=-2000,
.spd= 4200, // 转动速度
.init_spd=0,
.light_3508_flag=0,//3508平移处的光电，0初始，1到位置
.light_ball_flag=0,//检测球的flag
},
/*投球*/ /*投球*/
.PitchCfg = { .LaunchCfg = {
.m2006_init =-150, //释放的角度 .m2006_init = -140.0f, // M2006初始角度
.m2006_trig =0, //可以拉住发射的角度 .m2006_trig = 0.0f, // M2006触发角度
.go_init = -50, //仅用在go上电，初始位置 .go_pull_pos = -210.0f, // go上升去合并扳机，扳机位置
.go_pull_pos =-214, .go_up_speed = 15.0f, // 上升速度
.Pitch_angle =58, .go_down_speed = 12.0f, // 下降速度
.Pitch_angle = 66, //俯仰角
.pull_speed =5, .go_init = -50
}, },
.PitchCfg = {
.go_init = -50, //仅用在go上电，初始位置
.go_release_pos=-200,
.Pitch_angle =66,
.go_up_speed =15,
.go_down_speed =10,
},
.PassCfg={
.go_wait =-10,
.go_release_pos =-150,
.Pitch_angle=66,
.go_up_speed =15,
.go_down_speed =10,
},
.MID360Cfg={
.go_release_pos=-200, // GO电机发射位置
.go_up_speed=15, // GO电机上升速度
.go_down_speed=10, // GO电机下降速度
},
}, },
.chassis = {/**/ .chassis = {/**/
.M3508_param = { .M3508_param = {
.p = 15.1f, // .p = 15.1f,
.i = 0.02f, // .i = 0.02f,
// .d = 3.2f,
// .i_limit = 200.0f,
// .out_limit =6000.0f,
.p = 20.5f,
.i = 0.00005f,
.d = 3.2f, .d = 3.2f,
.i_limit = 200.0f, .i_limit = 200.0f,
.out_limit =6000.0f, .out_limit =6000.0f,
},
.M3508_param_other= {
.p = 16.5f,
.i = 0.00001f,
.d = 3.2f,
.i_limit = 200.0f,
.out_limit =6000.0f,
}, },
/*视觉*/ /*视觉*/
.chassis_PICKWzPID_HIGN_param ={ .chassis_PICKWzPID_HIGN_param ={
@ -146,32 +150,135 @@ static const ConfigParam_t param ={
.i_limit = 50.0f, .i_limit = 50.0f,
.out_limit =1000.0f, .out_limit =1000.0f,
}, },
.Sick_CaliWparam ={
// .SickCali_WInparam = {
// .p = 3.0f,
// .i = 0.000f,
// .d = 0.0f,
// .i_limit = 500.0f,
// .out_limit = 2000.0f,
// },
// .SickCali_WOutparam = {
// .p = 2.6f,
// .i = 0.0f,
// .d = 0.0f,
// .i_limit = 0.0f,
// .out_limit = 1000.0f,
// },
// .SickCali_YInparam = {
// .p = 1.0f,
// .i = 0.0f,
// .d = 0.0f,
// .i_limit = 0.0f,
// .out_limit = 5000.0f,
// },
// .SickCali_YOutparam = {
// .p = 4.5f,
// .i = 0.0f,
// .d = 0.0f,
// .i_limit = 00.0f,
// .out_limit = 1000.0f,
// },
// .SickCali_XInparam = {
// .p = 1.0f,
// .i = 0.0f,
// .d = 0.0f,
// .i_limit = 0.0f,
// .out_limit = 5000.0f,
// },
// .SickCali_XOutparam = {
// .p = 4.5f,
// .i = 0.00f,
// .d = 0.0f,
// .i_limit = 500.0f,
// .out_limit = 2000.0f,
// },
.SickCali_WInparam = {
.p = 3.0f,
.i = 0.000f,
.d = 0.0f,
.i_limit = 500.0f,
.out_limit = 2000.0f,
},
.SickCali_WOutparam = {
.p = 2.6f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 0.0f,
.out_limit = 1000.0f,
},
.SickCali_YInparam = {
.p = 1.0f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 0.0f,
.out_limit = 5000.0f,
},
.SickCali_YOutparam = {
.p = 4.5f, .p = 4.5f,
.i = 0.005f, .i = 0.0f,
.d = 0.0f,
.i_limit = 00.0f,
.out_limit = 1000.0f,
},
.SickCali_XInparam = {
.p = 1.0f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 0.0f,
.out_limit = 5000.0f,
},
.SickCali_XOutparam = {
.p = 4.5f,
.i = 0.00f,
.d = 0.0f, .d = 0.0f,
.i_limit = 500.0f, .i_limit = 500.0f,
.out_limit = 2000.0f,
},
.Chassis_AngleAdjust_param={
.p = 10.0f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 0.0f,
.out_limit =1000.0f, .out_limit =1000.0f,
}, },
.Sick_CaliYparam ={
.p = 2.9f, .Mid360AngleInPID_param={
.i = 0.0051f, .p = 0.0f,
.i = 0.0f,
.d = 0.0f, .d = 0.0f,
.i_limit = 500.0f, .i_limit = 0.0f,
.out_limit =3000.0f, .out_limit =1000.0f,
}, },
.Sick_CaliXparam ={ .Mid360AngleOutPID_param={
.p = 2.9f, .p = 10.0f,
.i = 0.0065f, .i = 0.0f,
.d = 0.0f, .d = 0.0f,
.i_limit = 500.0f, .i_limit = 0.0f,
.out_limit =3000.0f, .out_limit =1000.0f,
}, },
.sickparam={ .sickparam={
.w_error=5, .w_error=2,
.xy_error=5, .xy_error=3,
.x_set=927, .x_set=10000,
.y_set=1255, .y_set=2000,
}, },

597
User/Module/up.c
View File

@ -3,12 +3,9 @@
#include "user_math.h" #include "user_math.h"
#include "bsp_buzzer.h" #include "bsp_buzzer.h"
#include "bsp_delay.h" #include "bsp_delay.h"
/*接线
上层用到三个光电
1.PE9 发射用 输出高电平 LIGHTA /*接线
2.PE11 运球上3508 输出高电平 LIGHTB
3.PE13 三摩擦检测球 输出高电平LIGHTC
*/ */
@ -26,6 +23,7 @@
// 定义继电器控制 // 定义继电器控制
#define RELAY1_TOGGLE(state) HAL_GPIO_WritePin(GPIOE, RELAY_Pin, (state) ? GPIO_PIN_SET : GPIO_PIN_RESET) #define RELAY1_TOGGLE(state) HAL_GPIO_WritePin(GPIOE, RELAY_Pin, (state) ? GPIO_PIN_SET : GPIO_PIN_RESET)
int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq) int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
{ {
u->param = param; /*初始化参数 */ u->param = param; /*初始化参数 */
@ -42,30 +40,25 @@ int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
PID_init (&u->pid .Pitch_M2006_angle ,PID_POSITION ,&(u->param->Pitch_M2006_angle_param )); PID_init (&u->pid .Pitch_M2006_angle ,PID_POSITION ,&(u->param->Pitch_M2006_angle_param ));
PID_init (&u->pid .Pitch_M2006_speed ,PID_POSITION ,&(u->param->Pitch_M2006_speed_param )); PID_init (&u->pid .Pitch_M2006_speed ,PID_POSITION ,&(u->param->Pitch_M2006_speed_param ));
PID_init (&u->pid .Dribble_translate_M3508_angle ,PID_POSITION ,&(u->param->Dribble_translate_M3508_angle_param ));
PID_init (&u->pid .Dribble_translate_M3508_speed ,PID_POSITION ,&(u->param->Dribble_translate_M3508_speed_param ));
for(int i=0;i<3;i++){
PID_init (&u->pid .Dribble_M3508_speed[i] ,PID_POSITION ,&(u->param->Dribble_M3508_speed_param ));
}
u->go_cmd =u->param ->go_cmd ; u->go_cmd =u->param ->go_cmd ;
LowPassFilter2p_Init(&(u->filled[0]),target_freq,100.0f); LowPassFilter2p_Init(&(u->filled[0]),target_freq,100.0f);
// 初始化上层状态机 if (!u->PitchContext .is_init) {
if (!u->DribbleCont .is_init) { //检查是否为第一次运行状态机，运球 u->PitchContext .PitchConfig = u->param ->PitchCfg ;//赋值
u->DribbleCont .DribbleConfig = u->param ->DribbleCfg ;//赋值 u->PitchContext .PitchState = PITCH_PREPARE; //状态更新
u->DribbleCont .DribbleState = DRIBBLE_PREPARE; u->PitchContext .is_init = 1;
u->DribbleCont .is_init = 1;
} }
if(!u->PassContext.is_init) {
if (!u->Pitch_Cfg .is_init) { u->PassContext .PassCfg = u->param ->PassCfg ;//赋值
u->Pitch_Cfg .PitchConfig = u->param ->PitchCfg ;//赋值 u->PassContext .PassState = PASS_STOP; //状态更新
u->Pitch_Cfg .PitchState = PITCH_PREPARE; //状态更新,开始夹球 u->PassContext .is_init = 1;
u->Pitch_Cfg .is_init = 1;
} }
u->LaunchContext.LaunchCfg = u->param->LaunchCfg;
u->MID360Context.MID360Cfg = u->param->MID360Cfg;
BSP_UART_RegisterCallback(BSP_UART_RS485, BSP_UART_RX_CPLT_CB, USART6_RxCompleteCallback);//注册串口回调函数，bsp层 BSP_UART_RegisterCallback(BSP_UART_RS485, BSP_UART_RX_CPLT_CB, USART6_RxCompleteCallback);//注册串口回调函数，bsp层
} }
@ -95,38 +88,11 @@ int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *c) {
} }
u->cmd =c; u->cmd =c;
/*光电状态更新*/
u->DribbleCont .DribbleConfig .light_ball_flag =HAL_GPIO_ReadPin(LIGHT_C_GPIO_Port ,LIGHT_C_Pin);
u->DribbleCont .DribbleConfig .light_3508_flag =HAL_GPIO_ReadPin(LIGHT_B_GPIO_Port ,LIGHT_B_Pin);
return 0; return 0;
} }
int8_t DJ_processdata(DJmotor_feedback_t *f,fp32 ecd_to_angle)
{
int8_t cnt=0;
fp32 angle ,delta;
angle = f->ecd;
if (f->init_cnt < 50) {
f->orig_angle= angle;
f->last_angle = angle;
f->init_cnt++;
return 0;
}
delta = angle - f->last_angle;
if (delta > 4096) {
f->round_cnt--;
} else if (delta < -4096) {
f->round_cnt++;
}
f->last_angle = angle;
f->total_angle=(f->round_cnt*8191+(angle -f->orig_angle ))*ecd_to_angle;
}
/* /*
这里id范围为1-4， 这里id范围为1-4，
*/ */
@ -152,6 +118,7 @@ int8_t VESC_M5065_Control(UP_t *u,fp32 speed)
u->motor_target .VESC_5065_M1_rpm =speed; u->motor_target .VESC_5065_M1_rpm =speed;
u->motor_target .VESC_5065_M2_rpm =speed; u->motor_target .VESC_5065_M2_rpm =speed;
//根据正反加负号
u->final_out .final_VESC_5065_M1out =-u->motor_target .VESC_5065_M1_rpm; u->final_out .final_VESC_5065_M1out =-u->motor_target .VESC_5065_M1_rpm;
u->final_out .final_VESC_5065_M2out =u->motor_target .VESC_5065_M2_rpm; u->final_out .final_VESC_5065_M2out =u->motor_target .VESC_5065_M2_rpm;
@ -161,71 +128,34 @@ int8_t VESC_M5065_Control(UP_t *u,fp32 speed)
/*go电机控制*/
int8_t GO_SendData(UP_t *u, float pos, float limit)
{
static int is_calibration=0;
static fp32 error=0; //误差量
u->motorfeedback .go_data = get_GO_measure_point() ;
// 读取参数
float tff = u->go_cmd.T; // 前馈力矩
float kp = u->go_cmd.K_P; // 位置刚度
float kd = u->go_cmd.K_W; // 速度阻尼
float q_desired = u->go_cmd.Pos; // 期望位置（rad）
float q_current = u->motorfeedback.go_data->Pos; // 当前角度位置（rad）
float dq_desired = u->go_cmd.W; // 期望角速度（rad/s）
float dq_current = u->motorfeedback.go_data->W; // 当前角速度（rad/s）
// 计算输出力矩 tau
float tau = tff + kp * (q_desired - q_current) + kd * (dq_desired - dq_current);
/*限制最大输入来限制最大输出*/
if (pos - q_current > limit) {
u->go_cmd.Pos = q_current + limit; // 限制位置
}else if (pos - q_current < -limit) {
u->go_cmd.Pos = q_current - limit; // 限制位置
}else {
u->go_cmd.Pos = pos; // 允许位置
}
// 发送数据
GO_M8010_send_data(&u->go_cmd);
return 0;
}
int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out) int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
{ {
/*目标值限位*/
abs_limit_min_max_fp(&u->motor_target.go_shoot ,-215.0f,0.0f);
abs_limit_min_max_fp(&u->motor_target.Pitch_angle ,48.0f,71.5f);
//电机控制 ，传进can //电机控制 ，传进can
DJ_Speed_Control(u,0x201,&u->motorfeedback .DJmotor_feedback[0] ,&u->pid .Dribble_M3508_speed[0],u->motor_target .Dribble_M3508_speed[0]);
DJ_Speed_Control(u,0x202,&u->motorfeedback .DJmotor_feedback[1] ,&u->pid .Dribble_M3508_speed[1],u->motor_target .Dribble_M3508_speed[1]);
DJ_Speed_Control(u,0x203,&u->motorfeedback .DJmotor_feedback[2] ,&u->pid .Dribble_M3508_speed[2],u->motor_target .Dribble_M3508_speed[2]);
DJ_Angle_Control(u,0x204,&u->motorfeedback .DJmotor_feedback[3] ,
&u->pid .Dribble_translate_M3508_angle ,
&u->pid .Dribble_translate_M3508_speed ,
u->motor_target .Dribble_translate_M3508_angle );
DJ_Angle_Control(u,0x205,&u->motorfeedback .DJmotor_feedback[4] , DJ_Angle_Control(u,0x205,&u->motorfeedback .DJmotor_feedback[4] ,
&u->pid .Shoot_M2006angle , &u->pid .Shoot_M2006angle ,
&u->pid .Shoot_M2006speed , &u->pid .Shoot_M2006speed ,
u->motor_target .Shoot_M2006_angle ); u->motor_target .Shoot_M2006_angle );
// DJ_Angle_Control(u,0x206,&u->motorfeedback .DJmotor_feedback[5] ,
// &u->pid .Pitch_M2006_angle , /*俯仰2006，双环，内使用oid角度环，外电机速度环
// &u->pid .Pitch_M2006_speed ,
// u->motor_target .Pitch_M2006_angle ); */
DJ_Speed_Control(u,0x206,&u->motorfeedback .DJmotor_feedback[5],&u->pid .Pitch_M2006_speed, DJ_Speed_Control(u,0x206,&u->motorfeedback .DJmotor_feedback[5],&u->pid .Pitch_M2006_speed,
(PID_calc (&(u->pid .Pitch_M2006_angle ), (PID_calc (&(u->pid .Pitch_M2006_angle ),
u->motorfeedback .Encoder.angle,u->motor_target .Shoot_Pitch_angle)) u->motorfeedback .Encoder.angle,u->motor_target .Pitch_angle)+0.14)
); );
GO_SendData(u, GO_SendData(
u->motor_target .go_shoot, &u->motorfeedback.go_data,
u->Pitch_Cfg .PitchConfig .pull_speed );//对应的拉动速度 &u->go_cmd,u->motor_target .go_shoot,
u->motor_target.go_pull_speed
);
for(int i=0;i<4;i++){ for(int i=0;i<4;i++){
out ->motor_UP3508_Dribble.as_array[i]=u->final_out.DJfinal_out [i] ; out ->motor_UP3508_Dribble.as_array[i]=u->final_out.DJfinal_out [i] ;
@ -242,7 +172,7 @@ int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
} }
fp32 posss=0;
int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c) int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
{ {
@ -251,361 +181,290 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
if(c ==NULL) return 0; if(c ==NULL) return 0;
/*指针简化*/ /*指针简化*/
PitchCfg_t *pitch_cfg = &u->Pitch_Cfg.PitchConfig; PitchCfg_t *pitch_cfg = &u->PitchContext.PitchConfig;
DribbleCfg_t *DribbleCont = &u->DribbleCont.DribbleConfig; LaunchCfg_t *LaunchCfg =&u->LaunchContext.LaunchCfg;
up_motor_target_t *target=&u->motor_target ; up_motor_target_t *target=&u->motor_target ;
/*部分数据更新*/
static int is_pitch=1; static int is_pitch=1;
/*config值限位*/
abs_limit_min_max_fp(&u->Pitch_Cfg.PitchConfig.go_release_pos ,-215.0f,0.0f);
switch (c->CMD_CtrlType ) switch (c->CMD_CtrlType )
{ {
case RCcontrol: //在手动模式下 case RCcontrol: //在手动模式下
switch (c-> CMD_mode ) switch (c-> CMD_mode )
{ {
case Normal : case Normal : //复位，go位置和俯仰角保持LaunchCfg最后修改后的位置，扳机2006角度复位用于发射
/*投篮*/ /*投篮*/
if(is_pitch){ if(is_pitch){
target->go_shoot =pitch_cfg->go_init ; target->go_shoot = LaunchCfg->go_init ;
target->Shoot_Pitch_angle=pitch_cfg->Pitch_angle; target->Pitch_angle = LaunchCfg->Pitch_angle;
is_pitch=0; is_pitch=0;
} //让初始go位置只读一次，后面按调整模式更改的来,后面稳定了之后，可以跟随调整模式下一块删 } //初始上电
target->Shoot_M2006_angle =u->Pitch_Cfg .PitchConfig .m2006_init ; //LaunchCfg->go_up_speed=15;
u->Pitch_Cfg .PitchState = PITCH_PREPARE; //状态更新,开始夹球 // target->Pitch_angle =LaunchCfg->Pitch_angle;
target->go_pull_speed=LaunchCfg->go_up_speed;
/*运球*/ target->Shoot_M2006_angle=LaunchCfg->m2006_init;
u->PitchContext .PitchState = PITCH_PREPARE; //状态更新
RELAY1_TOGGLE(0);//关闭气缸 u->LaunchContext.LaunchState = Launch_Stop;
for(int i=0;i<3;i++){
target->Dribble_M3508_speed[i]=0;
}
target->Dribble_translate_M3508_angle =u->DribbleCont .DribbleConfig .m3508_translate_angle;
u->DribbleCont .DribbleState = DRIBBLE_PREPARE; //重置最初状态
break; break;
case Pitch : case Pitch :
if (u->Pitch_Cfg .PitchState ==PITCH_PREPARE) if (u->PitchContext .PitchState ==PITCH_PREPARE)
{ {
u->Pitch_Cfg .PitchState=PITCH_START;//置标志位用于启动投篮 u->PitchContext .PitchState=PITCH_START;//置标志位用于启动投篮
} }
Pitch_Process(u,out); Pitch_Process(u,out,c);
break ; break ;
case UP_Adjust: //测试用 case UP_Adjust: //测试用,手动使用pitch下的cfg
pitch_cfg ->pull_speed=5;
pitch_cfg ->go_release_pos += c->Vx ; pitch_cfg ->go_release_pos += c->Vx ;
pitch_cfg->Pitch_angle += c->Vy/100; LaunchCfg->Pitch_angle += c->Vy/100;
target->go_shoot=u->Pitch_Cfg.PitchConfig.go_release_pos; target->go_shoot=pitch_cfg ->go_release_pos;
target->Shoot_Pitch_angle=u->Pitch_Cfg.PitchConfig.Pitch_angle; target->Pitch_angle=LaunchCfg->Pitch_angle;
break ; break ;
case Dribble:
{
if(u->DribbleCont.DribbleState== DRIBBLE_PREPARE){
u->DribbleCont .DribbleState=DRIBBLE_PROCESS_DOWN;
}
//光电状态更新
Dribble_Process(u,out);
}break ;
default: default:
break; break;
} }
break;
case AUTO: case AUTO:
switch(c-> CMD_mode) UP_AUTO_Control(u,out,c);
{
case AUTO_MID360_Pitch:
pitch_cfg ->go_init=LowPassFilter2p_Apply(&u->filled[0],c->pos);
if (u->Pitch_Cfg .PitchState ==PITCH_PREPARE)
{
u->Pitch_Cfg .PitchState=PITCH_START;//置标志位用于启动投篮
}
/*根据距离实时计算所需pos*/
pitch_cfg ->go_release_pos=c->pos;
// if(u->CoContext .CoState == CoPITCH){
// u->CoContext.CoState=CoPITCH_MID360;
// }
// Co_Process(u,out);
Pitch_Process(u,out);
break ; break ;
case PASS_BALL:
case AUTO_MID360: Pass_Process(u,out,c);
target->Shoot_M2006_angle =pitch_cfg ->m2006_init ; break;
case RELAXED:
u->Pitch_Cfg .PitchState = PITCH_PREPARE; // target->go_shoot= pitch_cfg .
u->DribbleCont .DribbleState = DRIBBLE_PREPARE; target->Pitch_angle = 66;
target->go_shoot = -190.0f;
//失控下，最好切手动，手动用pitch
pitch_cfg ->go_release_pos=-190;
// target->Pitch_angle = 58;
break ; break ;
} }
return 0; return 0;
}
} }
//复用发射，
int8_t Pitch_Launch_Sequence(UP_t *u,CMD_t *c, LaunchContext_t *LaunchContext,fp32 StartPos,fp32 EndPos,CAN_Output_t *out){
/*电机位置到达判断*/
bool is_GoStartReach=is_reached(u->motorfeedback.go_data.Pos,StartPos,1.0f); //go开始位置
bool is_GoSpeedReach=is_reached(u->motorfeedback.go_data.W,0,1.0f) ; //go速度归零判断
bool is_GoEndReach =(u->motorfeedback .go_data .Pos < -209); //go去上拉钩子位置，判断到达
bool is_HookDone=(u->motorfeedback .DJmotor_feedback [4].total_angle>-5.0f); //2006钩子放下判断
bool is_Shoot=(u->motorfeedback.DJmotor_feedback[4].total_angle<-10);//是否发射判断
int8_t Pitch_Process(UP_t *u, CAN_Output_t *out) switch(LaunchContext->LaunchState){
case Launch_Stop: break;
case Launch_PREPARE:
u->motor_target.go_shoot = StartPos;
if(is_GoStartReach&& is_GoSpeedReach){
//根据位置和速度判断是否到达初始位置
LaunchContext->LaunchState = Launch_START;
}break;
case Launch_START:
u->motor_target.go_pull_speed=LaunchContext->LaunchCfg.go_up_speed;
u->motor_target.go_shoot = u->LaunchContext.LaunchCfg.go_pull_pos;
if(is_GoEndReach){ //检测go位置到达最上面，这里的检测条件可以更改
u->motor_target.Shoot_M2006_angle = u->LaunchContext.LaunchCfg.m2006_trig ;//设置2006角度，关闭
LaunchContext->LaunchState = Launch_TRIGGER;
}break;
case Launch_TRIGGER:
if( is_HookDone ){ //当2006的总角度小于1，可以认为已经勾上,误差为1
u->motor_target.go_pull_speed=LaunchContext->LaunchCfg.go_down_speed;
u->motor_target.go_shoot = EndPos ;
} break;
case Launch_SHOOT_WAIT:
if(is_Shoot) //认为发射
LaunchContext->LaunchState = Launch_DONE;
break;
default:break;
}
}
int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c)
{ {
/*简化*/
PitchCfg_t *cfg = &u->Pitch_Cfg.PitchConfig; PitchCfg_t *cfg = &u->PitchContext.PitchConfig;
PitchState_t *state =&u->Pitch_Cfg .PitchState; PitchState_t *state =&u->PitchContext .PitchState;
up_motor_target_t *target=&u->motor_target ; up_motor_target_t *target=&u->motor_target ;
LaunchContext_t *LaunchContext = &u->LaunchContext;
// 可根据实际需要传入不同的起始和末位置,起始：当前位置
LaunchContext->LaunchCfg.go_up_speed= cfg->go_up_speed;
LaunchContext->LaunchCfg.go_down_speed= cfg->go_down_speed;
abs_limit_min_max_fp(&cfg->go_release_pos ,-215.0f,0.0f);
abs_limit_min_max_fp(&cfg->Pitch_angle,48.0f,67.0f);
Pitch_Launch_Sequence(u, c,LaunchContext, u->motorfeedback.go_data.Pos, cfg->go_release_pos, out);
switch(*state){ switch(*state){
case PITCH_START: case PITCH_START:
// u->motor_target .Dribble_translate_M3508_angle =u->DribbleCont .DribbleConfig.m3508_translate_angle;//平行移动 LaunchContext->LaunchState = Launch_PREPARE;
cfg->pull_speed=10;
target->go_shoot = cfg->go_pull_pos;
if(u->motorfeedback .go_data ->Pos < -209){ //检测go位置到达最上面，这里的检测条件可以更改 *state=PITCH_WAIT;
target->Shoot_M2006_angle = cfg->m2006_trig ;//设置2006角度，0 break;
u->Pitch_Cfg .PitchConfig .pull_speed=6;
*state=PITCH_PULL_TRIGGER;
}//更改标志位
break ; case PITCH_WAIT:
case PITCH_PULL_TRIGGER:
if(LaunchContext->LaunchState==Launch_DONE)
*state=PITCH_COMPLETE;
break;
case PITCH_COMPLETE:
if( u->motorfeedback .DJmotor_feedback [4].total_angle>-1) //当2006的总角度小于1，可以认为已经勾上,误差为1 break;
{ default:
target->go_shoot=cfg->go_release_pos; break;
if(is_reached (u->motorfeedback .go_data ->Pos ,target->go_shoot ,1.0f))
{
*state=PITCH_LAUNCHING;
}
}
break ;
case PITCH_LAUNCHING: //等待发射
// *state=PITCH_COMPLETE;
break ;
case PITCH_COMPLETE: //发射完成
break ;
} }
return 0;
return 0;
} }
int8_t Pass_Process(UP_t *u,CAN_Output_t *out,CMD_t *c)
int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
{ {
static int common_speed_flag=0;//是否共速
static int common_speed_reverse_flag=0;//是否共速
DribbleCfg_t *DribbleCfg = &u->DribbleCont.DribbleConfig; PassCfg_t *PassCfg = &u->PassContext.PassCfg;
PassState_t *state = &u->PassContext.PassState;
PassContext_t *PassContext = &u->PassContext;
up_motor_target_t *target=&u->motor_target ; up_motor_target_t *target=&u->motor_target ;
DribbleState_t *DribbleState=&u->DribbleCont.DribbleState; LaunchContext_t *LaunchContext = &u->LaunchContext;
switch (u->DribbleCont.DribbleState) { Pass_Sequence_Check(u,c);
/*俯仰角度，力度转换*/
PassCfg->go_release_pos=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->cmd_MID360.dis),3.3,4.3,&u->PassContext.Curve);
case DRIBBLE_TRANSLATE:
target->Dribble_translate_M3508_angle =DribbleCfg->m3508_translate_angle;//平行移动
if(u->motorfeedback.DJmotor_feedback[3].rpm==0 && u->motorfeedback.DJmotor_feedback[3].total_angle < -500)//速度为0认为卡主 if (u->PassContext.Curve == CURVE_58) {
{
u->DribbleCont .DribbleState=DRIBBLE_PROCESS_DOWN;//到达位置后，转移状态 target->Pitch_angle = 58;
PassCfg->Pitch_angle=58;
} }
else {
target->Pitch_angle = 66;
PassCfg->Pitch_angle=66;
}
abs_limit_min_max_fp(&PassCfg->go_release_pos ,-215.0f,0.0f);
switch (*state) { //遥控器按键进行状态切换
case PASS_STOP:
break; break;
case DRIBBLE_PROCESS_DOWN: case PASS_IDLE:
LaunchContext->LaunchState = Launch_PREPARE;
break;
case PASS_PREPARE:
target->go_pull_speed=PassCfg->go_up_speed;
target->Dribble_M3508_speed[0]=DribbleCfg->spd; Pitch_Launch_Sequence(u,c,LaunchContext,u->motorfeedback.go_data.Pos,PassCfg->go_wait,out);
target->Dribble_M3508_speed[1]=DribbleCfg->spd;
target->Dribble_M3508_speed[2]=DribbleCfg->spd;
if(is_reached_multiple(u->motorfeedback .DJmotor_feedback [0].rpm,
u->motorfeedback .DJmotor_feedback [1].rpm,
u->motorfeedback .DJmotor_feedback [2].rpm,
DribbleCfg->spd,
DribbleCfg->spd,
DribbleCfg->spd, 50.0f,50)
) {
RELAY1_TOGGLE(1); //速度达到后打开气缸
common_speed_flag =1;
}
if(common_speed_flag){
if(u->DribbleCont .DribbleConfig .light_ball_flag == 0){//球下落检测，反转
target->Dribble_M3508_speed[0]=DribbleCfg->rev_spd;
target->Dribble_M3508_speed[1]=DribbleCfg->rev_spd;
target->Dribble_M3508_speed[2]=DribbleCfg->rev_spd;
*DribbleState=DRIBBLE_PROCESS_UP;
}
}
break; break;
case DRIBBLE_PROCESS_UP: case PASS_START:
common_speed_flag =0; if(LaunchContext->LaunchState == Launch_TRIGGER){
target->go_pull_speed=PassCfg->go_down_speed;
if((u->motorfeedback .DJmotor_feedback [0].rpm<-500)&& target->go_shoot = PassCfg->go_release_pos ;
(u->motorfeedback .DJmotor_feedback [1].rpm<-500)&&
(u->motorfeedback .DJmotor_feedback [2].rpm<-500)
){
common_speed_reverse_flag=1;
}
if(common_speed_reverse_flag){
if(DribbleCfg->light_ball_flag == 1){
*DribbleState=DRIBBLE_DONE;
RELAY1_TOGGLE(0); //关闭气缸
}
} }
break ; break ;
case DRIBBLE_DONE: case PASS_POS_PREPARE:
common_speed_reverse_flag=0; target->Shoot_M2006_angle=LaunchContext->LaunchCfg.m2006_init;//发射
for(int i=0;i<3;i++){
target->Dribble_M3508_speed[i]=DribbleCfg->init_spd ;//三摩擦速度归0
}
/*标志位清零*/
DribbleCfg->light_ball_flag=0;
DribbleCfg->light_3508_flag=0;
break;
case PASS_COMPLETE:
break; break;
default: default:
// 处理未知状态
break; break;
} }
return 0; return 0;
} }
// CoPREPARE, // 准备阶段
// CoTRANSLATE_OUT,//上方平移,去运球 int8_t UP_AUTO_Control(UP_t *u,CAN_Output_t *out,CMD_t *c){
// CoDRIBBLE, //运球阶段 /*自动下数据更新，根据距离切换俯仰角,如需自动调整，放在*/
// CoTRANSLATE_IN,//上方平移，送球发射，回到运球位置 up_motor_target_t *target=&u->motor_target ;
// CoPITCH, //发射 LaunchContext_t *LaunchContext = &u->LaunchContext;
// CoDONE, MID360Context_t *MID360Context=&u->MID360Context;
//int8_t Trans_Process (UP_t *u, CAN_Output_t *out) MID360Cfg_t *MID360Cfg = &u->MID360Context.MID360Cfg;
//{
// u->PitchContext.PitchConfig.go_release_pos =MID360Cfg->go_release_pos;
//} /*俯仰角度，力度转换，加数值限位*/
int8_t Co_Process(UP_t *u, CAN_Output_t *out) if(c->cmd_MID360.dis<1.3f){
{ MID360Cfg->go_release_pos=-150.0f;
switch(u->CoContext .CoState ) }else {
if(u->MID360Context.Curve==CURVE_58){
MID360Cfg->go_release_pos=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->cmd_MID360.dis)+0.20f,3.3,4.3,&u->MID360Context.Curve);
}
else {
MID360Cfg->go_release_pos=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->cmd_MID360.dis)+0.1f,3.3,4.3,&u->MID360Context.Curve);
}
}
if (u->MID360Context.Curve == CURVE_58) {
target->Pitch_angle = 58;
} else {
target->Pitch_angle = 66;
}
abs_limit_min_max_fp(&MID360Cfg->go_release_pos ,-215.0f,0.0f);
LaunchContext->LaunchCfg.go_up_speed=MID360Cfg->go_up_speed;
LaunchContext->LaunchCfg.go_down_speed=MID360Cfg->go_down_speed;
switch(c-> CMD_mode)
{ {
case CoPREPARE:
case AUTO_MID360_Pitch:
if(MID360Context->IsLaunch==0){
MID360Context->IsLaunch=1;
LaunchContext->LaunchState = Launch_PREPARE;
}
Pitch_Launch_Sequence(u,c,LaunchContext,u->motorfeedback.go_data.Pos,MID360Cfg->go_release_pos,out);
break ; break ;
case CoTRANSLATE_OUT:
u->motor_target .Dribble_translate_M3508_angle =u->DribbleCont .DribbleConfig.m3508_translate_angle;//平行移动
if(u->motorfeedback.DJmotor_feedback[3].rpm==0 && u->motorfeedback.DJmotor_feedback[3].total_angle > 500)//速度为0认为卡主 case AUTO_MID360:
{ target->Shoot_M2006_angle= LaunchContext->LaunchCfg.m2006_init;
u->DribbleCont .DribbleState=DRIBBLE_PROCESS_DOWN;//到达位置后，运球转移状态 MID360Context->IsLaunch=0;
u->Pitch_Cfg .PitchState = PITCH_START; // u->cmd_from_nuc.lock=0;
break ;
u->CoContext .CoState =CoDRIBBLE; default:
}
break; break;
case CoDRIBBLE:
Dribble_Process(u,out); //状态停在DRIBBLE_DONE
Pitch_Process(u,out); //状态停在PITCH_LAUNCHING
//状态停在对应位置时,平移去给发射送球
if(u->DribbleCont .DribbleState ==DRIBBLE_DONE && u->Pitch_Cfg.PitchState ==PITCH_LAUNCHING)
{
u->motor_target .Dribble_translate_M3508_angle =u->DribbleCont .DribbleConfig .m3508_init_ang;
}
if(is_reached(u->motorfeedback.DJmotor_feedback[3].total_angle ,u->DribbleCont .DribbleConfig.m3508_init_ang,1.0f))
{
u->motor_target.Dribble_M3508_speed[0]=1000;
u->motor_target.Dribble_M3508_speed[1]=1000;
u->motor_target.Dribble_M3508_speed[2]=1000;
u->CoContext .CoState =CoTRANSLATE_IN;
} }
break ;
case CoTRANSLATE_IN:
if(is_reached_multiple(u->motorfeedback .DJmotor_feedback [0].rpm,
u->motorfeedback .DJmotor_feedback [1].rpm,
u->motorfeedback .DJmotor_feedback [2].rpm,
u->motor_target.Dribble_M3508_speed [0],
u->motor_target.Dribble_M3508_speed [1],
u->motor_target.Dribble_M3508_speed[2],
50.0f,100)
) {
RELAY1_TOGGLE(1); //速度达到后打开气缸,送给发射
} }
if(u->DribbleCont .DribbleConfig.light_ball_flag == 0)
{
u->motor_target.Dribble_M3508_speed[0]=0;
u->motor_target.Dribble_M3508_speed[1]=0;
u->motor_target.Dribble_M3508_speed[2]=0;
u->motor_target .Dribble_translate_M3508_angle =u->DribbleCont .DribbleConfig.m3508_translate_angle;//平行移动
}
if(u->motorfeedback.DJmotor_feedback[3].rpm==0 && u->motorfeedback.DJmotor_feedback[3].total_angle > 900)//速度为0认为卡主,卡到最右端
{
u->CoContext .CoState =CoPITCH;
}
break ;
case CoPITCH:
case CoPITCH_MID360:
u->motor_target .go_shoot=u->Pitch_Cfg.PitchConfig.go_init;
break ;
break ;
}
}

196
User/Module/up.h
View File

@ -13,6 +13,7 @@
#include "vofa.h" #include "vofa.h"
#include "GO_M8010_6_Driver.h" #include "GO_M8010_6_Driver.h"
#include "bsp_usart.h" #include "bsp_usart.h"
#include "up_utils.h"
/* /*
状态机 状态机
@ -27,103 +28,105 @@
*/ */
/*配合过程状态*/ /*共用发射，重复部分*/
typedef enum {
CoPREPARE, // 准备阶段
CoTRANSLATE_OUT,//上方平移,去运球
CoDRIBBLE, //运球和蓄力阶段,再平移回去
CoTRANSLATE_IN,//上方平移，送球发射，回到运球位置
CoPITCH, //发射
CoPITCH_MID360, //雷达发射
CoDONE,
}CoState_t;
/*总配合上下文*/
typedef struct { typedef struct {
CoState_t CoState; fp32 m2006_init; // M2006初始角度
fp32 m2006_trig; // M2006触发角度0,用来合并扳机
fp32 go_init;
fp32 go_pull_pos; // go上升去合并扳机，扳机位置
fp32 go_up_speed;
fp32 go_down_speed;
uint8_t is_init ; fp32 Pitch_angle;
} CoCon_t; } LaunchCfg_t;
typedef enum {
Launch_Stop,
Launch_PREPARE, //
Launch_START, //启动，从指定位置上升，扣动扳机
Launch_TRIGGER, //拉动到指定位置
Launch_SHOOT_WAIT, // 发射等待
Launch_SHOOT,
Launch_DONE,
}LaunchState_t;
typedef struct {
LaunchCfg_t LaunchCfg;
LaunchState_t LaunchState;
} LaunchContext_t;
/* 投球状态定义 */ /* 投球状态定义 */
typedef enum { typedef enum {
PITCH_PREPARE, // 准备阶段 PITCH_PREPARE, // 准备阶段
PITCH_START, //启动，拉扳机 PITCH_START, //启动，拉扳机
PITCH_PULL_TRIGGER, PITCH_WAIT, // 发射等待
PITCH_LAUNCHING, // 发射中
PITCH_COMPLETE // 完成 PITCH_COMPLETE // 完成
} PitchState_t; } PitchState_t;
/* 投球参数配置 */ /* 投球参数配置 */
typedef struct { typedef struct {
fp32 m2006_init; // M2006初始角度
fp32 m2006_trig; // M2006触发角度0,用来合并扳机
fp32 go_init; // GO电机触发位置,初始位置，后续更改用于发射的位置
fp32 go_pull_pos; // go上升去合并扳机，扳机位置
fp32 Pitch_angle;
fp32 pull_speed;//go速度 fp32 go_init; // GO电机触发位置,初始位置，后续更改用于发射的位置
fp32 Pitch_angle; //俯仰角度，以oid为准
fp32 go_release_pos;//go松开，发射位置 fp32 go_release_pos;//go松开，发射位置
fp32 go_up_speed;
fp32 go_down_speed;
} PitchCfg_t; } PitchCfg_t;
//fp32 go_pull_pos;
// fp32 m2006_init; // M2006初始角度
// fp32 m2006_trig; // M2006触发角度
// fp32 go_init; // GO初始位置
// fp32 go_release; // GO释放阈值
// fp32 go_recv; // GO接球位置
// fp32 screw_init; // 螺杆初始值
// fp32 pitch_ang; // 投球角度
// fp32 pull_spd; // 拉动速度
/* 投球控制上下文 */
typedef struct { typedef struct {
PitchState_t PitchState; PitchState_t PitchState;
PitchCfg_t PitchConfig; PitchCfg_t PitchConfig;
uint8_t is_init ; uint8_t is_init ;
} Pitch_Cfg_t; } PitchContext_t;
/*运球*/ /*传球过程 */
typedef enum { typedef enum {
DRIBBLE_PREPARE, // 准备阶段 PASS_STOP, // 停止状态,未进入传球模式
DRIBBLE_START, PASS_IDLE, // 空闲状态，进入未开始
DRIBBLE_TRANSLATE, // 平移过程 PASS_PREPARE, // 传球准备
DRIBBLE_PROCESS_DOWN, // 运球过程，球下落 PASS_START, // 启动传球,拉动到等球位置
DRIBBLE_PROCESS_UP, // 运球过程，球上升 PASS_POS_PREPARE, // 传球位置准备，对准篮筐，go的位置
DRIBBLE_DONE // 运球结束 PASS_COMPLETE // 发射
} DribbleState_t; } PassState_t;
/* 参数配置结构体 */
typedef struct { typedef struct {
int8_t dribble_flag;//当移动三摩擦后为1，否则为0，防止发射撞到
fp32 m3508_init_ang; // 3508平移前位置 fp32 go_wait; // GO等待位置
fp32 m3508_translate_angle; // 平移后位置 fp32 go_release_pos; // GO电机发射位置
fp32 init_spd; fp32 Pitch_angle; // 俯仰角度，以oid为准
fp32 spd; // 转动速度 fp32 go_up_speed; // GO电机上升速度
fp32 rev_spd; fp32 go_down_speed; // GO电机下降速度
} PassCfg_t;
/*光电标志位，初始值均为0，触发为1*/
int light_3508_flag;//3508平移处的光电，0初始，1到位置
int light_ball_flag;//检测球的flag
} DribbleCfg_t;
/* 状态机上下文 */
typedef struct { typedef struct {
DribbleState_t DribbleState; PassState_t PassState;
DribbleCfg_t DribbleConfig; PassCfg_t PassCfg;
CurveType Curve; //当前函数，雷达距离->pos
uint8_t is_init ;
} PassContext_t;
/*雷达自动*/
typedef struct {
fp32 go_release_pos; // GO电机发射位置
fp32 go_up_speed; // GO电机上升速度
fp32 go_down_speed; // GO电机下降速度
} MID360Cfg_t;
typedef struct {
MID360Cfg_t MID360Cfg;
CurveType Curve; //当前函数，雷达距离->pos
int IsLaunch; //是否在发射过程
} MID360Context_t;
uint8_t is_init;
} DribbleCont_t;
typedef struct { typedef struct {
@ -148,14 +151,10 @@ typedef struct
pid_param_t Pitch_Angle_M2006_speed_param; pid_param_t Pitch_Angle_M2006_speed_param;
pid_param_t Dribble_M3508_speed_param;//三摩擦用的速度环 LaunchCfg_t LaunchCfg;
pid_param_t Dribble_translate_M3508_speed_param;//平移用的速度环
pid_param_t Dribble_translate_M3508_angle_param;//平移用的角度环
DribbleCfg_t DribbleCfg;
PitchCfg_t PitchCfg; PitchCfg_t PitchCfg;
PassCfg_t PassCfg;
MID360Cfg_t MID360Cfg;
GO_MotorCmd_t go_cmd; GO_MotorCmd_t go_cmd;
}UP_Param_t; }UP_Param_t;
@ -165,13 +164,10 @@ typedef struct{
fp32 VESC_5065_M2_rpm; fp32 VESC_5065_M2_rpm;
fp32 Shoot_M2006_angle; fp32 Shoot_M2006_angle;
fp32 Pitch_M2006_angle;
fp32 go_shoot; fp32 go_shoot;
fp32 Dribble_M3508_speed[3];//运球转速 fp32 go_pull_speed;
fp32 Dribble_translate_M3508_angle;//平移用的3508转动角度 fp32 Pitch_angle; //以oid的角度为准,目标俯仰角
fp32 Shoot_Pitch_angle;
}up_motor_target_t; }up_motor_target_t;
@ -189,36 +185,20 @@ typedef struct{
pid_type_def Pitch_Angle_M2006_speed; pid_type_def Pitch_Angle_M2006_speed;
pid_type_def Dribble_M3508_speed[3];//三摩擦用的速度环
pid_type_def Dribble_translate_M3508_speed;//平移用的速度环
pid_type_def Dribble_translate_M3508_angle;//平移用的角度环
}up_pid_t; }up_pid_t;
typedef struct
{
fp32 ecd;
fp32 rpm;
uint8_t id;
fp32 orig_angle;
fp32 last_angle;
int32_t round_cnt;
int init_cnt;
fp32 total_angle;
}DJmotor_feedback_t;
typedef struct{ typedef struct{
uint8_t up_task_run; uint8_t up_task_run;
const UP_Param_t *param; const UP_Param_t *param;
CoCon_t CoContext; LaunchContext_t LaunchContext;
/*运球过程*/
DribbleCont_t DribbleCont;
/*投篮过程*/ /*投篮过程*/
Pitch_Cfg_t Pitch_Cfg; PitchContext_t PitchContext;
/*传球过程*/
PassContext_t PassContext;
/*自动过程*/
MID360Context_t MID360Context;
CMD_t *cmd; CMD_t *cmd;
struct{ struct{
@ -227,7 +207,7 @@ typedef struct{
fp32 VESC_5065_M1_rpm; fp32 VESC_5065_M1_rpm;
fp32 VESC_5065_M2_rpm; fp32 VESC_5065_M2_rpm;
}VESC; }VESC;
GO_MotorData_t *go_data;//存放go电机数据 GO_MotorData_t go_data;//存放go电机数据
DJmotor_feedback_t DJmotor_feedback[6]; DJmotor_feedback_t DJmotor_feedback[6];
@ -235,6 +215,7 @@ typedef struct{
uint32_t ecd; uint32_t ecd;
float angle; float angle;
}Encoder; }Encoder;
}motorfeedback; }motorfeedback;
@ -254,7 +235,11 @@ typedef struct{
}final_out; }final_out;
// struct {
// float angle;
// float dis;
// uint8_t lock;
// }cmd_from_nuc;
LowPassFilter2p_t filled[6]; /* 输出滤波器滤波器数组 */ LowPassFilter2p_t filled[6]; /* 输出滤波器滤波器数组 */
fp32 vofa_send[8]; fp32 vofa_send[8];
@ -265,16 +250,17 @@ typedef struct{
int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq); int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq);
int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *c) ; int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *c) ;
int8_t VESC_M5065_Control(UP_t *u,fp32 speed); int8_t VESC_M5065_Control(UP_t *u,fp32 speed);
int8_t GO_SendData(UP_t *u,float pos,float limit);
int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c); int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c);
int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out); int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out);
int8_t DJ_processdata(DJmotor_feedback_t *f,fp32 ecd_to_angle); int8_t DJ_processdata(DJmotor_feedback_t *f,fp32 ecd_to_angle);
int8_t DJ_Angle_Control(UP_t *u,int id,DJmotor_feedback_t *f,pid_type_def *Angle_pid,pid_type_def *Speed_pid,fp32 target_angle); int8_t DJ_Angle_Control(UP_t *u,int id,DJmotor_feedback_t *f,pid_type_def *Angle_pid,pid_type_def *Speed_pid,fp32 target_angle);
int8_t DJ_Speed_Control(UP_t *u,int id,DJmotor_feedback_t *f,pid_type_def *Speed_pid,fp32 target_speed); int8_t DJ_Speed_Control(UP_t *u,int id,DJmotor_feedback_t *f,pid_type_def *Speed_pid,fp32 target_speed);
int8_t Dribble_Process(UP_t *u, CAN_Output_t *out); int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c);
int8_t Pitch_Process(UP_t *u, CAN_Output_t *out); int8_t Pass_Sequence_Check(UP_t *u,CMD_t *c);
int8_t Co_Process(UP_t *u, CAN_Output_t *out); int8_t Pass_Process(UP_t *u,CAN_Output_t *out,CMD_t *c);
int8_t Pitch_Launch_Sequence(UP_t *u,CMD_t *c, LaunchContext_t *LaunchContext,fp32 StartPos,fp32 EndPos,CAN_Output_t *out);
int8_t UP_AUTO_Control(UP_t *u,CAN_Output_t *out,CMD_t *c);
#endif #endif

173
User/Module/up_utils.c Normal file
View File

@ -0,0 +1,173 @@
#include "up_utils.h"
#include "up.h"
int8_t DJ_processdata(DJmotor_feedback_t *f, fp32 ecd_to_angle)
{
fp32 angle, delta;
angle = f->ecd;
// 初始化阶段，记录 offset
if (f->init_cnt < 50) {
f->offset_ecd = (uint16_t)angle; // 记录初始偏移
f->orig_angle = angle - f->offset_ecd; // orig_angle 归零
f->last_angle = angle - f->offset_ecd;
f->init_cnt++;
return 0;
}
// 使用 offset 修正
angle = angle - f->offset_ecd;
delta = angle - f->last_angle;
if (delta > 4096) {
f->round_cnt--;
} else if (delta < -4096) {
f->round_cnt++;
}
f->last_angle = angle;
f->total_angle=(f->round_cnt*8191+(angle -f->orig_angle ))*ecd_to_angle;
}
/*go电机控制*/
int8_t GO_SendData( GO_MotorData_t *go_data,GO_MotorCmd_t *go_cmd,float pos, float limit)
{
*go_data = *get_GO_measure_point() ;
// 读取参数
float tff = go_cmd->T; // 前馈力矩
float kp = go_cmd->K_P; // 位置刚度
float kd = go_cmd->K_W; // 速度阻尼
float q_desired = go_cmd->Pos; // 期望位置（rad）
float q_current = go_data->Pos; // 当前角度位置（rad）
float dq_desired = go_cmd->W; // 期望角速度（rad/s）
float dq_current = go_data->W; // 当前角速度（rad/s）
// 计算输出力矩 tau
float tau = tff + kp * (q_desired - q_current) + kd * (dq_desired - dq_current);
/*限制最大输入来限制最大输出*/
if (pos - q_current > limit) {
go_cmd->Pos = q_current + limit; // 限制位置
}else if (pos - q_current < -limit) {
go_cmd->Pos = q_current - limit; // 限制位置
}else {
go_cmd->Pos = pos; // 允许位置
}
// 发送数据
GO_M8010_send_data(go_cmd);
return 0;
}
// 计算66度曲线（偏上）
static float curve_66(float d) {
return 2.9851104*pow(d,4) -36.164382*pow(d,3) + 159.54844*pow(d,2) -273.62856*d + 43.092452;
}
// 计算58度曲线（偏下）
static float curve_58(float d) {
// return 0.9242f * d * d + 19.4246f * d - 154.9055f;
// return 2.638517*pow(d,4) -47.996138*pow(d,3) + 325.38515*pow(d,2) -950.18155*d + 919.86543;
return 2.7775f * d * d - 0.5798f * d - 113.1488;
}
/*
曲线切换,用于距离和pos拟合
迟滞区x-y
曲线x重合区，根据当前函数和变化方向切换
*/
fp32 ang_58;
fp32 ang_66;
float CurveChange(float d, float x, float y, CurveType *cs)
{
if (*cs == CURVE_66) {
if (d > y) {
*cs = CURVE_58;
}
} else { // CURVE_58
if (d < x) {
*cs = CURVE_66;
}
}
// 根据当前曲线返回结果
if (*cs == CURVE_58) {
ang_58=d;
return curve_58(d);
} else {
ang_66=d;
return curve_66(d);
}
}
int8_t Pass_Sequence_Check(UP_t *u, CMD_t *c) //按键顺序检测，传球，按需求改
{
PassState_t *state = &u->PassContext.PassState;
static enum {
SEQ_IDLE,
SEQ_MID1,
SEQ_UP,
SEQ_MID2,
SEQ_DOWN
} seq = SEQ_IDLE;
switch (seq) {
case SEQ_IDLE:
if (c->CMD_mode == PB_MID) {
seq = SEQ_MID1;
*state = PASS_IDLE;
}
break;
case SEQ_MID1:
if (c->CMD_mode == PB_UP) {
seq = SEQ_UP;
*state = PASS_PREPARE;
} else if (c->CMD_mode != PB_MID) {
seq = SEQ_IDLE;
*state = PASS_STOP;
}
break;
case SEQ_UP:
if (c->CMD_mode == PB_MID) {
seq = SEQ_MID2;
*state = PASS_START;
} else if (c->CMD_mode != PB_UP) {
seq = SEQ_IDLE;
*state = PASS_STOP;
}
break;
case SEQ_MID2:
if (c->CMD_mode == PB_DOWN) {
seq = SEQ_DOWN;
*state = PASS_POS_PREPARE;
} else if (c->CMD_mode != PB_MID) {
seq = SEQ_IDLE;
*state = PASS_STOP;
}
break;
case SEQ_DOWN:
if (c->CMD_mode == PB_MID) {
seq = SEQ_IDLE;
*state = PASS_IDLE; // 或 PASS_COMPLETE，看你需求
} else if (c->CMD_mode != PB_DOWN) {
seq = SEQ_IDLE;
*state = PASS_STOP;
}
break;
}
return 0;
}

58
User/Module/up_utils.h Normal file
View File

@ -0,0 +1,58 @@
#ifndef UP_UTILS_H
#define UP_UTILS_H
#include "gpio.h"
#include "user_math.h"
#include "bsp_buzzer.h"
#include "bsp_delay.h"
#include "struct_typedef.h"
#include "pid.h"
#include "user_math.h"
#include "ahrs.h"
#include "filter.h"
#include "bsp_usart.h"
#include "GO_M8010_6_Driver.h"
#define CAN2_G3508_ID (0x200)
#define GEAR_RATIO_2006 (36) // 2006减速比
#define GEAR_RATIO_3508 (19)
#define CAN_MOTOR_ENC_RES 8191 // 编码器分辨率
#define MOTOR2006_ECD_TO_ANGLE (360.0 / 8191.0 / (GEAR_RATIO_2006)) //2006编码值转轴角度
#define MOTOR3508_ECD_TO_ANGLE (360.0 / 8191.0 / (GEAR_RATIO_3508)) //3508编码值转轴角度
typedef struct
{
fp32 ecd;
fp32 rpm;
uint8_t id;
fp32 orig_angle;
fp32 last_angle;
int32_t round_cnt;
int init_cnt;
fp32 total_angle;
uint16_t offset_ecd;
uint32_t msg_cnt;
}DJmotor_feedback_t;
typedef enum {
CURVE_58,
CURVE_66
} CurveType;
static float curve_58(float d) ;
static float curve_66(float d) ;
float CurveChange(float d, float x, float y, CurveType *cs);
int8_t GO_SendData( GO_MotorData_t *go_data,GO_MotorCmd_t *go_cmd,float pos, float limit);
int8_t DJ_processdata(DJmotor_feedback_t *f,fp32 ecd_to_angle);
#endif

56
User/bsp/protocol.h
View File

@ -14,6 +14,11 @@ extern "C" {
#define HEAD (0xFF) #define HEAD (0xFF)
#define TAIL (0xFE) #define TAIL (0xFE)
#define IMU_ID (0x01)
#define CMD_ID (0x02)
#define TYPE (0x09)
#define NAVI (0x05) #define NAVI (0x05)
#define PICK (0x06) #define PICK (0x06)
@ -32,31 +37,55 @@ typedef struct __attribute__((packed)) {
typedef struct __attribute__((packed)) { typedef struct __attribute__((packed)) {
Protocol_ID_t recv_id;//作为帧头使用确认通信ID正确 Protocol_ID_t recv_id;//作为帧头使用确认通信ID正确
uint8_t status; /* 控制命令 */
struct __attribute__((packed)) { struct __attribute__((packed)) {
float posy; /*pitch轴*/
float posx; /*yaw轴*/
char cmd;
float dis; /*距离*/
}pick;
struct __attribute__((packed)) {
float vx; /* x轴移动速度 */
float vy; /* y轴移动速度 */
float wz; /* z轴转动速度 */ float wz; /* z轴转动速度 */
float dis;
}navi; }navi;
} Protocol_DownDataChassis_t; } Protocol_DownDataChassis_t;
/* 电控 -> 视觉 IMU数据结构体*/
typedef struct __attribute__((packed))
{
struct __attribute__((packed))
{
float x;
float y;
float z;
} gyro; /* 陀螺仪数据 */
struct __attribute__((packed))
{
float x;
float y;
float z;
} accl; /* 四元数 */
struct __attribute__((packed))
{
float q0;
float q1;
float q2;
float q3;
} quat; /* 四元数 */
} Protocol_UpDataIMU_t;
typedef struct __attribute__((packed))
{
uint8_t status;
} Protocol_UpDataCMD_t;
/* 视觉 -> 电控 上层机构数据结构体*/ /* 视觉 -> 电控 上层机构数据结构体*/
typedef struct __attribute__((packed)) { typedef struct __attribute__((packed)) {
float angle; // 角度信息
uint8_t status; /* 控制命令 */ float dis; // 位移信息
} Protocol_DownDataUpper_t; } Protocol_DownDataUpper_t;
typedef struct __attribute__((packed)) { typedef struct __attribute__((packed)) {
Protocol_UpDataMCU_t data; Protocol_UpDataMCU_t data;
} Protocol_UpPackageMCU_t; } Protocol_UpPackageMCU_t;
@ -69,6 +98,7 @@ typedef struct __attribute__((packed)) {
typedef struct __attribute__((packed)) { typedef struct __attribute__((packed)) {
Protocol_DownDataUpper_t data; Protocol_DownDataUpper_t data;
} Protocol_DownPackageUpper_t; } Protocol_DownPackageUpper_t;
#ifdef __cplusplus #ifdef __cplusplus
} }
#endif #endif

75
User/device/can_use.c
View File

@ -2,6 +2,9 @@
/* /*
CAN总线上的设备1到7 CAN总线上的设备1到7
将所有CAN总线上挂载的设备抽象成单个设备进行管理和控制 将所有CAN总线上挂载的设备抽象成单个设备进行管理和控制
底盘4个3508，3个dt35接收板，终端电阻勿开，会出现数据收发异常情况
*/ */
/* Includes ----------------------------------------------------------------- */ /* Includes ----------------------------------------------------------------- */
@ -11,13 +14,9 @@
#include "error_detect.h" #include "error_detect.h"
#define CAN_MOTOR_ENC_RES (8191) /* 电机编码器分辨率 */ #define CAN_MOTOR_ENC_RES (8191) /* 电机编码器分辨率 */
#define CAN_MOTOR_CUR_RES (16384) #define CAN_MOTOR_CUR_RES (16384)
#define CAN_PITCH6020_CTRL_ID (0x2ff) #define CAN_PITCH6020_CTRL_ID (0x2ff)
#define CAN_G3508_CTRL_ALL_CHASSIS_ID (0x200) #define CAN_G3508_CTRL_ALL_CHASSIS_ID (0x200)
/*CAN2上预留>4电机*/
/*CAN2上有6个大疆电机*/
#define CAN_G3508_CTRL_ALL_UP_SHOOT_ID (0x1ff) #define CAN_G3508_CTRL_ALL_UP_SHOOT_ID (0x1ff)
#define CAN_G3508_CTRL_ALL_UP_Dribble_ID (0x200) #define CAN_G3508_CTRL_ALL_UP_Dribble_ID (0x200)
@ -57,20 +56,33 @@ static void CAN_DJIMotor_Decode(CAN_MotorFeedback_t *feedback,
feedback->torque_current = feedback->torque_current =
raw_current * CAN_GM6020_MAX_ABS_CUR / (float)CAN_MOTOR_CUR_RES; raw_current * CAN_GM6020_MAX_ABS_CUR / (float)CAN_MOTOR_CUR_RES;
} }
static void CAN_Sick_Receive(CAN_SickFeedback_t *feedback, static void CAN_Sick_Receive(CAN_SickFeedback_t *feedback,int id,
const uint8_t *raw) { const uint8_t *raw) {
if (feedback == NULL || raw == NULL) return; if (feedback == NULL || raw == NULL) return;
static uint16_t sick_rec[4]; static uint16_t sick_rec[4];
switch (id)
{
case CAN_SICK_ID_1:
feedback->raw_dis[0] = (uint16_t)(raw[0] << 8| raw[1]);
sick_rec[0] = (uint16_t)(raw[0] << 8 | raw[1]); break ;
sick_rec[1] = (uint16_t)(raw[2] << 8 | raw[3]);
sick_rec[2] = (uint16_t)(raw[4] << 8 | raw[5]); case CAN_SICK_ID_2:
sick_rec[3] = (uint16_t)(raw[6] << 8 | raw[7]); feedback->raw_dis[1] = (uint16_t)(raw[0] << 8| raw[1]);
break ;
case CAN_SICK_ID_3:
feedback->raw_dis[2] = (uint16_t)(raw[0] << 8| raw[1]);
break ;
case CAN_SICK_ID_4:
feedback->raw_dis[3] = (uint16_t)(raw[0] << 8| raw[1]);
break ;
}
feedback ->raw_dis [0]= sick_rec[0];
feedback ->raw_dis [1]= sick_rec[1];
feedback ->raw_dis [2]= sick_rec[2];
feedback ->raw_dis [3]= sick_rec[3];
} }
void CAN_Encoder_Decode(CAN_EncoderFeedback_t *feedback, void CAN_Encoder_Decode(CAN_EncoderFeedback_t *feedback,
const uint8_t *raw) { const uint8_t *raw) {
@ -284,25 +296,6 @@ void CAN_Encoder_Control(CAN_t *can)
return DEVICE_OK; return DEVICE_OK;
} }
//用来问答接收来自sick的数据
void CAN_Sick_Control(CAN_t *can)
{
raw_tx.tx_header.StdId = 0x301;
raw_tx.tx_header.IDE = CAN_ID_STD;
raw_tx.tx_header.DLC = 0x08;
raw_tx.tx_header.RTR = CAN_RTR_DATA;//˽ߝ֡
raw_tx.tx_data[0] = 0x00;
raw_tx.tx_data[1] = 0x00;
raw_tx.tx_data[2] = 0x00;
raw_tx.tx_data[3] = 0x00;
raw_tx.tx_data[4] = 0x00;
raw_tx.tx_data[5] = 0x00;
raw_tx.tx_data[6] = 0x00;
raw_tx.tx_data[7] = 0x00;
HAL_CAN_AddTxMessage(BSP_CAN_GetHandle(can->param->sick),&raw_tx.tx_header,raw_tx.tx_data,&(can->mailbox.chassis));
}
int8_t CAN_VESC_Control(int id,CAN_MotorGroup_e group, CAN_Output_t *output,CAN_t *can){ int8_t CAN_VESC_Control(int id,CAN_MotorGroup_e group, CAN_Output_t *output,CAN_t *can){
if (output == NULL) return DEVICE_ERR_NULL; if (output == NULL) return DEVICE_ERR_NULL;
int Byte[4]; int Byte[4];
@ -374,7 +367,19 @@ int8_t CAN_StoreMsg(CAN_t *can, CAN_RawRx_t *can_rx) {
CAN_DJIMotor_Decode(&(can->motor.chassis_motor3508.as_array[index]), can_rx->rx_data); CAN_DJIMotor_Decode(&(can->motor.chassis_motor3508.as_array[index]), can_rx->rx_data);
detect_hook(CHASSIS3508M1_TOE + index); detect_hook(CHASSIS3508M1_TOE + index);
break; break;
case CAN_SICK_ID_1:
CAN_Sick_Receive(&(can->sickfed),CAN_SICK_ID_1, can_rx->rx_data);
break;
case CAN_SICK_ID_2:
CAN_Sick_Receive(&(can->sickfed),CAN_SICK_ID_2, can_rx->rx_data);
break;
case CAN_SICK_ID_3:
CAN_Sick_Receive(&(can->sickfed),CAN_SICK_ID_3, can_rx->rx_data);
break;
case CAN_SICK_ID_4:
CAN_Sick_Receive(&(can->sickfed),CAN_SICK_ID_4, can_rx->rx_data);
break;
default: default:
break; break;
@ -405,9 +410,7 @@ int8_t CAN_StoreMsg(CAN_t *can, CAN_RawRx_t *can_rx) {
CAN_DJIMotor_Decode(&(can->motor.up_shoot_motor3508.as_array[index]), can_rx->rx_data); CAN_DJIMotor_Decode(&(can->motor.up_shoot_motor3508.as_array[index]), can_rx->rx_data);
break; break;
case CAN_SICK_ID:
CAN_Sick_Receive(&(can->sickfed), can_rx->rx_data);
break;
case CAN_Encoder_ID: case CAN_Encoder_ID:
CAN_Encoder_Decode(&(can->Oid),can_rx->rx_data); CAN_Encoder_Decode(&(can->Oid),can_rx->rx_data);
can->recive_flag |= 1 << 10; can->recive_flag |= 1 << 10;

9
User/device/can_use.h
View File

@ -18,7 +18,10 @@ typedef enum {
CAN_UP_M3508_M5_ID = 0x205, /* 1 */ CAN_UP_M3508_M5_ID = 0x205, /* 1 */
CAN_UP_M3508_M6_ID = 0x206, /* 2 */ CAN_UP_M3508_M6_ID = 0x206, /* 2 */
CAN_SICK_ID=0x305, CAN_SICK_ID_1=0x301,
CAN_SICK_ID_2=0x302,
CAN_SICK_ID_3=0x303,
CAN_SICK_ID_4=0x304,
CAN_Encoder_ID=0x01, CAN_Encoder_ID=0x01,
@ -192,12 +195,10 @@ int8_t CAN_DJIMotor_Control(CAN_MotorGroup_e group,CAN_Output_t *output,CAN_t *c
int8_t CAN_VESC_Control(int id,CAN_MotorGroup_e group, CAN_Output_t *output,CAN_t *can); int8_t CAN_VESC_Control(int id,CAN_MotorGroup_e group, CAN_Output_t *output,CAN_t *can);
void CAN_Sick_Control(CAN_t *can);
void CAN_Encoder_Control(CAN_t *can); void CAN_Encoder_Control(CAN_t *can);
void CAN_Encoder_Decode(CAN_EncoderFeedback_t *feedback, void CAN_Encoder_Decode(CAN_EncoderFeedback_t *feedback,
const uint8_t *raw) ; const uint8_t *raw) ;
static void CAN_Sick_Receive(CAN_SickFeedback_t *feedback, static void CAN_Sick_Receive(CAN_SickFeedback_t *feedback,int id,
const uint8_t *raw) ; const uint8_t *raw) ;
#endif #endif

41
User/device/cmd.c
View File

@ -89,32 +89,23 @@ int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n){
if(cmd == NULL) return -1; if(cmd == NULL) return -1;
if(n == NULL) return -1; if(n == NULL) return -1;
cmd->cmd_status = n->status_fromnuc;
cmd->raw_status = n->ctrl_status;
for (int i = 0; i < 7; ++i) for (int i = 0; i < 7; ++i)
{ // 从最低位到最高位遍历 { // 从最低位到最高位遍历
cmd->status[i] = ((cmd->raw_status) & (1 << i)) ? 1 : 0; cmd->status[i] = ((cmd->raw_status) & (1 << i)) ? 1 : 0;
} }
switch(cmd->cmd_status){
case MID:
cmd->cmd_MID360.posx = n->MID360.vx;
cmd->cmd_MID360.posy = n->MID360.vy;
cmd->cmd_MID360.posw = n->MID360.wz;
cmd->pos =n->MID360 .pos ;
break;
case VISION :
cmd ->cmd_pick .posx =n->camera.data1 ;
cmd ->cmd_pick .posy =n->camera.data2 ;
cmd ->cmd_pick .posw =n->camera.data3 ;
break;
}
cmd->cmd_MID360.angle = n->MID360.angle;
cmd->cmd_MID360.dis = n->MID360.dis;
return 0; return 0;
} }
@ -146,7 +137,7 @@ int8_t CMD_DR16(CMD_t *cmd,const CMD_RC_t *rc) {
if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode =Normal; //左上，右中，无模式 if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode =Normal; //左上，右中，无模式
if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Chassis_Adjust; //左上，右上，手动调整 if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Normal; //左上，右上，
if(rc->dr16.res > 3000) cmd ->CMD_mode =UP_Adjust; if(rc->dr16.res > 3000) cmd ->CMD_mode =UP_Adjust;
} }
@ -165,8 +156,8 @@ int8_t CMD_DR16(CMD_t *cmd,const CMD_RC_t *rc) {
else if(rc->dr16.sw_l==CMD_SW_DOWN) else if(rc->dr16.sw_l==CMD_SW_DOWN)
{ {
cmd ->CMD_CtrlType =RCcontrol; cmd ->CMD_CtrlType =RCcontrol;
if(rc->dr16.sw_r ==CMD_SW_UP) cmd ->CMD_mode =Normal; //左下，右上，投篮 if(rc->dr16.sw_r ==CMD_SW_UP) cmd ->CMD_mode =Normal; //左下，右上，无模式
if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode =Normal; //左下，右中，无模式 if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode = Normal; //左下，右中，无模式
if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Normal; //左下，右上，无模式 if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Normal; //左下，右上，无模式
} }
@ -193,7 +184,7 @@ int8_t CMD_LD(CMD_t *cmd,const CMD_RC_t *rc){
if(cmd ->CMD_CtrlType ==AUTO){ if(cmd ->CMD_CtrlType ==AUTO){
/*自动下的*/ /*自动下的*/
if (rc->LD.key_C ==CMD_SW_UP) cmd ->CMD_mode =AUTO_MID360_Adjust; if (rc->LD.key_C ==CMD_SW_UP) cmd ->CMD_mode =Normal;
else { else {
if(rc->LD.key_E ==CMD_SW_DOWN ) cmd ->CMD_mode =Normal; if(rc->LD.key_E ==CMD_SW_DOWN ) cmd ->CMD_mode =Normal;
@ -204,10 +195,10 @@ int8_t CMD_LD(CMD_t *cmd,const CMD_RC_t *rc){
} }
else if(cmd ->CMD_CtrlType ==RCcontrol){ else if(cmd ->CMD_CtrlType ==RCcontrol){
/*手动下的*/ /*手动下的*/
if(rc->LD.key_G ==CMD_SW_DOWN ) cmd ->CMD_mode=Dribble ; if(rc->LD.key_G ==CMD_SW_DOWN ) cmd ->CMD_mode=Normal ;
else if(rc->LD.key_G ==CMD_SW_UP) cmd ->CMD_mode =Pitch; else if(rc->LD.key_G ==CMD_SW_UP) cmd ->CMD_mode =Pitch;
else if(rc->LD .key_H==CMD_SW_UP) cmd ->CMD_mode =UP_Adjust; else if(rc->LD .key_H==CMD_SW_UP) cmd ->CMD_mode =UP_Adjust;
else if(rc->LD.key_C == CMD_SW_UP) cmd->CMD_mode =Chassis_Adjust; else if(rc->LD.key_C == CMD_SW_UP) cmd->CMD_mode =Chassis_Adjust_Sick;
else cmd ->CMD_mode =Normal; else cmd ->CMD_mode =Normal;
} }
} }

28
User/device/cmd.h
View File

@ -14,6 +14,7 @@
typedef enum{ typedef enum{
RCcontrol,//遥控器控制，左按键上，控制上层 RCcontrol,//遥控器控制，左按键上，控制上层
AUTO, AUTO,
PASS_BALL,//传球模式
RELAXED,//异常模式 RELAXED,//异常模式
}CMD_CtrlType_t; }CMD_CtrlType_t;
@ -25,29 +26,28 @@ typedef enum{
AUTO_MID360, AUTO_MID360,
AUTO_MID360_Pitch, AUTO_MID360_Pitch,
AUTO_MID360_Adjust,//雷达调整 REPOSITION,//雷达调整
UP_Adjust,//上层调整 UP_Adjust,//上层调整
Chassis_Adjust, Chassis_Adjust_Sick,
/*传球占三个,逻辑在up.h里，简化就这样写了*/
PB_UP,
PB_MID,
PB_DOWN,
Dribble , //运球
Pitch, //投篮,底盘锁定 Pitch,
Dribbl_transfer Dribbl_transfer
}CMD_mode_t; }CMD_mode_t;
typedef struct { typedef struct {
uint8_t status_fromnuc;
uint8_t ctrl_status; //取其中每一个二进制位用作通信
struct struct
{ {
fp32 vx;
fp32 vy;
fp32 wz;
fp32 pos;
fp32 angle; fp32 angle;
char flag; fp32 dis;
}MID360; }MID360;
struct struct
@ -142,9 +142,9 @@ typedef struct {
}cmd_pick; }cmd_pick;
struct struct
{ {
fp32 posx; fp32 angle;
fp32 posy; fp32 dis;
fp32 posw;
}cmd_MID360; }cmd_MID360;

228
User/device/nuc.c
View File

@ -2,167 +2,145 @@
#include "crc16.h" #include "crc16.h"
#include <string.h> #include <string.h>
#include "error_detect.h" #include "error_detect.h"
static volatile uint32_t drop_message = 0; volatile uint32_t drop_message = 0;
static osThreadId_t thread_alert; static osThreadId_t thread_alert;
uint8_t nucbuf[31]; uint8_t nucbuf[10];
static char SendBuffer[19]; char SendBuffer[7];
static void NUC_CBLTCallback(void)
static void NUC_IdleCallback(void) { {
osThreadFlagsSet(thread_alert,SIGNAL_NUC_RAW_REDY); osThreadFlagsSet(thread_alert, SIGNAL_NUC_RAW_REDY);
detect_hook(NUC_TOE); // detect_hook(NUC_TOE);
} }
int8_t NUC_Init(NUC_t *nuc){ static void NUC_ERRORCALLBACK(void)
if(nuc == NULL) return DEVICE_ERR_NULL; {
if((thread_alert = osThreadGetId()) == NULL ) return DEVICE_ERR_NULL; NUC_Restart();
BSP_UART_RegisterCallback(BSP_UART_NUC,BSP_UART_IDLE_LINE_CB, uint32_t error_code = HAL_UART_GetError(BSP_UART_GetHandle(BSP_UART_NUC));
NUC_IdleCallback); // osThreadFlagsSet(thread_alert, SIGNAL_NUC_ERROR);
}
int8_t NUC_Init(NUC_t *nuc)
{
if (nuc == NULL)
return DEVICE_ERR_NULL;
if ((thread_alert = osThreadGetId()) == NULL)
return DEVICE_ERR_NULL;
BSP_UART_RegisterCallback(BSP_UART_NUC, BSP_UART_RX_CPLT_CB,
NUC_CBLTCallback);
BSP_UART_RegisterCallback(BSP_UART_NUC, BSP_UART_ERROR_CB,
NUC_ERRORCALLBACK);
return DEVICE_OK; return DEVICE_OK;
} }
int8_t NUC_StartReceiving() { int8_t NUC_StartReceiving()
if (HAL_UARTEx_ReceiveToIdle_DMA(BSP_UART_GetHandle(BSP_UART_NUC), {
if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_NUC),
(uint8_t *)nucbuf, (uint8_t *)nucbuf,
sizeof(nucbuf)) == HAL_OK) sizeof(nucbuf)) == HAL_OK)
return DEVICE_OK; return DEVICE_OK;
return DEVICE_ERR; return DEVICE_ERR;
} }
int8_t NUC_StartSending(fp32 *data) { int8_t NUC_Restart(void)
{
union
{
float x[4];
char data[16];
}instance;
for (int i = 0; i < 4; i++) {
instance.x[i] = data[i];
}
SendBuffer[0] = 0xFC; //发送ID
SendBuffer[1] = 0x01; //控制帧
for(int i = 2; i < 18; i++)
{
SendBuffer[i] = instance.data[i-2];
}
SendBuffer[18] = 0xFD; //结束符
if (HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_NUC),
(uint8_t *)SendBuffer,
sizeof(SendBuffer)) == HAL_OK)
return DEVICE_OK;
return DEVICE_ERR;
}
int8_t NUC_Restart(void) {
__HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_NUC)); __HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_NUC));
__HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_NUC)); __HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_NUC));
return DEVICE_OK; return DEVICE_OK;
} }
bool_t NUC_WaitDmaCplt(void) { bool_t NUC_WaitDmaCplt(void)
return (osThreadFlagsWait(SIGNAL_NUC_RAW_REDY, osFlagsWaitAll,100) == {
return (osThreadFlagsWait(SIGNAL_NUC_RAW_REDY, osFlagsWaitAll, 0) ==
SIGNAL_NUC_RAW_REDY); SIGNAL_NUC_RAW_REDY);
} }
int8_t NUC_RawParse(CMD_NUC_t *n, NUC_t *nuc)
int8_t NUC_RawParse(CMD_NUC_t *n){ {
if(n ==NULL) return DEVICE_ERR_NULL; if (n == NULL)
return DEVICE_ERR_NULL;
union union
{ {
float x[5]; float x[2];
char data[20]; char data[8];
}instance; } instance;
if(nucbuf[0]!=HEAD) goto error; //发送ID不是底盘 if (nucbuf[0] != HEAD)
else{ goto error; // 发送ID不是底盘
n->status_fromnuc =nucbuf[1]; else
n->ctrl_status =nucbuf[2];
switch (n->status_fromnuc)
{ {
case VISION://控制帧0x02 if (nucbuf[9] != TAIL)
/* 协议格式 goto error;
0xFF HEAD
0x02 控制帧
0x01 相机帧
vx fp32
vy fp32
wz fp32
0xFE TAIL
*/
instance.data[3] = nucbuf[3]; instance.data[3] = nucbuf[4];
instance.data[2] = nucbuf[4]; instance.data[2] = nucbuf[3];
instance.data[1] = nucbuf[5]; instance.data[1] = nucbuf[2];
instance.data[0] = nucbuf[6]; instance.data[0] = nucbuf[1];
n->camera .data1 = instance.x[0]; // n->MID360.angle = instance.x[0]; //
instance.data[7] = nucbuf[7]; instance.data[7] = nucbuf[8];
instance.data[6] = nucbuf[8]; instance.data[6] = nucbuf[7];
instance.data[5] = nucbuf[9]; instance.data[5] = nucbuf[6];
instance.data[4] = nucbuf[10]; instance.data[4] = nucbuf[5];
n->camera .data2 = instance.x[1];// n->MID360.dis = instance.x[1]; //
instance.data[11] = nucbuf[11];
instance.data[10] = nucbuf[12];
instance.data[9] = nucbuf[13];
instance.data[8] = nucbuf[14];
n->camera .data3 = instance.x[2];//
break;
case MID ://控制帧0x08
/* 协议格式
0xFF HEAD
0x09 控制帧
0x01 相机帧
vx fp32
vy fp32
wz fp32
0xFE TAIL
*/
if(nucbuf[24]!=TAIL)goto error;
instance.data[3] = nucbuf[6];
instance.data[2] = nucbuf[5];
instance.data[1] = nucbuf[4];
instance.data[0] = nucbuf[3];
n->MID360.vx = instance.x[0]; //
instance.data[7] = nucbuf[10];
instance.data[6] = nucbuf[9];
instance.data[5] = nucbuf[8];
instance.data[4] = nucbuf[7];
n->MID360.vy = instance.x[1];//
instance.data[11] = nucbuf[14];
instance.data[10] = nucbuf[13];
instance.data[9] = nucbuf[12];
instance.data[8] = nucbuf[11];
n->MID360.wz = instance.x[2];//
instance.data[15] = nucbuf[18];
instance.data[14] = nucbuf[17];
instance.data[13] = nucbuf[16];
instance.data[12] = nucbuf[15];
n->MID360.pos = instance.x[3];//
instance.data[19] = nucbuf[22];
instance.data[18] = nucbuf[21];
instance.data[17] = nucbuf[20];
instance.data[16] = nucbuf[19];
n->MID360.angle = instance.x[4];//
n->MID360.flag = nucbuf[23];//
break;
} nuc->unc_online = true; // 设置为在线状态
return DEVICE_OK; return DEVICE_OK;
} }
error: error:
drop_message++; drop_message++;
NUC_Restart();
return DEVICE_ERR; return DEVICE_ERR;
} }
int8_t NUC_HandleOffline(CMD_NUC_t *cmd) int8_t NUC_HandleOffline(CMD_NUC_t *cmd, NUC_t *nuc)
{ {
if(cmd == NULL) return DEVICE_ERR_NULL; if (cmd == NULL)
return DEVICE_ERR_NULL;
nuc->unc_online = false; // 设置为离线状态
memset(cmd, 0, sizeof(*cmd)); memset(cmd, 0, sizeof(*cmd));
return 0; return 0;
} }
int8_t NUC_PackIMU(NUC_t *nuc, const AHRS_Quaternion_t *quat, const AHRS_Accl_t *accl, const AHRS_Gyro_t *gyro){
nuc->to_nuc.imu.head = HEAD;
nuc->to_nuc.imu.id = IMU_ID;
nuc->to_nuc.imu.type = TYPE;
nuc->to_nuc.imu.end = TAIL;
memcpy((void *)&(nuc->to_nuc.imu.package.quat), (const void *)quat,sizeof(*quat));
memcpy((void *)&(nuc->to_nuc.imu.package.gyro), (const void *)gyro,sizeof(*gyro));
memcpy((void *)&(nuc->to_nuc.imu.package.accl), (const void *)accl,sizeof(*accl));
return DEVICE_OK;
}
int8_t NUC_PackCMD(NUC_t *nuc, const NUC_send_t *send){
nuc->to_nuc.cmd.head = HEAD;
nuc->to_nuc.cmd.id = IMU_ID;
nuc->to_nuc.cmd.end = TAIL;
memcpy((void *)&(nuc->to_nuc.cmd.package.status), (const void *)send,sizeof(*send));
/*在这里添加你们的控制命令*/
return DEVICE_OK;
}
int8_t NUC_StartSend(NUC_t *nuc, bool cmd_update){
if (cmd_update) {
// if (HAL_UART_Transmit_DMA(
// BSP_UART_GetHandle(BSP_UART_NUC), (uint8_t *)&(nuc->to_nuc.cmd),
// sizeof(nuc->to_nuc.cmd) + sizeof(nuc->to_nuc.imu)) == HAL_OK)
/*仅发送cmd数据*/
if (HAL_UART_Transmit_DMA(
BSP_UART_GetHandle(BSP_UART_NUC), (uint8_t *)&(nuc->to_nuc.cmd),
sizeof(nuc->to_nuc.cmd) ) == HAL_OK)
return DEVICE_OK;
else
return DEVICE_ERR;
}
// else {
// if (HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_NUC),
// (uint8_t *)&(nuc->to_nuc.imu),
// sizeof(nuc->to_nuc.imu)) == HAL_OK)
// return DEVICE_OK;
// else
// return DEVICE_ERR;
// }
}

43
User/device/nuc.h
View File

@ -6,9 +6,14 @@
#include "bsp_usart.h" #include "bsp_usart.h"
#include "cmd.h" #include "cmd.h"
#include "protocol.h" #include "protocol.h"
#include "Algorithm/ahrs.h"
#include "Module/Chassis.h"
typedef struct
{
fp32 send;
}NUC_send_t;
typedef struct { typedef struct {
uint8_t id; uint8_t id;
@ -16,11 +21,32 @@ typedef struct {
} NUC_UpPackageMCU_t; } NUC_UpPackageMCU_t;
typedef struct { typedef struct {
// osThreadId_t thread_alert; uint8_t head;
uint8_t type;
uint8_t id; // 0x01 IMU帧
Protocol_UpDataIMU_t package;
uint8_t end;
} NUC_UpPackageIMU_t;
typedef struct {
uint8_t head;
uint8_t id;
Protocol_UpDataCMD_t package;
uint8_t end;
} NUC_UpPackageCMD_t;
typedef struct {
// osThreadId_t thread_alert;
bool unc_online; //是否在线
Protocol_DownPackageChassis_t Nucfor_chassis; //接收的数据协议 Protocol_DownPackageChassis_t Nucfor_chassis; //接收的数据协议
NUC_UpPackageMCU_t to_nuc; //发送的数据协议 // NUC_UpPackageMCU_t to_nuc; //发送的数据协议
struct {
NUC_UpPackageIMU_t imu;
NUC_UpPackageCMD_t cmd;
} to_nuc;
uint8_t status;//控制状态 uint8_t status;//控制状态
@ -29,9 +55,14 @@ typedef struct {
int8_t NUC_Init(NUC_t *nuc); int8_t NUC_Init(NUC_t *nuc);
int8_t NUC_StartReceiving(void); int8_t NUC_StartReceiving(void);
int8_t NUC_StartSending(fp32 *data) ;
bool_t NUC_WaitDmaCplt(void); bool_t NUC_WaitDmaCplt(void);
int8_t NUC_RawParse(CMD_NUC_t *n); int8_t NUC_RawParse(CMD_NUC_t *n, NUC_t *nuc);
int8_t NUC_HandleOffline(CMD_NUC_t *cmd); int8_t NUC_HandleOffline(CMD_NUC_t *cmd, NUC_t *nuc);
int8_t NUC_Restart(void);
int8_t NUC_PackIMU(NUC_t *nuc, const AHRS_Quaternion_t *quat, const AHRS_Accl_t *accl, const AHRS_Gyro_t *gyro);
int8_t NUC_PackCMD(NUC_t *nuc, const NUC_send_t *send);
int8_t NUC_StartSend(NUC_t *nuc, bool cmd_update);
#endif #endif

6
User/device/rc.c
View File

@ -1,5 +1,9 @@
/* /*
DR16接收机 遥控
dr16：dbus 乐迪：s-ubs
dr16掉线不输出信号，可以使用超时检测方式判断掉线
即RC_SBUS_WaitDmaCplt
乐迪掉线依然有信号输出，只是部分通道变为特定值，无法使用以上方式
*/ */

4
User/device/vofa.c
View File

@ -59,7 +59,7 @@ void vofa_tx_main(float *data)
/*在下面使用对应的串口发送函数*/ /*在下面使用对应的串口发送函数*/
// HAL_UART_Transmit_DMA(&huart6, packet, sizeof(packet)); // HAL_UART_Transmit_DMA(&huart6, packet, sizeof(packet));
// CDC_Transmit_FS( packet, sizeof(packet)); CDC_Transmit_FS( packet, sizeof(packet));
// osDelay(1); osDelay(1);
} }

10
User/task/atti_esti.c
View File

@ -119,7 +119,7 @@ void Task_AttiEsti(void *argument) {
AHRS_GetEulr(&imu_eulr, &gimbal_ahrs); AHRS_GetEulr(&imu_eulr, &gimbal_ahrs);
detect_hook(IMU_TOE); detect_hook(IMU_TOE);
osKernelUnlock();
osMessageQueueReset(task_runtime.msgq.imu.accl); osMessageQueueReset(task_runtime.msgq.imu.accl);
osMessageQueuePut(task_runtime.msgq.imu.accl, &bmi088.accl, 0, 0); osMessageQueuePut(task_runtime.msgq.imu.accl, &bmi088.accl, 0, 0);
@ -128,7 +128,13 @@ void Task_AttiEsti(void *argument) {
osMessageQueueReset(task_runtime.msgq.imu.eulr); osMessageQueueReset(task_runtime.msgq.imu.eulr);
osMessageQueuePut(task_runtime.msgq.imu.eulr, &imu_eulr, 0, 0); osMessageQueuePut(task_runtime.msgq.imu.eulr, &imu_eulr, 0, 0);
osKernelUnlock();
osMessageQueueReset(task_runtime.msgq.nuc.gyro);
osMessageQueuePut(task_runtime.msgq.nuc.gyro, &bmi088.gyro, 0, 0);
osMessageQueueReset(task_runtime.msgq.nuc.accl);
osMessageQueuePut(task_runtime.msgq.nuc.accl, &bmi088.accl, 0, 0);
osMessageQueueReset(task_runtime.msgq.nuc.quat);
osMessageQueuePut(task_runtime.msgq.nuc.quat, &gimbal_ahrs.quat, 0, 0);
/* PID控制IMU温度，PWM输出 */ /* PID控制IMU温度，PWM输出 */
BSP_PWM_Set(BSP_PWM_IMU_HEAT, BSP_PWM_Set(BSP_PWM_IMU_HEAT,

9
User/task/chassis_task.c
View File

@ -34,7 +34,7 @@ static Chassis_Ctrl_t ctrl;
#endif #endif
fp32 freq;
/** /**
* \brief 底盘任务 * \brief 底盘任务
@ -50,6 +50,7 @@ void Task_Chassis(void *argument)
Chassis_init(&chassis,&(task_runtime.config.config->chassis),TASK_FREQ_CHASSIS); Chassis_init(&chassis,&(task_runtime.config.config->chassis),TASK_FREQ_CHASSIS);
uint32_t tick = osKernelGetTickCount(); uint32_t tick = osKernelGetTickCount();
uint32_t last_tick=tick;
while(1) while(1)
@ -59,6 +60,8 @@ void Task_Chassis(void *argument)
task_runtime.freq.chassis=TASK_FREQ_CHASSIS; task_runtime.freq.chassis=TASK_FREQ_CHASSIS;
task_runtime.last_up_time.chassis=tick; task_runtime.last_up_time.chassis=tick;
#endif #endif
freq=(tick-last_tick)/1000.0f;
last_tick =tick;
/*imu数据获取*/ /*imu数据获取*/
osMessageQueueGet(task_runtime.msgq.imu.eulr, &chassis.pos088.imu_eulr, NULL, 0); osMessageQueueGet(task_runtime.msgq.imu.eulr, &chassis.pos088.imu_eulr, NULL, 0);
@ -88,7 +91,7 @@ void Task_Chassis(void *argument)
osMessageQueueReset(task_runtime.msgq.can.output.chassis3508);//清空队列 osMessageQueueReset(task_runtime.msgq.can.output.chassis3508);//清空队列
osMessageQueuePut(task_runtime.msgq.can.output.chassis3508, &out.motor_CHASSIS3508, 0, 0); //发送数据 osMessageQueuePut(task_runtime.msgq.can.output.chassis3508, &out.motor_CHASSIS3508, 0, 0); //发送数据
osMessageQueueReset(task_runtime.msgq.cmd.raw.nuc_send);//清空队列 osMessageQueueReset(task_runtime.msgq.cmd.raw.nuc_send);//清空队列
osMessageQueuePut(task_runtime.msgq.cmd.raw.nuc_send, &chassis.NUC_send, 0, 0); //发送数据 osMessageQueuePut(task_runtime.msgq.cmd.raw.nuc_send, &chassis.to_nuc, 0, 0); //发送数据
vofa_send[0] = chassis.vofa_send[0]; vofa_send[0] = chassis.vofa_send[0];
vofa_send[1] = chassis.vofa_send[1]; vofa_send[1] = chassis.vofa_send[1];
vofa_send[2] = chassis.vofa_send[2]; vofa_send[2] = chassis.vofa_send[2];
@ -98,7 +101,7 @@ void Task_Chassis(void *argument)
vofa_send[6] = chassis.vofa_send[6]; vofa_send[6] = chassis.vofa_send[6];
vofa_send[7] = chassis.vofa_send[7]; vofa_send[7] = chassis.vofa_send[7];
vofa_tx_main(vofa_send); // vofa_tx_main(vofa_send);
tick += delay_tick; tick += delay_tick;
osDelayUntil(tick); osDelayUntil(tick);

9
User/task/cmd_task.c
View File

@ -40,9 +40,14 @@ void Task_cmd(void *argument){
/*将各任务接收到的原始数据解析为通用的控制命令*/ /*将各任务接收到的原始数据解析为通用的控制命令*/
/*注意，不能将nuc和码盘导航一块使用*/
if(osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc, &Nuc,0 ,0) ==osOK) //nuc
if((osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc, &Nuc,0 ,0) ==osOK)){ //nuc
}
else
{
memset(&Nuc, 0, sizeof(CMD_NUC_t));
}
CMD_ParseNuc(&cmd,&Nuc); CMD_ParseNuc(&cmd,&Nuc);

9
User/task/init.c
View File

@ -11,7 +11,7 @@
#include "task\user_task.h" #include "task\user_task.h"
#include "can_use.h" #include "can_use.h"
#include "cmd.h" #include "cmd.h"
#include "nuc.h"
/* Private typedef ---------------------------------------------------------- */ /* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */ /* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */ /* Private macro ------------------------------------------------------------ */
@ -64,6 +64,13 @@ void Task_Init(void *argument) {
task_runtime.msgq.can.output.up_dribble_3508 = task_runtime.msgq.can.output.up_dribble_3508 =
osMessageQueueNew(2u, sizeof(CAN_DJIOutput_t), NULL); osMessageQueueNew(2u, sizeof(CAN_DJIOutput_t), NULL);
/*nuc*/
task_runtime.msgq.nuc.quat =
osMessageQueueNew(2u, sizeof(AHRS_Quaternion_t), NULL);
task_runtime.msgq.nuc.accl =
osMessageQueueNew(2u, sizeof(AHRS_Accl_t), NULL);
task_runtime.msgq.nuc.gyro =
osMessageQueueNew(2u, sizeof(AHRS_Gyro_t), NULL);
/* imu */ /* imu */

54
User/task/nuc_task.c
View File

@ -5,54 +5,82 @@
NUC_t nuc_raw; NUC_t nuc_raw;
CMD_NUC_t cmd_fromnuc; CMD_NUC_t cmd_fromnuc;
AHRS_Quaternion_t quat;
AHRS_Accl_t accl;
AHRS_Gyro_t gyro;
NUC_send_t NUC_send; NUC_send_t NUC_send;
#else #else
static NUC_t nuc_raw; static NUC_t nuc_raw;
static CMD_NUC_t cmd_fromnuc; static CMD_NUC_t cmd_fromnuc;
static AHRS_Quaternion_t quat;
static AHRS_Accl_t accl;
static AHRS_Gyro_t gyro;
NUC_send_t NUC_send; NUC_send_t NUC_send;
#endif #endif
fp32 send[4]={11.0f,45.0,1.f,4.0f}; fp32 send[4]={11.0f,45.0,1.f,4.0f};
int a=0;
void Task_nuc(void *argument){ void Task_nuc(void *argument){
(void)argument; /**/ (void)argument; /**/
// osDelay(TASK_INIT_DELAY_NUC); osDelay(TASK_INIT_DELAY_NUC);
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_NUC; const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_NUC;
NUC_Init(&nuc_raw); NUC_Init(&nuc_raw);
uint32_t tick = osKernelGetTickCount(); uint32_t tick = osKernelGetTickCount();
uint32_t last_online_tick = tick;
while (1) while (1)
{ {
#ifdef DEBUG #ifdef DEBUG
task_runtime.stack_water_mark.nuc= osThreadGetStackSpace(osThreadGetId()); task_runtime.stack_water_mark.nuc= osThreadGetStackSpace(osThreadGetId());
task_runtime .freq.nuc = TASK_FREQ_NUC;
task_runtime.last_up_time.nuc= tick;
#endif #endif
NUC_StartReceiving(); NUC_StartReceiving();
NUC_StartSending(NUC_send.send);
// NUC_StartSending (send);
if (NUC_WaitDmaCplt()){ if (NUC_WaitDmaCplt()){
NUC_RawParse(&cmd_fromnuc); NUC_RawParse(&cmd_fromnuc, &nuc_raw);
last_online_tick = tick;
} }
else{ else{
NUC_HandleOffline(&cmd_fromnuc); if (tick - last_online_tick > 300)
NUC_HandleOffline(&cmd_fromnuc, &nuc_raw);
} }
if (nuc_raw.unc_online) {
osMessageQueueReset(task_runtime.msgq.cmd.raw.nuc); osMessageQueueReset(task_runtime.msgq.cmd.raw.nuc);
osMessageQueuePut(task_runtime.msgq.cmd.raw.nuc,&(cmd_fromnuc),0,0); osMessageQueuePut(task_runtime.msgq.cmd.raw.nuc,&(cmd_fromnuc),0,0);
osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc_send, &NUC_send, NULL, 0); }
else
osMessageQueueReset(task_runtime.msgq.cmd.raw.nuc);
osMessageQueueGet(task_runtime.msgq.nuc.quat, &(quat), NULL, 0);
osMessageQueueGet(task_runtime.msgq.nuc.accl, &(accl), NULL, 0);
osMessageQueueGet(task_runtime.msgq.nuc.gyro, &(gyro), NULL, 0);
bool cmd_update = (osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc_send,
&(NUC_send), NULL, 0) == osOK);
// NUC_PackIMU(&nuc_raw, &quat, &accl, &gyro);
if (cmd_update) {
NUC_PackCMD(&nuc_raw,&NUC_send);
NUC_StartSend(&nuc_raw,cmd_update);
}
// NUC_StartSend(&nuc_raw, cmd_update);
tick += delay_tick; /* 计算下一个唤醒时刄1ķ*/ tick += delay_tick; /* 计算下一个唤醒时刄1ķ*/
osDelayUntil(tick); osDelayUntil(tick);
} }
} }

3
User/task/rc_task.c
View File

@ -33,7 +33,7 @@ static LD_Data_t LD;
* \brief dr16接收机 * \brief dr16接收机
* *
* \param argument 未使用 * \param argument 未使用
*/int aaaaaaaaaaaaaaaaaaa=0; */
void Task_rc(void *argument) { void Task_rc(void *argument) {
(void)argument; /* 未使用，消除警告 */ (void)argument; /* 未使用，消除警告 */
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_RC; const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_RC;
@ -48,7 +48,6 @@ void Task_rc(void *argument) {
#endif #endif
/* 开启DMA */ /* 开启DMA */
RC_StartDmaRecv(); RC_StartDmaRecv();
aaaaaaaaaaaaaaaaaaa++;
if (RC_WaitDmaCplt(30)) { if (RC_WaitDmaCplt(30)) {
RC_ParseRC(&dr16,&LD_raw,&LD, &cmd_rc); RC_ParseRC(&dr16,&LD_raw,&LD, &cmd_rc);

7
User/task/up_task.c
View File

@ -91,8 +91,11 @@ void Task_up(void *argument)
osMessageQueuePut(task_runtime.msgq.can.output.shoot5065 ,&UP_CAN_out.chassis5065, 0, 0); osMessageQueuePut(task_runtime.msgq.can.output.shoot5065 ,&UP_CAN_out.chassis5065, 0, 0);
// vofa_send [0]=UP.vofa_send [0]; vofa_send [0]=UP.vofa_send [0];
// vofa_send [1]=UP.vofa_send [1]; vofa_send [1]=UP.vofa_send [1];
vofa_send [2]=UP.vofa_send [2];
vofa_send [3]=UP.vofa_send [3];
vofa_send [4]=UP.vofa_send [4];
vofa_tx_main (vofa_send); vofa_tx_main (vofa_send);
tick += delay_tick; tick += delay_tick;

15
User/task/user_task.h
View File

@ -13,14 +13,14 @@
/* 所有任务都要定义自己的任务运行频率 */ /* 所有任务都要定义自己的任务运行频率 */
// 分配的频率该如何给定?
#define TASK_FREQ_CHASSIS (900u) #define TASK_FREQ_CHASSIS (900u)
#define TASK_FREQ_UP (900u) #define TASK_FREQ_UP (900u)
#define TASK_FREQ_CTRL_CMD (500u) #define TASK_FREQ_CTRL_CMD (100u)
#define TASK_FREQ_NUC (500u) #define TASK_FREQ_NUC (100u)
#define TASK_FREQ_CAN (1000u) #define TASK_FREQ_CAN (1000u)
#define TASK_FREQ_RC (1000u) #define TASK_FREQ_RC (100u)
#define TASK_FREQ_ERROR_DTC (3u) #define TASK_FREQ_ERROR_DTC (3u)
@ -54,13 +54,18 @@ typedef struct {
osMessageQueueId_t eulr; /* 姿态结算得到欧拉角 */ osMessageQueueId_t eulr; /* 姿态结算得到欧拉角 */
} imu; } imu;
/* 控制指令 */ /* 控制指令 */
struct {
osMessageQueueId_t quat; /* 姿态解算得到 */
osMessageQueueId_t accl; /* IMU读取 */
osMessageQueueId_t gyro; /* IMU读取 */
} nuc;
struct { struct {
struct { struct {
osMessageQueueId_t rc; osMessageQueueId_t rc;
osMessageQueueId_t nuc; // osMessageQueueId_t nuc; //
osMessageQueueId_t eulr;/*姿态结算得到的欧拉角*/ osMessageQueueId_t eulr;/*姿态结算得到的欧拉角*/
osMessageQueueId_t nuc_send; //给nuc发 osMessageQueueId_t nuc_send; //给nuc发
}raw; }raw;
/*控制分离*/ /*控制分离*/