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

Compare commits

base: LYL:410d62198f1ecdb41a6b1fa3efff6f6dc828166e
Branches Tags
LYL:main
LYL:ballgame
LYL:maingame
LYL:mid360_plan2
LYL:6.23
LYL:Cooperation
LYL:chassis
...
compare: LYL:47ac0cf2a0554c6ffb2033f2b44e54e2e3171999
Branches Tags
LYL:ballgame
LYL:main
LYL:maingame
LYL:mid360_plan2
LYL:6.23
LYL:Cooperation
LYL:chassis

4 Commits
410d62198f ... 47ac0cf2a0

Author SHA1 Message Date
ZHAISHUI04
47ac0cf2a0 改运球，can1适配sick，sick的id为0x301-0x304 2025-05-12 21:07:38 +08:00
ZHAISHUI04
6e780a2920 改了运球，效果一般 2025-05-10 15:25:41 +08:00
ZHAISHUI04
0b0ba31a48 上层和底盘均改 2025-05-07 23:28:50 +08:00
ZHAISHUI04
52668517bd 未改完 2025-05-07 14:04:51 +08:00
17 changed files with 453 additions and 316 deletions
Show Stats Expand all files Collapse all files

10
Core/Inc/main.h
View File

@ -75,11 +75,17 @@ void Error_Handler(void);
#define ACCL_INT_Pin GPIO_PIN_4 #define ACCL_INT_Pin GPIO_PIN_4
#define ACCL_INT_GPIO_Port GPIOC #define ACCL_INT_GPIO_Port GPIOC
#define ACCL_INT_EXTI_IRQn EXTI4_IRQn #define ACCL_INT_EXTI_IRQn EXTI4_IRQn
#define LIGHT_C_Pin GPIO_PIN_13
#define LIGHT_C_GPIO_Port GPIOE
#define GYRO_INT_Pin GPIO_PIN_5 #define GYRO_INT_Pin GPIO_PIN_5
#define GYRO_INT_GPIO_Port GPIOC #define GYRO_INT_GPIO_Port GPIOC
#define GYRO_INT_EXTI_IRQn EXTI9_5_IRQn #define GYRO_INT_EXTI_IRQn EXTI9_5_IRQn
#define FlagForUpper_Pin GPIO_PIN_9 #define LIGHT_A_Pin GPIO_PIN_9
#define FlagForUpper_GPIO_Port GPIOE #define LIGHT_A_GPIO_Port GPIOE
#define LIGHT_B_Pin GPIO_PIN_11
#define LIGHT_B_GPIO_Port GPIOE
#define RELAY_Pin GPIO_PIN_14
#define RELAY_GPIO_Port GPIOE
#define GYRO_CS_Pin GPIO_PIN_0 #define GYRO_CS_Pin GPIO_PIN_0
#define GYRO_CS_GPIO_Port GPIOB #define GYRO_CS_GPIO_Port GPIOB

14
Core/Src/gpio.c
View File

@ -61,7 +61,7 @@ void MX_GPIO_Init(void)
HAL_GPIO_WritePin(ACCL_CS_GPIO_Port, ACCL_CS_Pin, GPIO_PIN_SET); HAL_GPIO_WritePin(ACCL_CS_GPIO_Port, ACCL_CS_Pin, GPIO_PIN_SET);
/*Configure GPIO pin Output Level */ /*Configure GPIO pin Output Level */
HAL_GPIO_WritePin(FlagForUpper_GPIO_Port, FlagForUpper_Pin, GPIO_PIN_SET); HAL_GPIO_WritePin(RELAY_GPIO_Port, RELAY_Pin, GPIO_PIN_RESET);
/*Configure GPIO pin Output Level */ /*Configure GPIO pin Output Level */
HAL_GPIO_WritePin(GYRO_CS_GPIO_Port, GYRO_CS_Pin, GPIO_PIN_SET); HAL_GPIO_WritePin(GYRO_CS_GPIO_Port, GYRO_CS_Pin, GPIO_PIN_SET);
@ -92,12 +92,18 @@ void MX_GPIO_Init(void)
GPIO_InitStruct.Pull = GPIO_PULLUP; GPIO_InitStruct.Pull = GPIO_PULLUP;
HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
/*Configure GPIO pins : PEPin PEPin */
GPIO_InitStruct.Pin = LIGHT_C_Pin|LIGHT_B_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
GPIO_InitStruct.Pull = GPIO_PULLDOWN;
HAL_GPIO_Init(GPIOE, &GPIO_InitStruct);
/*Configure GPIO pin : PtPin */ /*Configure GPIO pin : PtPin */
GPIO_InitStruct.Pin = FlagForUpper_Pin; GPIO_InitStruct.Pin = RELAY_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_PULLUP; GPIO_InitStruct.Pull = GPIO_PULLDOWN;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(FlagForUpper_GPIO_Port, &GPIO_InitStruct); HAL_GPIO_Init(RELAY_GPIO_Port, &GPIO_InitStruct);
/*Configure GPIO pin : PtPin */ /*Configure GPIO pin : PtPin */
GPIO_InitStruct.Pin = GYRO_CS_Pin; GPIO_InitStruct.Pin = GYRO_CS_Pin;

134
MDK-ARM/R2.uvoptx
View File

