Compare commits
4 Commits
410d62198f
...
47ac0cf2a0
| Author | SHA1 | Date | |
|---|---|---|---|
| 47ac0cf2a0 | |||
| 6e780a2920 | |||
| 0b0ba31a48 | |||
| 52668517bd |
@ -75,11 +75,17 @@ void Error_Handler(void);
|
||||
#define ACCL_INT_Pin GPIO_PIN_4
|
||||
#define ACCL_INT_GPIO_Port GPIOC
|
||||
#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_GPIO_Port GPIOC
|
||||
#define GYRO_INT_EXTI_IRQn EXTI9_5_IRQn
|
||||
#define FlagForUpper_Pin GPIO_PIN_9
|
||||
#define FlagForUpper_GPIO_Port GPIOE
|
||||
#define LIGHT_A_Pin GPIO_PIN_9
|
||||
#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_GPIO_Port GPIOB
|
||||
|
||||
|
||||
@ -61,7 +61,7 @@ void MX_GPIO_Init(void)
|
||||
HAL_GPIO_WritePin(ACCL_CS_GPIO_Port, ACCL_CS_Pin, GPIO_PIN_SET);
|
||||
|
||||
/*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 */
|
||||
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;
|
||||
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 */
|
||||
GPIO_InitStruct.Pin = FlagForUpper_Pin;
|
||||
GPIO_InitStruct.Pin = RELAY_Pin;
|
||||
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
|
||||
GPIO_InitStruct.Pull = GPIO_PULLUP;
|
||||
GPIO_InitStruct.Pull = GPIO_PULLDOWN;
|
||||
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 */
|
||||
GPIO_InitStruct.Pin = GYRO_CS_Pin;
|
||||
|
||||
@ -103,7 +103,7 @@
|
||||
<bEvRecOn>1</bEvRecOn>
|
||||
<bSchkAxf>0</bSchkAxf>
|
||||
<bTchkAxf>0</bTchkAxf>
|
||||
<nTsel>3</nTsel>
|
||||
<nTsel>6</nTsel>
|
||||
<sDll></sDll>
|
||||
<sDllPa></sDllPa>
|
||||
<sDlgDll></sDlgDll>
|
||||
@ -114,9 +114,14 @@
|
||||
<tDlgDll></tDlgDll>
|
||||
<tDlgPa></tDlgPa>
|
||||
<tIfile></tIfile>
|
||||
<pMon>BIN\CMSIS_AGDI.dll</pMon>
|
||||
<pMon>STLink\ST-LINKIII-KEIL_SWO.dll</pMon>
|
||||
</DebugOpt>
|
||||
<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>
|
||||
<Number>0</Number>
|
||||
<Key>ARMRTXEVENTFLAGS</Key>
|
||||
@ -135,7 +140,7 @@
|
||||
<SetRegEntry>
|
||||
<Number>0</Number>
|
||||
<Key>DLGUARM</Key>
|
||||
<Name></Name>
|
||||
<Name>(105=-1,-1,-1,-1,0)</Name>
|
||||
</SetRegEntry>
|
||||
<SetRegEntry>
|
||||
<Number>0</Number>
|
||||
@ -205,6 +210,41 @@
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>can_out</ItemText>
|
||||
</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>
|
||||
<Tracepoint>
|
||||
<THDelay>0</THDelay>
|
||||
@ -250,7 +290,7 @@
|
||||
<pMultCmdsp></pMultCmdsp>
|
||||
<DebugDescription>
|
||||
<Enable>1</Enable>
|
||||
<EnableFlashSeq>1</EnableFlashSeq>
|
||||
<EnableFlashSeq>0</EnableFlashSeq>
|
||||
<EnableLog>0</EnableLog>
|
||||
<Protocol>2</Protocol>
|
||||
<DbgClock>5000000</DbgClock>
|
||||
@ -967,18 +1007,6 @@
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<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>
|
||||
<FilenameWithoutPath>bsp_can.c</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
@ -986,7 +1014,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>7</GroupNumber>
|
||||
<FileNumber>57</FileNumber>
|
||||
<FileNumber>56</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -998,7 +1026,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>7</GroupNumber>
|
||||
<FileNumber>58</FileNumber>
|
||||
<FileNumber>57</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1010,7 +1038,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>7</GroupNumber>
|
||||
<FileNumber>59</FileNumber>
|
||||
<FileNumber>58</FileNumber>
|
||||
<FileType>5</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1022,7 +1050,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>7</GroupNumber>
|
||||
<FileNumber>60</FileNumber>
|
||||
<FileNumber>59</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1034,7 +1062,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>7</GroupNumber>
|
||||
<FileNumber>61</FileNumber>
|
||||
<FileNumber>60</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1046,7 +1074,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>7</GroupNumber>
|
||||
<FileNumber>62</FileNumber>
|
||||
<FileNumber>61</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1066,7 +1094,7 @@
|
||||
<RteFlg>0</RteFlg>
|
||||
<File>
|
||||
<GroupNumber>8</GroupNumber>
|
||||
<FileNumber>63</FileNumber>
|
||||
<FileNumber>62</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1078,7 +1106,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>8</GroupNumber>
|
||||
<FileNumber>64</FileNumber>
|
||||
<FileNumber>63</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1090,7 +1118,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>8</GroupNumber>
|
||||
<FileNumber>65</FileNumber>
|
||||
<FileNumber>64</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1102,7 +1130,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>8</GroupNumber>
|
||||
<FileNumber>66</FileNumber>
|
||||
<FileNumber>65</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1114,7 +1142,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>8</GroupNumber>
|
||||
<FileNumber>67</FileNumber>
|
||||
<FileNumber>66</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1126,7 +1154,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>8</GroupNumber>
|
||||
<FileNumber>68</FileNumber>
|
||||
<FileNumber>67</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1138,7 +1166,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>8</GroupNumber>
|
||||
<FileNumber>69</FileNumber>
|
||||
<FileNumber>68</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1150,7 +1178,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>8</GroupNumber>
|
||||
<FileNumber>70</FileNumber>
|
||||
<FileNumber>69</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1162,7 +1190,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>8</GroupNumber>
|
||||
<FileNumber>71</FileNumber>
|
||||
<FileNumber>70</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1174,7 +1202,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>8</GroupNumber>
|
||||
<FileNumber>72</FileNumber>
|
||||
<FileNumber>71</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1194,7 +1222,7 @@
|
||||
<RteFlg>0</RteFlg>
|
||||
<File>
|
||||
<GroupNumber>9</GroupNumber>
|
||||
<FileNumber>73</FileNumber>
|
||||
<FileNumber>72</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1206,7 +1234,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>9</GroupNumber>
|
||||
<FileNumber>74</FileNumber>
|
||||
<FileNumber>73</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1218,7 +1246,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>9</GroupNumber>
|
||||
<FileNumber>75</FileNumber>
|
||||
<FileNumber>74</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1230,7 +1258,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>9</GroupNumber>
|
||||
<FileNumber>76</FileNumber>
|
||||
<FileNumber>75</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1242,7 +1270,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>9</GroupNumber>
|
||||
<FileNumber>77</FileNumber>
|
||||
<FileNumber>76</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1254,7 +1282,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>9</GroupNumber>
|
||||
<FileNumber>78</FileNumber>
|
||||
<FileNumber>77</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1274,7 +1302,7 @@
|
||||
<RteFlg>0</RteFlg>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>79</FileNumber>
|
||||
<FileNumber>78</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1286,7 +1314,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>80</FileNumber>
|
||||
<FileNumber>79</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1298,7 +1326,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>81</FileNumber>
|
||||
<FileNumber>80</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1310,7 +1338,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>82</FileNumber>
|
||||
<FileNumber>81</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1322,7 +1350,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>83</FileNumber>
|
||||
<FileNumber>82</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1334,7 +1362,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>84</FileNumber>
|
||||
<FileNumber>83</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1346,7 +1374,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>85</FileNumber>
|
||||
<FileNumber>84</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1366,7 +1394,7 @@
|
||||
<RteFlg>0</RteFlg>
|
||||
<File>
|
||||
<GroupNumber>11</GroupNumber>
|
||||
<FileNumber>86</FileNumber>
|
||||
<FileNumber>85</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1378,7 +1406,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>11</GroupNumber>
|
||||
<FileNumber>87</FileNumber>
|
||||
<FileNumber>86</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1390,7 +1418,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>11</GroupNumber>
|
||||
<FileNumber>88</FileNumber>
|
||||
<FileNumber>87</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1410,7 +1438,7 @@
|
||||
<RteFlg>0</RteFlg>
|
||||
<File>
|
||||
<GroupNumber>12</GroupNumber>
|
||||
<FileNumber>89</FileNumber>
|
||||
<FileNumber>88</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1430,7 +1458,7 @@
|
||||
<RteFlg>0</RteFlg>
|
||||
<File>
|
||||
<GroupNumber>13</GroupNumber>
|
||||
<FileNumber>90</FileNumber>
|
||||
<FileNumber>89</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1442,7 +1470,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>13</GroupNumber>
|
||||
<FileNumber>91</FileNumber>
|
||||
<FileNumber>90</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1454,7 +1482,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>13</GroupNumber>
|
||||
<FileNumber>92</FileNumber>
|
||||
<FileNumber>91</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1466,7 +1494,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>13</GroupNumber>
|
||||
<FileNumber>93</FileNumber>
|
||||
<FileNumber>92</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
|
||||
@ -1143,11 +1143,6 @@
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\User\bsp\pwm.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>map.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\User\bsp\map.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>bsp_can.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
|
||||
Binary file not shown.
52
R2.ioc
52
R2.ioc
@ -155,25 +155,28 @@ Mcu.Pin22=PD14
|
||||
Mcu.Pin23=PA0-WKUP
|
||||
Mcu.Pin24=PA4
|
||||
Mcu.Pin25=PC4
|
||||
Mcu.Pin26=PC5
|
||||
Mcu.Pin27=PE9
|
||||
Mcu.Pin28=PA7
|
||||
Mcu.Pin29=PB0
|
||||
Mcu.Pin26=PE13
|
||||
Mcu.Pin27=PC5
|
||||
Mcu.Pin28=PE9
|
||||
Mcu.Pin29=PE11
|
||||
Mcu.Pin3=PB3
|
||||
Mcu.Pin30=VP_CRC_VS_CRC
|
||||
Mcu.Pin31=VP_FREERTOS_VS_CMSIS_V2
|
||||
Mcu.Pin32=VP_SYS_VS_Systick
|
||||
Mcu.Pin33=VP_TIM4_VS_ClockSourceINT
|
||||
Mcu.Pin34=VP_TIM7_VS_ClockSourceINT
|
||||
Mcu.Pin35=VP_TIM10_VS_ClockSourceINT
|
||||
Mcu.Pin36=VP_USB_DEVICE_VS_USB_DEVICE_CDC_FS
|
||||
Mcu.Pin30=PE14
|
||||
Mcu.Pin31=PA7
|
||||
Mcu.Pin32=PB0
|
||||
Mcu.Pin33=VP_CRC_VS_CRC
|
||||
Mcu.Pin34=VP_FREERTOS_VS_CMSIS_V2
|
||||
Mcu.Pin35=VP_SYS_VS_Systick
|
||||
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.Pin5=PA13
|
||||
Mcu.Pin6=PB7
|
||||
Mcu.Pin7=PB6
|
||||
Mcu.Pin8=PD0
|
||||
Mcu.Pin9=PC11
|
||||
Mcu.PinsNb=37
|
||||
Mcu.PinsNb=40
|
||||
Mcu.ThirdPartyNb=0
|
||||
Mcu.UserConstants=
|
||||
Mcu.UserName=STM32F407IGHx
|
||||
@ -285,12 +288,28 @@ PD14.GPIOParameters=GPIO_Label
|
||||
PD14.GPIO_Label=Buzzer
|
||||
PD14.Locked=true
|
||||
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.GPIO_Label=FlagForUpper
|
||||
PE9.GPIO_PuPd=GPIO_PULLUP
|
||||
PE9.GPIO_Label=LIGHT_A
|
||||
PE9.GPIO_PuPd=GPIO_PULLDOWN
|
||||
PE9.Locked=true
|
||||
PE9.PinState=GPIO_PIN_SET
|
||||
PE9.Signal=GPIO_Output
|
||||
PE9.Signal=SharedStack_PE9
|
||||
PE9.Stacked=true
|
||||
PF6.GPIOParameters=GPIO_Speed,GPIO_Label
|
||||
PF6.GPIO_Label=IMU_HEAT_PWM
|
||||
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_TIM4_CH3.0=TIM4_CH3,PWM Generation3 CH3
|
||||
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.CLKPhase=SPI_PHASE_2EDGE
|
||||
SPI1.CLKPolarity=SPI_POLARITY_HIGH
|
||||
|
||||
@ -320,4 +320,75 @@ int abs_value(int num) {
|
||||
} else {
|
||||
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
|
||||
}
|
||||
@ -14,7 +14,6 @@
|
||||
#include <math.h>
|
||||
#include <stdbool.h>
|
||||
#include "struct_typedef.h"
|
||||
|
||||
#define M_DEG2RAD_MULT (0.01745329251f)
|
||||
#define M_RAD2DEG_MULT (57.2957795131f)
|
||||
|
||||
@ -160,4 +159,10 @@ uint8_t average(uint8_t arr[], uint8_t n);
|
||||
|
||||
int abs_value(int num);
|
||||
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
|
||||
|
||||
@ -22,7 +22,6 @@
|
||||
static int8_t Chassis_SetCtrl(Chassis_t *c,CMD_t *ctrl){
|
||||
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; /*模式未改变直接返回*/
|
||||
//此处源代码处做了pid的reset 待添加
|
||||
c->chassis_ctrl.ctrl =ctrl->CMD_CtrlType ;
|
||||
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;//右前
|
||||
c->hopemotorout.OmniSpeedOut[1] = -Vx-Vy+Vw;//右后
|
||||
c->hopemotorout.OmniSpeedOut[2] = Vx-Vy+Vw;//左后
|
||||
c->hopemotorout.OmniSpeedOut[3] = Vx+Vy+Vw;//左前
|
||||
fp64 Nor_Vx,Nor_Vy;//归一化后的数据
|
||||
normalize_vector(Vx,Vy,&Nor_Vx,&Nor_Vy);
|
||||
|
||||
|
||||
// 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)
|
||||
{
|
||||
@ -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);
|
||||
|
||||
|
||||
|
||||
@ -37,10 +37,10 @@ static const ConfigParam_t param ={
|
||||
.out_limit = 3000.0f,
|
||||
},
|
||||
.Pitch_M2006_angle_param = {
|
||||
.p = 25.0f,
|
||||
.p = 600.0f,
|
||||
.i = 0.0f,
|
||||
.d = 1.5f,
|
||||
.i_limit = 1000.0f,
|
||||
.d = 3.0f,
|
||||
.i_limit = 2000.0f,
|
||||
.out_limit = 3000.0f,
|
||||
},
|
||||
.Pitch_M2006_speed_param={
|
||||
@ -59,11 +59,11 @@ static const ConfigParam_t param ={
|
||||
},
|
||||
|
||||
.Dribble_M3508_speed_param={ //三摩擦速度环
|
||||
.p = 5.0f,
|
||||
.i = 0.3f,
|
||||
.p = 30.0f,
|
||||
.i = 0.45f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 2000.0f,
|
||||
.out_limit = 3000.0f,
|
||||
.i_limit = 3000.0f,
|
||||
.out_limit = 5000.0f,
|
||||
},
|
||||
.Dribble_translate_M3508_speed_param={//平移用3508速度环
|
||||
.p = 5.0f,
|
||||
@ -96,12 +96,17 @@ static const ConfigParam_t param ={
|
||||
/*上层其他参数*/
|
||||
/*运球*/
|
||||
.DribbleConfig_Config = {
|
||||
.dribble_flag=0,
|
||||
.m3508_init_angle = 50,
|
||||
.m3508_high_angle = 1200,
|
||||
.go2_init_angle = 0,
|
||||
.go2_flip_angle = -250,
|
||||
.flip_timing = 200,
|
||||
.go2_release_threshold = -550.0f,
|
||||
.m3508_translate_angle = -930,
|
||||
.m3508_dribble_Reverse_speed=-3500,
|
||||
. m3508_dribble_speed= 4000, // 转动速度
|
||||
.m3508_dribble_init_speed=0,
|
||||
|
||||
.light_3508_flag=0,//3508平移处的光电，0初始，1到位置
|
||||
.light_ball_flag=0,//检测球的flag
|
||||
|
||||
|
||||
},
|
||||
/*投球*/
|
||||
.PitchConfig_Config = {
|
||||
@ -110,7 +115,7 @@ static const ConfigParam_t param ={
|
||||
.go1_init_position = -50,
|
||||
.go1_release_threshold =-210,
|
||||
.m2006_Screw_init=0,
|
||||
.Pitch_angle =57,
|
||||
.Pitch_angle =56,
|
||||
},
|
||||
|
||||
|
||||
|
||||
295
User/Module/up.c
295
User/Module/up.c
@ -3,6 +3,15 @@
|
||||
#include "user_math.h"
|
||||
#include "bsp_buzzer.h"
|
||||
#include "bsp_delay.h"
|
||||
/*接线
|
||||
|
||||
上层用到三个光电
|
||||
1.PE9 发射用 输出高电平 LIGHTA
|
||||
2.PE11 运球上3508 输出高电平 LIGHTB
|
||||
3.PE13 三摩擦检测球 输出高电平LIGHTC
|
||||
|
||||
|
||||
*/
|
||||
|
||||
#define GEAR_RATIO_2006 (36) // 2006减速比
|
||||
#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 RELAY2_TOGGLE(state) HAL_GPIO_WritePin(GPIOE, GPIO_PIN_11, (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)
|
||||
{
|
||||
@ -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_speed ,PID_POSITION ,&(u->param->Dribble_translate_M3508_speed_param ));
|
||||
|
||||
PID_init (&u->pid .Dribble_M3508_speed ,PID_POSITION ,&(u->param->Dribble_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 ;
|
||||
|
||||
// 初始化上层状态机
|
||||
if (!u->DribbleContext .is_initialized) { //检查是否为第一次运行状态机，运球
|
||||
u->DribbleContext .DribbleConfig = u->param ->DribbleConfig_Config ;//赋值
|
||||
u->DribbleContext .DribbleState = Dribble_PREPARE;
|
||||
u->DribbleContext .DribbleState = DRIBBLE_PREPARE;
|
||||
u->DribbleContext .is_initialized = 1;
|
||||
|
||||
}
|
||||
@ -197,10 +205,10 @@ int8_t GO_SendData(UP_t *u, float pos, float limit)
|
||||
int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
|
||||
{
|
||||
//电机控制 ，传进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,0x202,&u->motorfeedback .DJmotor_feedback[1] ,&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,u->motor_target .Dribble_M3508_speed);
|
||||
DJ_Angle_Control(u,0x204,&u->motorfeedback .DJmotor_feedback[3] ,
|
||||
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 );
|
||||
@ -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_speed ,
|
||||
// u->motor_target .Pitch_M2006_angle );
|
||||
u->final_out .DJfinal_out [5]=PID_calc (&(u->pid .Pitch_Angle_M2006_speed ),
|
||||
u->motorfeedback .Encoder.angle,u->motor_target .Shoot_Pitch_angle);
|
||||
DJ_Speed_Control(u,0x206,&u->motorfeedback .DJmotor_feedback[5],&u->pid .Pitch_M2006_speed,
|
||||
(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 );
|
||||
|
||||
@ -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)
|
||||
@ -241,9 +252,12 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
|
||||
|
||||
if(u ==NULL) return 0;
|
||||
static int is_pitch=1;
|
||||
|
||||
|
||||
|
||||
switch (c->CMD_CtrlType )
|
||||
{
|
||||
case RCcontrol: //在手动模式下
|
||||
case RCcontrol: //在手动模式下
|
||||
|
||||
switch (c-> CMD_mode )
|
||||
{
|
||||
@ -252,23 +266,22 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
|
||||
/*投篮*/
|
||||
if(is_pitch){
|
||||
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;
|
||||
|
||||
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->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);//关闭气缸
|
||||
for(int i=0;i<3;i++){
|
||||
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; //重置最初状态
|
||||
|
||||
|
||||
// RELAY1_TOGGLE (1);//夹球，0夹1开
|
||||
// RELAY2_TOGGLE (0);//球，0接1收
|
||||
//
|
||||
|
||||
break;
|
||||
|
||||
case Pitch :
|
||||
@ -280,31 +293,28 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
|
||||
Pitch_Process(u,out,c);
|
||||
|
||||
break ;
|
||||
case UP_Adjust:
|
||||
case UP_Adjust: //测试用
|
||||
|
||||
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->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 ;
|
||||
case Dribble:
|
||||
{
|
||||
/*夹球 -> 3508 升起 ，同时go2翻转 -> 到位置后，继电器开，放球，同时3508降，go2翻回->接球，收 */
|
||||
|
||||
if(u->DribbleContext.DribbleState== Dribble_PREPARE){
|
||||
|
||||
u->DribbleContext .DribbleState =STATE_GRAB_BALL;
|
||||
if(u->DribbleContext.DribbleState== DRIBBLE_PREPARE){
|
||||
u->DribbleContext .DribbleState=DRIBBLE_TRANSLATE;
|
||||
}
|
||||
// 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 ;
|
||||
case Dribbl_transfer:
|
||||
break ;
|
||||
// Dribbl_transfer_a(u,out);
|
||||
}
|
||||
break;
|
||||
|
||||
|
||||
|
||||
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)
|
||||
{
|
||||
|
||||
// go1_position=u->PitchContext .PitchConfig .go1_init_position ;
|
||||
// M2006_Screw_position=u->PitchContext .PitchConfig .m2006_Screw_init;
|
||||
|
||||
|
||||
switch(u->PitchContext .PitchState){
|
||||
|
||||
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:
|
||||
|
||||
|
||||
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->PitchContext.PitchConfig.go1_init_position += c->Vx ;
|
||||
// u->PitchContext.PitchConfig.m2006_Screw_init += c->Vy;
|
||||
|
||||
}
|
||||
|
||||
break ;
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
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)
|
||||
{
|
||||
|
||||
u->PitchContext.PitchConfig.go1_init_position += c->Vx ;
|
||||
u->PitchContext.PitchConfig.Pitch_angle += c->Vy;
|
||||
u->motor_target .Shoot_Pitch_angle=u->PitchContext.PitchConfig.Pitch_angle;
|
||||
|
||||
static int common_speed_flag=0;//是否共速
|
||||
|
||||
switch (u->DribbleContext.DribbleState) {
|
||||
|
||||
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;
|
||||
|
||||
// }
|
||||
//}
|
||||
|
||||
@ -76,23 +76,29 @@ typedef struct {
|
||||
|
||||
/*运球*/
|
||||
typedef enum {
|
||||
Dribble_PREPARE,
|
||||
STATE_GRAB_BALL, // 夹球升起阶段
|
||||
STATE_RELEASE_BALL, // 释放球体
|
||||
STATE_CATCH_PREP, // 接球准备
|
||||
STATE_CATCH_BALL, // 接球动作
|
||||
STATE_TRANSFER, //转移球
|
||||
STATE_CATCH_DONE //完成
|
||||
DRIBBLE_PREPARE, // 准备阶段
|
||||
DRIBBLE_START,
|
||||
DRIBBLE_TRANSLATE, // 平移过程
|
||||
DRIBBLE_PROCESS_DOWN, // 运球过程，球下落
|
||||
DRIBBLE_PROCESS_UP, // 运球过程，球上升
|
||||
DRIBBLE_DONE // 运球结束
|
||||
} DribbleState_t;
|
||||
|
||||
/* 参数配置结构体 */
|
||||
typedef struct {
|
||||
fp32 m3508_init_angle; // 3508初始角度
|
||||
fp32 m3508_high_angle; // 3508升起角度
|
||||
fp32 go2_init_angle; // GO2初始角度
|
||||
fp32 go2_flip_angle; // GO2翻转角度
|
||||
fp32 flip_timing; // 翻转触发时机
|
||||
fp32 go2_release_threshold; // 释放球
|
||||
int8_t dribble_flag;//当移动三摩擦后为1，否则为0，防止发射撞到
|
||||
|
||||
fp32 m3508_init_angle; // 平移前位置
|
||||
fp32 m3508_translate_angle; // 平移后位置
|
||||
fp32 m3508_dribble_init_speed;
|
||||
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;
|
||||
|
||||
/* 状态机上下文 */
|
||||
@ -102,6 +108,8 @@ typedef struct {
|
||||
|
||||
uint8_t is_initialized;
|
||||
|
||||
|
||||
|
||||
} DribbleContext_t;
|
||||
|
||||
|
||||
@ -191,10 +199,11 @@ typedef struct{
|
||||
fp32 Pitch_M2006_angle;
|
||||
|
||||
fp32 go_shoot;
|
||||
fp32 Dribble_M3508_speed;//运球转速
|
||||
fp32 Dribble_M3508_speed[3];//运球转速
|
||||
fp32 Dribble_translate_M3508_angle;//平移用的3508转动角度
|
||||
|
||||
fp32 Shoot_Pitch_angle;
|
||||
|
||||
}motor_target;
|
||||
|
||||
struct{
|
||||
@ -211,7 +220,7 @@ typedef struct{
|
||||
|
||||
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_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_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);
|
||||
|
||||
|
||||
|
||||
@ -56,14 +56,31 @@ static void CAN_DJIMotor_Decode(CAN_MotorFeedback_t *feedback,
|
||||
feedback->torque_current =
|
||||
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) {
|
||||
if (feedback == NULL || raw == NULL) return;
|
||||
feedback->raw_dis[0] = (uint16_t)((raw[0] << 8) | raw[1]);
|
||||
feedback->raw_dis[1] = (uint16_t)((raw[2] << 8) | raw[3]);
|
||||
feedback->raw_dis[2] = (uint16_t)((raw[4] << 8) | raw[5]);
|
||||
feedback->raw_dis[3]= (uint16_t)((raw[6] << 8) | raw[7]);
|
||||
|
||||
if (feedback == NULL || raw == NULL) return;
|
||||
switch (id)
|
||||
{
|
||||
case CAN_SICK_ID_1:
|
||||
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,
|
||||
const uint8_t *raw) {
|
||||
@ -76,7 +93,7 @@ void CAN_Encoder_Decode(CAN_EncoderFeedback_t *feedback,
|
||||
{
|
||||
case 0x01:
|
||||
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;
|
||||
}
|
||||
}
|
||||
@ -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);
|
||||
detect_hook(CHASSIS3508M1_TOE + index);
|
||||
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:
|
||||
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);
|
||||
|
||||
break;
|
||||
case CAN_SICK_ID:
|
||||
// 存储消息到sickfed结构体中
|
||||
CAN_Sick_Receive(&(can->sickfed), can_rx->rx_data);
|
||||
break;
|
||||
|
||||
case CAN_Encoder_ID:
|
||||
CAN_Encoder_Decode(&(can->Oid),can_rx->rx_data);
|
||||
can->recive_flag |= 1 << 10;
|
||||
|
||||
@ -18,7 +18,10 @@ typedef enum {
|
||||
CAN_UP_M3508_M5_ID = 0x205, /* 1 */
|
||||
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,
|
||||
|
||||
@ -121,7 +124,7 @@ typedef struct {
|
||||
/* sick反馈信息 */
|
||||
typedef struct {
|
||||
uint16_t raw_dis[4];
|
||||
int changed_dis[4];
|
||||
|
||||
}CAN_SickFeedback_t;
|
||||
/* encoder编码器反馈信息 */
|
||||
typedef struct {
|
||||
@ -197,5 +200,6 @@ void CAN_Encoder_Control(CAN_t *can);
|
||||
|
||||
void CAN_Encoder_Decode(CAN_EncoderFeedback_t *feedback,
|
||||
const uint8_t *raw) ;
|
||||
|
||||
static void CAN_Sick_Receive(CAN_SickFeedback_t *feedback,uint16_t id,
|
||||
const uint8_t *raw) ;
|
||||
#endif
|
||||
|
||||
@ -196,8 +196,9 @@ int8_t CMD_LD(CMD_t *cmd,const CMD_RC_t *rc){
|
||||
}
|
||||
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_H==CMD_SW_UP) cmd ->CMD_mode =UP_Adjust;
|
||||
else cmd ->CMD_mode =Normal;
|
||||
}
|
||||
}
|
||||
|
||||
@ -51,9 +51,7 @@ void Task_Chassis(void *argument)
|
||||
|
||||
uint32_t tick = osKernelGetTickCount();
|
||||
|
||||
|
||||
HAL_GPIO_WritePin(FlagForUpper_GPIO_Port,FlagForUpper_Pin,GPIO_PIN_RESET); //拉低电平 避免未清除
|
||||
|
||||
|
||||
while(1)
|
||||
{
|
||||
#ifdef DEBUG
|
||||
|
||||
@ -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);
|
||||
}
|
||||
// }
|
||||
// else HAL_GPIO_TogglePin(GPIOH,LED_R_Pin);
|
||||
|
||||
for (int i = 0; i < ERROR_LIST_LENGHT; i++)
|
||||
|
||||
Loading…
Reference in New Issue
Block a user