@ -103,7 +103,7 @@
<bEvRecOn>1</bEvRecOn> <bEvRecOn>1</bEvRecOn>
<bSchkAxf>0</bSchkAxf> <bSchkAxf>0</bSchkAxf>
<bTchkAxf>0</bTchkAxf> <bTchkAxf>0</bTchkAxf>
<nTsel>3</nTsel> <nTsel>6</nTsel>
<sDll></sDll> <sDll></sDll>
<sDllPa></sDllPa> <sDllPa></sDllPa>
<sDlgDll></sDlgDll> <sDlgDll></sDlgDll>
@ -114,9 +114,14 @@
<tDlgDll></tDlgDll> <tDlgDll></tDlgDll>
<tDlgPa></tDlgPa> <tDlgPa></tDlgPa>
<tIfile></tIfile> <tIfile></tIfile>
<pMon>BIN\CMSIS_AGDI.dll</pMon> <pMon>STLink\ST-LINKIII-KEIL_SWO.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>
@ -135,7 +140,7 @@
<SetRegEntry> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
<Key>DLGUARM</Key> <Key>DLGUARM</Key>
<Name></Name> <Name>(105=-1,-1,-1,-1,0)</Name>
</SetRegEntry> </SetRegEntry>
<SetRegEntry> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
@ -205,6 +210,41 @@
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>can_out</ItemText> <ItemText>can_out</ItemText>
</Ww> </Ww>
<Ww>
<count>11</count>
<WinNumber>1</WinNumber>
<ItemText>Nor_Vx</ItemText>
</Ww>
<Ww>
<count>12</count>
<WinNumber>1</WinNumber>
<ItemText>Nor_Vy</ItemText>
</Ww>
<Ww>
<count>13</count>
<WinNumber>1</WinNumber>
<ItemText>a</ItemText>
</Ww>
<Ww>
<count>14</count>
<WinNumber>1</WinNumber>
<ItemText>b</ItemText>
</Ww>
<Ww>
<count>15</count>
<WinNumber>1</WinNumber>
<ItemText>count,0x0A</ItemText>
</Ww>
<Ww>
<count>16</count>
<WinNumber>1</WinNumber>
<ItemText>count</ItemText>
</Ww>
<Ww>
<count>17</count>
<WinNumber>1</WinNumber>
<ItemText>raw_rx1,0x0A</ItemText>
</Ww>
</WatchWindow1> </WatchWindow1>
<Tracepoint> <Tracepoint>
<THDelay>0</THDelay> <THDelay>0</THDelay>
@ -250,7 +290,7 @@
<pMultCmdsp></pMultCmdsp> <pMultCmdsp></pMultCmdsp>
<DebugDescription> <DebugDescription>
<Enable>1</Enable> <Enable>1</Enable>
<EnableFlashSeq>1</EnableFlashSeq> <EnableFlashSeq>0</EnableFlashSeq>
<EnableLog>0</EnableLog> <EnableLog>0</EnableLog>
<Protocol>2</Protocol> <Protocol>2</Protocol>
<DbgClock>5000000</DbgClock> <DbgClock>5000000</DbgClock>
@ -967,18 +1007,6 @@
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2> <bDave2>0</bDave2>
<PathWithFileName>..\User\bsp\map.c</PathWithFileName>
<FilenameWithoutPath>map.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>56</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>..\User\bsp\bsp_can.c</PathWithFileName> <PathWithFileName>..\User\bsp\bsp_can.c</PathWithFileName>
<FilenameWithoutPath>bsp_can.c</FilenameWithoutPath> <FilenameWithoutPath>bsp_can.c</FilenameWithoutPath>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
@ -986,7 +1014,7 @@
</File> </File>
<File> <File>
<GroupNumber>7</GroupNumber> <GroupNumber>7</GroupNumber>
<FileNumber>57</FileNumber> <FileNumber>56</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -998,7 +1026,7 @@
</File> </File>
<File> <File>
<GroupNumber>7</GroupNumber> <GroupNumber>7</GroupNumber>
<FileNumber>58</FileNumber> <FileNumber>57</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1010,7 +1038,7 @@
</File> </File>
<File> <File>
<GroupNumber>7</GroupNumber> <GroupNumber>7</GroupNumber>
<FileNumber>59</FileNumber> <FileNumber>58</FileNumber>
<FileType>5</FileType> <FileType>5</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1022,7 +1050,7 @@
</File> </File>
<File> <File>
<GroupNumber>7</GroupNumber> <GroupNumber>7</GroupNumber>
<FileNumber>60</FileNumber> <FileNumber>59</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1034,7 +1062,7 @@
</File> </File>
<File> <File>
<GroupNumber>7</GroupNumber> <GroupNumber>7</GroupNumber>
<FileNumber>61</FileNumber> <FileNumber>60</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1046,7 +1074,7 @@
</File> </File>
<File> <File>
<GroupNumber>7</GroupNumber> <GroupNumber>7</GroupNumber>
<FileNumber>62</FileNumber> <FileNumber>61</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1066,7 +1094,7 @@
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<File> <File>
<GroupNumber>8</GroupNumber> <GroupNumber>8</GroupNumber>
<FileNumber>63</FileNumber> <FileNumber>62</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1078,7 +1106,7 @@
</File> </File>
<File> <File>
<GroupNumber>8</GroupNumber> <GroupNumber>8</GroupNumber>
<FileNumber>64</FileNumber> <FileNumber>63</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1090,7 +1118,7 @@
</File> </File>
<File> <File>
<GroupNumber>8</GroupNumber> <GroupNumber>8</GroupNumber>
<FileNumber>65</FileNumber> <FileNumber>64</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1102,7 +1130,7 @@
</File> </File>
<File> <File>
<GroupNumber>8</GroupNumber> <GroupNumber>8</GroupNumber>
<FileNumber>66</FileNumber> <FileNumber>65</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1114,7 +1142,7 @@
</File> </File>
<File> <File>
<GroupNumber>8</GroupNumber> <GroupNumber>8</GroupNumber>
<FileNumber>67</FileNumber> <FileNumber>66</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1126,7 +1154,7 @@
</File> </File>
<File> <File>
<GroupNumber>8</GroupNumber> <GroupNumber>8</GroupNumber>
<FileNumber>68</FileNumber> <FileNumber>67</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1138,7 +1166,7 @@
</File> </File>
<File> <File>
<GroupNumber>8</GroupNumber> <GroupNumber>8</GroupNumber>
<FileNumber>69</FileNumber> <FileNumber>68</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1150,7 +1178,7 @@
</File> </File>
<File> <File>
<GroupNumber>8</GroupNumber> <GroupNumber>8</GroupNumber>
<FileNumber>70</FileNumber> <FileNumber>69</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1162,7 +1190,7 @@
</File> </File>
<File> <File>
<GroupNumber>8</GroupNumber> <GroupNumber>8</GroupNumber>
<FileNumber>71</FileNumber> <FileNumber>70</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1174,7 +1202,7 @@
</File> </File>
<File> <File>
<GroupNumber>8</GroupNumber> <GroupNumber>8</GroupNumber>
<FileNumber>72</FileNumber> <FileNumber>71</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1194,7 +1222,7 @@
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<File> <File>
<GroupNumber>9</GroupNumber> <GroupNumber>9</GroupNumber>
<FileNumber>73</FileNumber> <FileNumber>72</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1206,7 +1234,7 @@
</File> </File>
<File> <File>
<GroupNumber>9</GroupNumber> <GroupNumber>9</GroupNumber>
<FileNumber>74</FileNumber> <FileNumber>73</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1218,7 +1246,7 @@
</File> </File>
<File> <File>
<GroupNumber>9</GroupNumber> <GroupNumber>9</GroupNumber>
<FileNumber>75</FileNumber> <FileNumber>74</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1230,7 +1258,7 @@
</File> </File>
<File> <File>
<GroupNumber>9</GroupNumber> <GroupNumber>9</GroupNumber>
<FileNumber>76</FileNumber> <FileNumber>75</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1242,7 +1270,7 @@
</File> </File>
<File> <File>
<GroupNumber>9</GroupNumber> <GroupNumber>9</GroupNumber>
<FileNumber>77</FileNumber> <FileNumber>76</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1254,7 +1282,7 @@
</File> </File>
<File> <File>
<GroupNumber>9</GroupNumber> <GroupNumber>9</GroupNumber>
<FileNumber>78</FileNumber> <FileNumber>77</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1274,7 +1302,7 @@
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<File> <File>
<GroupNumber>10</GroupNumber> <GroupNumber>10</GroupNumber>
<FileNumber>79</FileNumber> <FileNumber>78</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1286,7 +1314,7 @@
</File> </File>
<File> <File>
<GroupNumber>10</GroupNumber> <GroupNumber>10</GroupNumber>
<FileNumber>80</FileNumber> <FileNumber>79</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1298,7 +1326,7 @@
</File> </File>
<File> <File>
<GroupNumber>10</GroupNumber> <GroupNumber>10</GroupNumber>
<FileNumber>81</FileNumber> <FileNumber>80</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1310,7 +1338,7 @@
</File> </File>
<File> <File>
<GroupNumber>10</GroupNumber> <GroupNumber>10</GroupNumber>
<FileNumber>82</FileNumber> <FileNumber>81</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1322,7 +1350,7 @@
</File> </File>
<File> <File>
<GroupNumber>10</GroupNumber> <GroupNumber>10</GroupNumber>
<FileNumber>83</FileNumber> <FileNumber>82</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1334,7 +1362,7 @@
</File> </File>
<File> <File>
<GroupNumber>10</GroupNumber> <GroupNumber>10</GroupNumber>
<FileNumber>84</FileNumber> <FileNumber>83</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1346,7 +1374,7 @@
</File> </File>
<File> <File>
<GroupNumber>10</GroupNumber> <GroupNumber>10</GroupNumber>
<FileNumber>85</FileNumber> <FileNumber>84</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1366,7 +1394,7 @@
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<File> <File>
<GroupNumber>11</GroupNumber> <GroupNumber>11</GroupNumber>
<FileNumber>86</FileNumber> <FileNumber>85</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1378,7 +1406,7 @@
</File> </File>
<File> <File>
<GroupNumber>11</GroupNumber> <GroupNumber>11</GroupNumber>
<FileNumber>87</FileNumber> <FileNumber>86</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1390,7 +1418,7 @@
</File> </File>
<File> <File>
<GroupNumber>11</GroupNumber> <GroupNumber>11</GroupNumber>
<FileNumber>88</FileNumber> <FileNumber>87</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1410,7 +1438,7 @@
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<File> <File>
<GroupNumber>12</GroupNumber> <GroupNumber>12</GroupNumber>
<FileNumber>89</FileNumber> <FileNumber>88</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1430,7 +1458,7 @@
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<File> <File>
<GroupNumber>13</GroupNumber> <GroupNumber>13</GroupNumber>
<FileNumber>90</FileNumber> <FileNumber>89</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1442,7 +1470,7 @@
</File> </File>
<File> <File>
<GroupNumber>13</GroupNumber> <GroupNumber>13</GroupNumber>
<FileNumber>91</FileNumber> <FileNumber>90</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1454,7 +1482,7 @@
</File> </File>
<File> <File>
<GroupNumber>13</GroupNumber> <GroupNumber>13</GroupNumber>
<FileNumber>92</FileNumber> <FileNumber>91</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1466,7 +1494,7 @@
</File> </File>
<File> <File>
<GroupNumber>13</GroupNumber> <GroupNumber>13</GroupNumber>
<FileNumber>93</FileNumber> <FileNumber>92</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

@ -1143,11 +1143,6 @@
<FileType>1</FileType> <FileType>1</FileType>
<FilePath>..\User\bsp\pwm.c</FilePath> <FilePath>..\User\bsp\pwm.c</FilePath>
</File> </File>
<File>
<FileName>map.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\bsp\map.c</FilePath>
</File>
<File> <File>
<FileName>bsp_can.c</FileName> <FileName>bsp_can.c</FileName>
<FileType>1</FileType> <FileType>1</FileType>

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

Binary file not shown.

52
R2.ioc
View File

@ -155,25 +155,28 @@ Mcu.Pin22=PD14
Mcu.Pin23=PA0-WKUP Mcu.Pin23=PA0-WKUP
Mcu.Pin24=PA4 Mcu.Pin24=PA4
Mcu.Pin25=PC4 Mcu.Pin25=PC4
Mcu.Pin26=PC5 Mcu.Pin26=PE13
Mcu.Pin27=PE9 Mcu.Pin27=PC5
Mcu.Pin28=PA7 Mcu.Pin28=PE9
Mcu.Pin29=PB0 Mcu.Pin29=PE11
Mcu.Pin3=PB3 Mcu.Pin3=PB3
Mcu.Pin30=VP_CRC_VS_CRC Mcu.Pin30=PE14
Mcu.Pin31=VP_FREERTOS_VS_CMSIS_V2 Mcu.Pin31=PA7
Mcu.Pin32=VP_SYS_VS_Systick Mcu.Pin32=PB0
Mcu.Pin33=VP_TIM4_VS_ClockSourceINT Mcu.Pin33=VP_CRC_VS_CRC
Mcu.Pin34=VP_TIM7_VS_ClockSourceINT Mcu.Pin34=VP_FREERTOS_VS_CMSIS_V2
Mcu.Pin35=VP_TIM10_VS_ClockSourceINT Mcu.Pin35=VP_SYS_VS_Systick
Mcu.Pin36=VP_USB_DEVICE_VS_USB_DEVICE_CDC_FS Mcu.Pin36=VP_TIM4_VS_ClockSourceINT
Mcu.Pin37=VP_TIM7_VS_ClockSourceINT
Mcu.Pin38=VP_TIM10_VS_ClockSourceINT
Mcu.Pin39=VP_USB_DEVICE_VS_USB_DEVICE_CDC_FS
Mcu.Pin4=PA14 Mcu.Pin4=PA14
Mcu.Pin5=PA13 Mcu.Pin5=PA13
Mcu.Pin6=PB7 Mcu.Pin6=PB7
Mcu.Pin7=PB6 Mcu.Pin7=PB6
Mcu.Pin8=PD0 Mcu.Pin8=PD0
Mcu.Pin9=PC11 Mcu.Pin9=PC11
Mcu.PinsNb=37 Mcu.PinsNb=40
Mcu.ThirdPartyNb=0 Mcu.ThirdPartyNb=0
Mcu.UserConstants= Mcu.UserConstants=
Mcu.UserName=STM32F407IGHx Mcu.UserName=STM32F407IGHx
@ -285,12 +288,28 @@ PD14.GPIOParameters=GPIO_Label
PD14.GPIO_Label=Buzzer PD14.GPIO_Label=Buzzer
PD14.Locked=true PD14.Locked=true
PD14.Signal=S_TIM4_CH3 PD14.Signal=S_TIM4_CH3
PE11.GPIOParameters=GPIO_PuPd,GPIO_Label
PE11.GPIO_Label=LIGHT_B
PE11.GPIO_PuPd=GPIO_PULLDOWN
PE11.Locked=true
PE11.Signal=GPIO_Input
PE13.GPIOParameters=GPIO_PuPd,GPIO_Label
PE13.GPIO_Label=LIGHT_C
PE13.GPIO_PuPd=GPIO_PULLDOWN
PE13.Locked=true
PE13.Signal=GPIO_Input
PE14.GPIOParameters=GPIO_PuPd,GPIO_Label
PE14.GPIO_Label=RELAY
PE14.GPIO_PuPd=GPIO_PULLDOWN
PE14.Locked=true
PE14.Signal=GPIO_Output
PE9.GPIOParameters=PinState,GPIO_PuPd,GPIO_Label PE9.GPIOParameters=PinState,GPIO_PuPd,GPIO_Label
PE9.GPIO_Label=FlagForUpper PE9.GPIO_Label=LIGHT_A
PE9.GPIO_PuPd=GPIO_PULLUP PE9.GPIO_PuPd=GPIO_PULLDOWN
PE9.Locked=true PE9.Locked=true
PE9.PinState=GPIO_PIN_SET PE9.PinState=GPIO_PIN_SET
PE9.Signal=GPIO_Output PE9.Signal=SharedStack_PE9
PE9.Stacked=true
PF6.GPIOParameters=GPIO_Speed,GPIO_Label PF6.GPIOParameters=GPIO_Speed,GPIO_Label
PF6.GPIO_Label=IMU_HEAT_PWM PF6.GPIO_Label=IMU_HEAT_PWM
PF6.GPIO_Speed=GPIO_SPEED_FREQ_HIGH PF6.GPIO_Speed=GPIO_SPEED_FREQ_HIGH
@ -401,6 +420,9 @@ SH.S_TIM10_CH1.0=TIM10_CH1,PWM Generation1 CH1
SH.S_TIM10_CH1.ConfNb=1 SH.S_TIM10_CH1.ConfNb=1
SH.S_TIM4_CH3.0=TIM4_CH3,PWM Generation3 CH3 SH.S_TIM4_CH3.0=TIM4_CH3,PWM Generation3 CH3
SH.S_TIM4_CH3.ConfNb=1 SH.S_TIM4_CH3.ConfNb=1
SH.SharedStack_PE9.0=GPIO_Output+0
SH.SharedStack_PE9.1=GPIO_Input
SH.SharedStack_PE9.ConfNb=2
SPI1.BaudRatePrescaler=SPI_BAUDRATEPRESCALER_16 SPI1.BaudRatePrescaler=SPI_BAUDRATEPRESCALER_16
SPI1.CLKPhase=SPI_PHASE_2EDGE SPI1.CLKPhase=SPI_PHASE_2EDGE
SPI1.CLKPolarity=SPI_POLARITY_HIGH SPI1.CLKPolarity=SPI_POLARITY_HIGH

71
User/Algorithm/user_math.c
View File

@ -321,3 +321,74 @@ int abs_value(int num) {
return -num; // 如果数是负的，返回它的相反数 return -num; // 如果数是负的，返回它的相反数
} }
} }
/**
* @brief 判断是否到达目标值
* @param current 当前值
* @param target 目标值
* @param mistake 阈值
* @return 如果到达目标值返回 true，否则返回 false
*/
/* * @brief 读取标志位状态
* @param flag 当前标志位的值（0 或 1）
* @return 返回状态值：0、1、2...
*/
int read_flag_state(uint8_t flag) {
static int last_flag = 1; // 保存上一次的标志位状态
static int state = 1; // 当前状态值
if (flag == 0 && last_flag == 1) {
// 标志位从 0 变为 1，状态值递增
state++;
}
// 更新上一次的标志位状态
last_flag = flag;
return state;
}
// 归一化函数，将正方形坐标映射到单位圆
void normalize_vector(double x, double y, double *out_x, double *out_y) {
// 处理原点情况
if (x == 0.0 && y == 0.0) {
*out_x = 0.0;
*out_y = 0.0;
return;
}
// 计算最大坐标绝对值和原始模长
const double abs_x = fabs(x);
const double abs_y = fabs(y);
const double s = fmax(abs_x, abs_y); // 最大坐标绝对值
const double r = sqrt(x*x + y*y); // 原始向量模长
const double scale = s / r; // 缩放因子
// 应用缩放并保持方向
*out_x = x * scale;
*out_y = y * scale;
}
bool is_reached(float current, float target, float mistake) {
return fabs(current - target) < mistake;
}
bool is_reached_multiple(float current1, float current2, float current3, float target1, float target2, float target3, float mistake, int threshold) {
static int count = 0; // 满足条件的计数
if (count >= threshold) {
count=0;//重置
return true; // 如果已经满足条件，返回 1
}
// 判断三个值是否都满足条件
bool all_reached = (fabs(current1 - target1) < mistake) &&
(fabs(current2 - target2) < mistake) &&
(fabs(current3 - target3) < mistake);
if (all_reached) {
count++; // 所有条件都满足，计数加 1
}
return false; // 未满足条件达到阈值，返回 0
}

7
User/Algorithm/user_math.h
View File

@ -14,7 +14,6 @@
#include <math.h> #include <math.h>
#include <stdbool.h> #include <stdbool.h>
#include "struct_typedef.h" #include "struct_typedef.h"
#define M_DEG2RAD_MULT (0.01745329251f) #define M_DEG2RAD_MULT (0.01745329251f)
#define M_RAD2DEG_MULT (57.2957795131f) #define M_RAD2DEG_MULT (57.2957795131f)
@ -160,4 +159,10 @@ uint8_t average(uint8_t arr[], uint8_t n);
int abs_value(int num); int abs_value(int num);
float expo_map(float value, float expo_factor) ; float expo_map(float value, float expo_factor) ;
int read_flag_state(uint8_t flag) ;
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_multiple(float current1, float current2, float current3, float target1, float target2, float target3, float mistake, int threshold) ;
#endif #endif

35
User/Module/Chassis.c
View File

@ -22,7 +22,6 @@
static int8_t Chassis_SetCtrl(Chassis_t *c,CMD_t *ctrl){ static int8_t Chassis_SetCtrl(Chassis_t *c,CMD_t *ctrl){
if (c == NULL) return CHASSIS_ERR_NULL; /*主结构体不能为空 */ if (c == NULL) return CHASSIS_ERR_NULL; /*主结构体不能为空 */
if (ctrl->CMD_CtrlType== c->chassis_ctrl.ctrl && ctrl->CMD_mode == c->chassis_ctrl.mode) return CHASSIS_OK; /*模式未改变直接返回*/ if (ctrl->CMD_CtrlType== c->chassis_ctrl.ctrl && ctrl->CMD_mode == c->chassis_ctrl.mode) return CHASSIS_OK; /*模式未改变直接返回*/
//此处源代码处做了pid的reset 待添加
c->chassis_ctrl.ctrl =ctrl->CMD_CtrlType ; c->chassis_ctrl.ctrl =ctrl->CMD_CtrlType ;
c->chassis_ctrl.mode =ctrl->CMD_mode ; c->chassis_ctrl.mode =ctrl->CMD_mode ;
@ -83,22 +82,23 @@ int8_t Chassis_init(Chassis_t *c,const Chassis_Param_t *param,float target_freq)
} }
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) //底盘逆运动学的解算,
{ {
c->hopemotorout.OmniSpeedOut[0] = -Vx+Vy+Vw;//右前 fp64 Nor_Vx,Nor_Vy;//归一化后的数据
c->hopemotorout.OmniSpeedOut[1] = -Vx-Vy+Vw;//右后 normalize_vector(Vx,Vy,&Nor_Vx,&Nor_Vy);
c->hopemotorout.OmniSpeedOut[2] = Vx-Vy+Vw;//左后
c->hopemotorout.OmniSpeedOut[3] = Vx+Vy+Vw;//左前
// c->hopemotorout.OmniSpeedOut[0] = -Nor_Vx+Nor_Vy+Vw + c->pos088 .bmi088.gyro.z;//右前
// c->hopemotorout.OmniSpeedOut[1] = -Nor_Vx-Nor_Vy+Vw + c->pos088 .bmi088.gyro.z;//右后
// c->hopemotorout.OmniSpeedOut[2] = Nor_Vx-Nor_Vy+Vw + c->pos088 .bmi088.gyro.z;//左后
// c->hopemotorout.OmniSpeedOut[3] = Nor_Vx+Nor_Vy+Vw + c->pos088 .bmi088.gyro.z;//左前
c->hopemotorout.OmniSpeedOut[0] = -Nor_Vx+Nor_Vy+Vw ;//右前
c->hopemotorout.OmniSpeedOut[1] = -Nor_Vx-Nor_Vy+Vw ;//右后
c->hopemotorout.OmniSpeedOut[2] = Nor_Vx-Nor_Vy+Vw ;//左后
c->hopemotorout.OmniSpeedOut[3] = Nor_Vx+Nor_Vy+Vw ;//左前
} }
//bool isArrive(fp32 real_pos,fp32 target_pos,fp32 mistake)
//{
// uint16_t xArrive = 0, yArrive = 0;
// xArrive = abs_float_double(real_pos,target_pos) < mistake ? 1:0;
// yArrive = abs_float_double(real_pos,target_pos) < mistake ? 1:0;
// if(xArrive && yArrive) return true;
// else return false;
//}
int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out) int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out)
{ {
@ -215,11 +215,12 @@ int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out)
//电机速度限幅 //电机速度限幅
abs_limit_fp(&c->move_vec.Vx,8000.0f); abs_limit_fp(&c->move_vec.Vx,6000.0f);
abs_limit_fp(&c->move_vec.Vy,8000.0f); abs_limit_fp(&c->move_vec.Vy,6000.0f);
abs_limit_fp(&c->move_vec.Vw,6000.0f);
abs_limit_fp(&c->move_vec.Vw,8000.0f);
Chassis_speed_calculate(c,c->move_vec.Vx,c->move_vec.Vy,c->move_vec.Vw); Chassis_speed_calculate(c,c->move_vec.Vx,c->move_vec.Vy,c->move_vec.Vw);

31
User/Module/config.c
View File

@ -37,10 +37,10 @@ static const ConfigParam_t param ={
.out_limit = 3000.0f, .out_limit = 3000.0f,
}, },
.Pitch_M2006_angle_param = { .Pitch_M2006_angle_param = {
.p = 25.0f, .p = 600.0f,
.i = 0.0f, .i = 0.0f,
.d = 1.5f, .d = 3.0f,
.i_limit = 1000.0f, .i_limit = 2000.0f,
.out_limit = 3000.0f, .out_limit = 3000.0f,
}, },
.Pitch_M2006_speed_param={ .Pitch_M2006_speed_param={
@ -59,11 +59,11 @@ static const ConfigParam_t param ={
}, },
.Dribble_M3508_speed_param={ //三摩擦速度环 .Dribble_M3508_speed_param={ //三摩擦速度环
.p = 5.0f, .p = 30.0f,
.i = 0.3f, .i = 0.45f,
.d = 0.0f, .d = 0.0f,
.i_limit = 2000.0f, .i_limit = 3000.0f,
.out_limit = 3000.0f, .out_limit = 5000.0f,
}, },
.Dribble_translate_M3508_speed_param={//平移用3508速度环 .Dribble_translate_M3508_speed_param={//平移用3508速度环
.p = 5.0f, .p = 5.0f,
@ -96,12 +96,17 @@ static const ConfigParam_t param ={
/*上层其他参数*/ /*上层其他参数*/
/*运球*/ /*运球*/
.DribbleConfig_Config = { .DribbleConfig_Config = {
.dribble_flag=0,
.m3508_init_angle = 50, .m3508_init_angle = 50,
.m3508_high_angle = 1200, .m3508_translate_angle = -930,
.go2_init_angle = 0, .m3508_dribble_Reverse_speed=-3500,
.go2_flip_angle = -250, . m3508_dribble_speed= 4000, // 转动速度
.flip_timing = 200, .m3508_dribble_init_speed=0,
.go2_release_threshold = -550.0f,
.light_3508_flag=0,//3508平移处的光电，0初始，1到位置
.light_ball_flag=0,//检测球的flag
}, },
/*投球*/ /*投球*/
.PitchConfig_Config = { .PitchConfig_Config = {
@ -110,7 +115,7 @@ static const ConfigParam_t param ={
.go1_init_position = -50, .go1_init_position = -50,
.go1_release_threshold =-210, .go1_release_threshold =-210,
.m2006_Screw_init=0, .m2006_Screw_init=0,
.Pitch_angle =57, .Pitch_angle =56,
}, },

277
User/Module/up.c
View File

@ -3,6 +3,15 @@
#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
*/
#define GEAR_RATIO_2006 (36) // 2006减速比 #define GEAR_RATIO_2006 (36) // 2006减速比
#define GEAR_RATIO_3508 (19) #define GEAR_RATIO_3508 (19)
@ -15,8 +24,7 @@
// 定义继电器控制 // 定义继电器控制
#define RELAY1_TOGGLE(state) HAL_GPIO_WritePin(GPIOE, GPIO_PIN_9, (state) ? GPIO_PIN_SET : GPIO_PIN_RESET) #define RELAY1_TOGGLE(state) HAL_GPIO_WritePin(GPIOE, RELAY_Pin, (state) ? GPIO_PIN_SET : GPIO_PIN_RESET)
#define RELAY2_TOGGLE(state) HAL_GPIO_WritePin(GPIOE, GPIO_PIN_11, (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)
{ {
@ -36,16 +44,16 @@ int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
PID_init (&u->pid .Dribble_translate_M3508_angle ,PID_POSITION ,&(u->param->Dribble_translate_M3508_angle_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 )); 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 ,PID_POSITION ,&(u->param->Dribble_M3508_speed_param )); 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 ;
// 初始化上层状态机 // 初始化上层状态机
if (!u->DribbleContext .is_initialized) { //检查是否为第一次运行状态机，运球 if (!u->DribbleContext .is_initialized) { //检查是否为第一次运行状态机，运球
u->DribbleContext .DribbleConfig = u->param ->DribbleConfig_Config ;//赋值 u->DribbleContext .DribbleConfig = u->param ->DribbleConfig_Config ;//赋值
u->DribbleContext .DribbleState = Dribble_PREPARE; u->DribbleContext .DribbleState = DRIBBLE_PREPARE;
u->DribbleContext .is_initialized = 1; u->DribbleContext .is_initialized = 1;
} }
@ -197,9 +205,9 @@ int8_t GO_SendData(UP_t *u, float pos, float limit)
int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out) int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
{ {
//电机控制 ，传进can //电机控制 ，传进can
DJ_Speed_Control(u,0x201,&u->motorfeedback .DJmotor_feedback[0] ,&u->pid .Dribble_M3508_speed,u->motor_target .Dribble_M3508_speed); 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,u->motor_target .Dribble_M3508_speed); 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,u->motor_target .Dribble_M3508_speed); 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] , DJ_Angle_Control(u,0x204,&u->motorfeedback .DJmotor_feedback[3] ,
&u->pid .Dribble_translate_M3508_angle , &u->pid .Dribble_translate_M3508_angle ,
&u->pid .Dribble_translate_M3508_speed , &u->pid .Dribble_translate_M3508_speed ,
@ -212,8 +220,10 @@ int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
// &u->pid .Pitch_M2006_angle , // &u->pid .Pitch_M2006_angle ,
// &u->pid .Pitch_M2006_speed , // &u->pid .Pitch_M2006_speed ,
// u->motor_target .Pitch_M2006_angle ); // u->motor_target .Pitch_M2006_angle );
u->final_out .DJfinal_out [5]=PID_calc (&(u->pid .Pitch_Angle_M2006_speed ), DJ_Speed_Control(u,0x206,&u->motorfeedback .DJmotor_feedback[5],&u->pid .Pitch_M2006_speed,
u->motorfeedback .Encoder.angle,u->motor_target .Shoot_Pitch_angle); (PID_calc (&(u->pid .Pitch_M2006_angle ),
u->motorfeedback .Encoder.angle,u->motor_target .Shoot_Pitch_angle))
);
GO_SendData(u,u->motor_target .go_shoot,5 ); GO_SendData(u,u->motor_target .go_shoot,5 );
@ -233,7 +243,8 @@ int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
} }
int a=0;
int b=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)
@ -241,6 +252,9 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
if(u ==NULL) return 0; if(u ==NULL) return 0;
static int is_pitch=1; static int is_pitch=1;
switch (c->CMD_CtrlType ) switch (c->CMD_CtrlType )
{ {
case RCcontrol: //在手动模式下 case RCcontrol: //在手动模式下
@ -252,22 +266,21 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
/*投篮*/ /*投篮*/
if(is_pitch){ if(is_pitch){
u->motor_target .go_shoot =u->PitchContext .PitchConfig .go1_init_position ; u->motor_target .go_shoot =u->PitchContext .PitchConfig .go1_init_position ;
is_pitch=0;
}
u->motor_target .Shoot_M2006_angle =u->PitchContext .PitchConfig .m2006_init_angle ;
u->motor_target .Shoot_Pitch_angle=u->PitchContext.PitchConfig.Pitch_angle; u->motor_target .Shoot_Pitch_angle=u->PitchContext.PitchConfig.Pitch_angle;
is_pitch=0;
} //让初始go位置只读一次，后面按调整模式更改的来,后面稳定了之后，可以跟随调整模式下一块删
u->motor_target .Shoot_M2006_angle =u->PitchContext .PitchConfig .m2006_init_angle ;
u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig.m3508_translate_angle;//平行移动
u->PitchContext .PitchState = PITCH_PREPARE; //状态更新,开始夹球 u->PitchContext .PitchState = PITCH_PREPARE; //状态更新,开始夹球
// /*运球*/ /*运球*/
// u->motor_target .go_spin = u->DribbleContext.DribbleConfig .go2_init_angle ;
// u->motor_target .M3508_angle =u->DribbleContext .DribbleConfig .m3508_init_angle ;
// u->DribbleContext .DribbleState = Dribble_PREPARE; //重置最初状态
RELAY1_TOGGLE(0);//关闭气缸
// RELAY1_TOGGLE (1);//夹球，0夹1开 for(int i=0;i<3;i++){
// RELAY2_TOGGLE (0);//球，0接1收 u->motor_target.Dribble_M3508_speed[i]=0;
// }
u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig .m3508_init_angle;
u->DribbleContext .DribbleState = DRIBBLE_PREPARE; //重置最初状态
break; break;
@ -280,31 +293,28 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
Pitch_Process(u,out,c); Pitch_Process(u,out,c);
break ; break ;
case UP_Adjust: case UP_Adjust: //测试用
u->PitchContext.PitchConfig.go1_init_position += c->Vx ; u->PitchContext.PitchConfig.go1_init_position += c->Vx ;
// u->PitchContext.PitchConfig.m2006_Screw_init = c->Vy;
u->PitchContext.PitchConfig.Pitch_angle += c->Vy/100; u->PitchContext.PitchConfig.Pitch_angle += c->Vy/100;
u->motor_target .go_shoot=u->PitchContext.PitchConfig.go1_init_position; u->motor_target .go_shoot=u->PitchContext.PitchConfig.go1_init_position;
// u->motor_target .Pitch_M2006_angle=u->PitchContext.PitchConfig.m2006_Screw_init; u->motor_target .Shoot_Pitch_angle=u->PitchContext.PitchConfig.Pitch_angle;
break ; break ;
case Dribble: case Dribble:
{ {
/*夹球 -> 3508 升起 ，同时go2翻转 -> 到位置后，继电器开，放球，同时3508降，go2翻回->接球，收 */
if(u->DribbleContext.DribbleState== Dribble_PREPARE){ if(u->DribbleContext.DribbleState== DRIBBLE_PREPARE){
u->DribbleContext .DribbleState=DRIBBLE_TRANSLATE;
u->DribbleContext .DribbleState =STATE_GRAB_BALL;
} }
// Dribble_Process(u,out); //光电状态更新
u->DribbleContext .DribbleConfig .light_ball_flag =HAL_GPIO_ReadPin(LIGHT_C_GPIO_Port ,LIGHT_C_Pin);
u->DribbleContext .DribbleConfig .light_3508_flag =HAL_GPIO_ReadPin(LIGHT_B_GPIO_Port ,LIGHT_B_Pin);
a=HAL_GPIO_ReadPin(LIGHT_C_GPIO_Port ,LIGHT_C_Pin);
b=HAL_GPIO_ReadPin(LIGHT_B_GPIO_Port ,LIGHT_B_Pin);
Dribble_Process(u,out);
}break ; }break ;
case Dribbl_transfer:
break ;
// Dribbl_transfer_a(u,out);
}
break;
default: default:
@ -319,7 +329,7 @@ return 0;
} }
}
@ -334,10 +344,6 @@ return 0;
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,CMD_t *c)
{ {
// go1_position=u->PitchContext .PitchConfig .go1_init_position ;
// M2006_Screw_position=u->PitchContext .PitchConfig .m2006_Screw_init;
switch(u->PitchContext .PitchState){ switch(u->PitchContext .PitchState){
case PITCH_START: case PITCH_START:
@ -353,19 +359,15 @@ int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c)
case PITCH_PULL_TRIGGER: case PITCH_PULL_TRIGGER:
if(u->motorfeedback .DJmotor_feedback [0].total_angle >-1) //当2006的总角度小于1，可以认为已经勾上,误差为1 if( u->motorfeedback .DJmotor_feedback [4].total_angle>-1) //当2006的总角度小于1，可以认为已经勾上,误差为1
{ {
u->motor_target .go_shoot=u->PitchContext.PitchConfig.go1_init_position; u->motor_target .go_shoot=u->PitchContext.PitchConfig.go1_init_position;
// u->PitchContext.PitchConfig.go1_init_position += c->Vx ;
// u->PitchContext.PitchConfig.m2006_Screw_init += c->Vy;
} }
break ; break ;
} }
return 0; return 0;
@ -376,124 +378,81 @@ return 0;
int8_t Pitch_Adjust(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;//是否共速
u->PitchContext.PitchConfig.go1_init_position += c->Vx ; switch (u->DribbleContext.DribbleState) {
u->PitchContext.PitchConfig.Pitch_angle += c->Vy;
u->motor_target .Shoot_Pitch_angle=u->PitchContext.PitchConfig.Pitch_angle;
case DRIBBLE_TRANSLATE:
if(is_reached(u->motorfeedback.DJmotor_feedback[3].total_angle ,u->DribbleContext .DribbleConfig.m3508_translate_angle,1.0f))
{
u->DribbleContext .DribbleState=DRIBBLE_PROCESS_DOWN;//到达位置后，转移状态
}
break;
case DRIBBLE_PROCESS_DOWN:
u->motor_target.Dribble_M3508_speed[0]=u->DribbleContext .DribbleConfig.m3508_dribble_speed;
u->motor_target.Dribble_M3508_speed[1]=-u->DribbleContext .DribbleConfig.m3508_dribble_speed;
u->motor_target.Dribble_M3508_speed[2]=-u->DribbleContext .DribbleConfig.m3508_dribble_speed;
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,50)
) {
RELAY1_TOGGLE(1); //速度达到后打开气缸
common_speed_flag =1;
}
if(common_speed_flag){
if(u->DribbleContext .DribbleConfig .light_ball_flag == 1){//球下落检测，反转
u->motor_target.Dribble_M3508_speed[0]=u->DribbleContext .DribbleConfig.m3508_dribble_Reverse_speed;
u->motor_target.Dribble_M3508_speed[1]=-u->DribbleContext .DribbleConfig.m3508_dribble_Reverse_speed;
u->motor_target.Dribble_M3508_speed[2]=-u->DribbleContext .DribbleConfig.m3508_dribble_Reverse_speed;
u->DribbleContext .DribbleState=DRIBBLE_PROCESS_UP;
}
} }
break;
case DRIBBLE_PROCESS_UP:
if((u->motorfeedback .DJmotor_feedback [0].rpm<0)&&
(u->motorfeedback .DJmotor_feedback [1].rpm>0)&&
(u->motorfeedback .DJmotor_feedback [2].rpm>0)
){
if(u->DribbleContext .DribbleConfig .light_ball_flag == 0){
u->DribbleContext .DribbleState=DRIBBLE_DONE;
RELAY1_TOGGLE(0); //关闭气缸
}
}
break ;
case DRIBBLE_DONE:
for(int i=0;i<3;i++){
u->motor_target.Dribble_M3508_speed[i]=u->DribbleContext .DribbleConfig.m3508_dribble_init_speed ;//三摩擦速度归0
}
/*标志位清零*/
u->DribbleContext.DribbleConfig.light_ball_flag=0;
u->DribbleContext.DribbleConfig.light_3508_flag=0;
break;
default:
// 处理未知状态
break;
}
return 0;
}
//int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
//{
//
// switch (u->DribbleContext.DribbleState) {
// case STATE_GRAB_BALL://开始
//
// RELAY1_TOGGLE (0);//夹球
//
// u->motor_target.M3508_angle =u->DribbleContext .DribbleConfig .m3508_high_angle;//3508升起
// u->motor_target.go_spin =u->DribbleContext .DribbleConfig .go2_flip_angle;
//
// if((u->motorfeedback .M3508 .total_angle >400)) {
// RELAY2_TOGGLE (1);
//
//// if(((u->motorfeedback.GO_motor_info[1]->Pos ) <-1.1)&&(u->motorfeedback.M3508 .total_angle >1190)){
////
//// u->DribbleContext .DribbleState = STATE_RELEASE_BALL; //当go2到标准位置，标志位改变
//// }
//
// }
// break;
// case STATE_RELEASE_BALL:
// RELAY1_TOGGLE (1);//松球
//
//
//
//// if((u->motorfeedback.GO_motor_info[1]->Pos ) <-1.3){
//// u->motor_target.go_spin =-40; //恢复go2位置
//// u->DribbleContext .DribbleState = STATE_CATCH_PREP; //当go2到标准位置，标志位改变
//// }
// break;
// case STATE_CATCH_PREP:
//// if((u->motorfeedback.GO_motor_info[1]->Pos )> -0.4){
//// u->motor_target.M3508_angle =0 ; //当go2到初始位置，3508降
////
//// RELAY2_TOGGLE (0);//接球
//// }
// if(u->motorfeedback .M3508 .total_angle <51.0f){
// RELAY1_TOGGLE (0);//夹球，0夹1开
// u->DribbleContext .DribbleState = STATE_TRANSFER;
// }
//
// break;
//
//
//
// break ;
// default:
// break;
// }
// return 0;
//}
//void Dribbl_transfer_a(UP_t *u, CAN_Output_t *out)
//{
// switch (u->DribbleContext.DribbleState) {
//
//
//
// case STATE_TRANSFER:
// if((u->motorfeedback.M3508 .total_angle <52.0f )) //满足这个状态时认为go和3508到达初始位置，再夹上球
// {
//
// u->motor_target .go_spin =u->DribbleContext .DribbleConfig .go2_release_threshold ;
// }
//
//// if(u->motorfeedback .GO_motor_info [1]->Pos < -4.9)
//// {
//// RELAY1_TOGGLE (1);//夹球，0夹1开
////
//// if(u->motorfeedback .GO_motor_info [1]->Pos > -4.8)
//// {
//// u->DribbleContext .DribbleState = STATE_CATCH_DONE; //当go2到标准位置，标志位改变
//// }
//// }
//
//
// break ;
// case STATE_CATCH_DONE:
//
// RELAY1_TOGGLE (1);//夹球，0夹1开
// RELAY2_TOGGLE (0);//夹球，0接1收
// u->motor_target.go_spin=u->DribbleContext .DribbleConfig.go2_init_angle ;
// break;
// break;
// }
//}

40
User/Module/up.h
View File

@ -76,23 +76,29 @@ typedef struct {
/*运球*/ /*运球*/
typedef enum { typedef enum {
Dribble_PREPARE, DRIBBLE_PREPARE, // 准备阶段
STATE_GRAB_BALL, // 夹球升起阶段 DRIBBLE_START,
STATE_RELEASE_BALL, // 释放球体 DRIBBLE_TRANSLATE, // 平移过程
STATE_CATCH_PREP, // 接球准备 DRIBBLE_PROCESS_DOWN, // 运球过程，球下落
STATE_CATCH_BALL, // 接球动作 DRIBBLE_PROCESS_UP, // 运球过程，球上升
STATE_TRANSFER, //转移球 DRIBBLE_DONE // 运球结束
STATE_CATCH_DONE //完成
} DribbleState_t; } DribbleState_t;
/* 参数配置结构体 */ /* 参数配置结构体 */
typedef struct { typedef struct {
fp32 m3508_init_angle; // 3508初始角度 int8_t dribble_flag;//当移动三摩擦后为1，否则为0，防止发射撞到
fp32 m3508_high_angle; // 3508升起角度
fp32 go2_init_angle; // GO2初始角度 fp32 m3508_init_angle; // 平移前位置
fp32 go2_flip_angle; // GO2翻转角度 fp32 m3508_translate_angle; // 平移后位置
fp32 flip_timing; // 翻转触发时机 fp32 m3508_dribble_init_speed;
fp32 go2_release_threshold; // 释放球 fp32 m3508_dribble_speed; // 转动速度
fp32 m3508_dribble_Reverse_speed;
/*光电标志位，初始值均为0，触发为1*/
int light_3508_flag;//3508平移处的光电，0初始，1到位置
int light_ball_flag;//检测球的flag
} DribbleConfig_t; } DribbleConfig_t;
/* 状态机上下文 */ /* 状态机上下文 */
@ -102,6 +108,8 @@ typedef struct {
uint8_t is_initialized; uint8_t is_initialized;
} DribbleContext_t; } DribbleContext_t;
@ -191,10 +199,11 @@ typedef struct{
fp32 Pitch_M2006_angle; fp32 Pitch_M2006_angle;
fp32 go_shoot; fp32 go_shoot;
fp32 Dribble_M3508_speed;//运球转速 fp32 Dribble_M3508_speed[3];//运球转速
fp32 Dribble_translate_M3508_angle;//平移用的3508转动角度 fp32 Dribble_translate_M3508_angle;//平移用的3508转动角度
fp32 Shoot_Pitch_angle; fp32 Shoot_Pitch_angle;
}motor_target; }motor_target;
struct{ struct{
@ -211,7 +220,7 @@ typedef struct{
pid_type_def Pitch_Angle_M2006_speed; pid_type_def Pitch_Angle_M2006_speed;
pid_type_def Dribble_M3508_speed;//三摩擦用的速度环 pid_type_def Dribble_M3508_speed[3];//三摩擦用的速度环
pid_type_def Dribble_translate_M3508_speed;//平移用的速度环 pid_type_def Dribble_translate_M3508_speed;//平移用的速度环
pid_type_def Dribble_translate_M3508_angle;//平移用的角度环 pid_type_def Dribble_translate_M3508_angle;//平移用的角度环
@ -252,6 +261,7 @@ 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,CMD_t *c);

46
User/device/can_use.c
View File

@ -56,14 +56,31 @@ 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,uint16_t id,
const uint8_t *raw) { const uint8_t *raw) {
if (feedback == NULL || raw == NULL) return; if (feedback == NULL || raw == NULL) return;
feedback->raw_dis[0] = (uint16_t)((raw[0] << 8) | raw[1]); switch (id)
feedback->raw_dis[1] = (uint16_t)((raw[2] << 8) | raw[3]); {
feedback->raw_dis[2] = (uint16_t)((raw[4] << 8) | raw[5]); case CAN_SICK_ID_1:
feedback->raw_dis[3]= (uint16_t)((raw[6] << 8) | raw[7]); feedback->raw_dis[0] = (uint16_t)(raw[0] << 8| raw[1]);
break ;
case CAN_SICK_ID_2:
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 ;
}
} }
void CAN_Encoder_Decode(CAN_EncoderFeedback_t *feedback, void CAN_Encoder_Decode(CAN_EncoderFeedback_t *feedback,
const uint8_t *raw) { const uint8_t *raw) {
@ -76,7 +93,7 @@ void CAN_Encoder_Decode(CAN_EncoderFeedback_t *feedback,
{ {
case 0x01: case 0x01:
feedback->ecd =raw[3]|raw[4]<<8|raw[5]<<16|raw[6]<<24; feedback->ecd =raw[3]|raw[4]<<8|raw[5]<<16|raw[6]<<24;
feedback->angle=feedback->ecd*360/CAN_ENCODER_RESOLUTION-151.0f; feedback->angle=(fp32)feedback->ecd*360/CAN_ENCODER_RESOLUTION-151.0f;
break; break;
} }
} }
@ -366,7 +383,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;
} }
@ -396,10 +425,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:
// 存储消息到sickfed结构体中
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;

10
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=0x301, 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,
@ -121,7 +124,7 @@ typedef struct {
/* sick反馈信息 */ /* sick反馈信息 */
typedef struct { typedef struct {
uint16_t raw_dis[4]; uint16_t raw_dis[4];
int changed_dis[4];
}CAN_SickFeedback_t; }CAN_SickFeedback_t;
/* encoder编码器反馈信息 */ /* encoder编码器反馈信息 */
typedef struct { typedef struct {
@ -197,5 +200,6 @@ 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,uint16_t id,
const uint8_t *raw) ;
#endif #endif

3
User/device/cmd.c
View File

@ -196,8 +196,9 @@ 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 =UP_Adjust; if(rc->LD.key_G ==CMD_SW_DOWN ) cmd ->CMD_mode=Dribble ;
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 cmd ->CMD_mode =Normal; else cmd ->CMD_mode =Normal;
} }
} }

2
User/task/chassis_task.c
View File

@ -52,8 +52,6 @@ void Task_Chassis(void *argument)
uint32_t tick = osKernelGetTickCount(); uint32_t tick = osKernelGetTickCount();
HAL_GPIO_WritePin(FlagForUpper_GPIO_Port,FlagForUpper_Pin,GPIO_PIN_RESET); //拉低电平 避免未清除
while(1) while(1)
{ {
#ifdef DEBUG #ifdef DEBUG

6
User/task/error_detect_task.c
View File

@ -96,10 +96,10 @@ void Task_error_detect(void *argument){
//若所有设备在线，会不断翻转绿灯 存在设备离线则会红灯灯翻转 //若所有设备在线，会不断翻转绿灯 存在设备离线则会红灯灯翻转
if(online_dev == ERROR_LIST_LENGHT) // if(online_dev == ERROR_LIST_LENGHT)
{ // {
HAL_GPIO_TogglePin(GPIOH,LED_G_Pin); HAL_GPIO_TogglePin(GPIOH,LED_G_Pin);
} // }
// else HAL_GPIO_TogglePin(GPIOH,LED_R_Pin); // else HAL_GPIO_TogglePin(GPIOH,LED_R_Pin);
for (int i = 0; i < ERROR_LIST_LENGHT; i++) for (int i = 0; i < ERROR_LIST_LENGHT; i++)