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

Compare commits

merge into: LYL:main
Branches Tags
LYL:main
LYL:new-branch
LYL:up_try
LYL:up_branch
LYL:JIAXING_UP
LYL:xiaomi
...
pull from: LYL:up_try
Branches Tags
LYL:new-branch
LYL:up_try
LYL:up_branch
LYL:JIAXING_UP
LYL:main
LYL:xiaomi

8 Commits
main ... up_try

Author SHA1 Message Date
ZHAISHUI04
0ef286e5f2 更新 2025-04-14 21:11:43 +08:00
ZHAISHUI04
37dc1ac1e9 目的，无线串口发送需要数据 2025-04-13 14:55:49 +08:00
ZHAISHUI04
3d6cb3d799 最终了，不大改了，改改参数就这样了 2025-04-07 19:58:25 +08:00
ZHAISHUI04
0aee725ece 重新写的流程，状态机实现，部分数据需要改 2025-04-05 01:59:17 +08:00
ZHAISHUI04
fe1c02b130 r2上能用，有点慢，代码待优化 2025-04-02 17:07:58 +08:00
ZHAISHUI04
6cbf728f10 恢复stash 2025-03-31 16:17:10 +08:00
ZHAISHUI04
1488098d60 3.30更改的暂存区丢失，这个修改了部分，处于可用状态，但不完善 2025-03-31 16:13:20 +08:00
ZHAISHUI04
5de502c448 只提交测试后的，遥控器投篮完成 2025-03-29 14:16:44 +08:00
33 changed files with 962 additions and 1679 deletions
Show Stats Expand all files Collapse all files

28
AUTO_CHASSIS.ioc
View File

@ -145,23 +145,24 @@ Mcu.Pin24=PA4
Mcu.Pin25=PC4
Mcu.Pin26=PC5
Mcu.Pin27=PE9
Mcu.Pin28=PA7
Mcu.Pin29=PB0
Mcu.Pin28=PE11
Mcu.Pin29=PA7
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=PB0
Mcu.Pin31=VP_CRC_VS_CRC
Mcu.Pin32=VP_FREERTOS_VS_CMSIS_V2
Mcu.Pin33=VP_SYS_VS_Systick
Mcu.Pin34=VP_TIM4_VS_ClockSourceINT
Mcu.Pin35=VP_TIM7_VS_ClockSourceINT
Mcu.Pin36=VP_TIM10_VS_ClockSourceINT
Mcu.Pin37=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=38
Mcu.ThirdPartyNb=0
Mcu.UserConstants=
Mcu.UserName=STM32F407IGHx
@ -272,9 +273,14 @@ PD14.GPIOParameters=GPIO_Label
PD14.GPIO_Label=Buzzer
PD14.Locked=true
PD14.Signal=S_TIM4_CH3
PE11.GPIOParameters=PinState,GPIO_PuPd
PE11.GPIO_PuPd=GPIO_PULLDOWN
PE11.Locked=true
PE11.PinState=GPIO_PIN_SET
PE11.Signal=GPIO_Output
PE9.GPIOParameters=PinState,GPIO_PuPd,GPIO_Label
PE9.GPIO_Label=FlagForUpper
PE9.GPIO_PuPd=GPIO_PULLUP
PE9.GPIO_PuPd=GPIO_PULLDOWN
PE9.Locked=true
PE9.PinState=GPIO_PIN_SET
PE9.Signal=GPIO_Output

2
Core/Inc/stm32f4xx_hal_conf.h
View File

@ -190,7 +190,7 @@
#define USE_HAL_SMBUS_REGISTER_CALLBACKS 0U /* SMBUS register callback disabled */
#define USE_HAL_SPI_REGISTER_CALLBACKS 0U /* SPI register callback disabled */
#define USE_HAL_TIM_REGISTER_CALLBACKS 0U /* TIM register callback disabled */
#define USE_HAL_UART_REGISTER_CALLBACKS 1U /* UART register callback disabled */
#define USE_HAL_UART_REGISTER_CALLBACKS 0U /* UART register callback disabled */
#define USE_HAL_USART_REGISTER_CALLBACKS 0U /* USART register callback disabled */
#define USE_HAL_WWDG_REGISTER_CALLBACKS 0U /* WWDG register callback disabled */

10
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);
/*Configure GPIO pin Output Level */
HAL_GPIO_WritePin(FlagForUpper_GPIO_Port, FlagForUpper_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(GPIOE, FlagForUpper_Pin|GPIO_PIN_11, GPIO_PIN_SET);
/*Configure GPIO pin Output Level */
HAL_GPIO_WritePin(GYRO_CS_GPIO_Port, GYRO_CS_Pin, GPIO_PIN_SET);
@ -92,12 +92,12 @@ void MX_GPIO_Init(void)
GPIO_InitStruct.Pull = GPIO_PULLUP;
HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
/*Configure GPIO pin : PtPin */
GPIO_InitStruct.Pin = FlagForUpper_Pin;
/*Configure GPIO pins : PEPin PE11 */
GPIO_InitStruct.Pin = FlagForUpper_Pin|GPIO_PIN_11;
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(GPIOE, &GPIO_InitStruct);
/*Configure GPIO pin : PtPin */
GPIO_InitStruct.Pin = GYRO_CS_Pin;

5
Core/Src/main.c
View File

@ -32,7 +32,6 @@
/* USER CODE BEGIN Includes */
#include "bsp_delay.h"
#include "bsp_can.h"
#include "Action.h"
//#include "bsp_dwt.h"
/* USER CODE END Includes */
@ -80,7 +79,8 @@ int main(void)
/* MCU Configuration--------------------------------------------------------*/
/* Reset of all peripherals, Initializes the Flash interface and the Systick. * HAL_Init();
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
HAL_Init();
/* USER CODE BEGIN Init */
@ -95,7 +95,6 @@ int main(void)
/* Initialize all configured peripherals */
MX_GPIO_Init();
MX_DMA_Init();
MX_SPI1_Init();
MX_TIM4_Init();

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

@ -475,3 +475,5 @@ Log at : 2025/2/23|14:39:36|GMT+0800
[info] Log at : 2025/3/11|21:09:29|GMT+0800
[info] Log at : 2025/3/28|21:47:12|GMT+0800

130
MDK-ARM/AUTO_CHASSIS.uvoptx
View File

@ -103,7 +103,7 @@
<bEvRecOn>1</bEvRecOn>
<bSchkAxf>0</bSchkAxf>
<bTchkAxf>0</bTchkAxf>
<nTsel>6</nTsel>
<nTsel>3</nTsel>
<sDll></sDll>
<sDllPa></sDllPa>
<sDlgDll></sDlgDll>
@ -114,13 +114,13 @@
<tDlgDll></tDlgDll>
<tDlgPa></tDlgPa>
<tIfile></tIfile>
<pMon>STLink\ST-LINKIII-KEIL_SWO.dll</pMon>
<pMon>BIN\CMSIS_AGDI.dll</pMon>
</DebugOpt>
<TargetDriverDllRegistry>
<SetRegEntry>
<Number>0</Number>
<Key>ST-LINKIII-KEIL_SWO</Key>
<Name>-U00160029510000164E574E32 -O206 -SF5000 -C0 -A0 -I0 -HNlocalhost -HP7184 -P1 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO131091 -TC12000000 -TT12000000 -TP21 -TDS8005 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)</Name>
<Name>-U00260035480000034E575152 -O206 -SF5000 -C0 -A0 -I0 -HNlocalhost -HP7184 -P1 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO131091 -TC12000000 -TT12000000 -TP21 -TDS8005 -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>
@ -140,7 +140,7 @@
<SetRegEntry>
<Number>0</Number>
<Key>CMSIS_AGDI</Key>
<Name>-X"Horco CMSIS-DAP" -U8626380832 -O206 -S0 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO65554 -TC168000000 -TT168000000 -TP20 -TDS8010 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)</Name>
<Name>-X"Horco CMSIS-DAP" -U4626385832 -O206 -S0 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO65554 -TC168000000 -TT168000000 -TP20 -TDS8010 -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>
@ -158,42 +158,32 @@
<Ww>
<count>0</count>
<WinNumber>1</WinNumber>
<ItemText>raw,0x0A</ItemText>
<ItemText>UP,0x0A</ItemText>
</Ww>
<Ww>
<count>1</count>
<WinNumber>1</WinNumber>
<ItemText>UP,0x0A</ItemText>
<ItemText>GO_motor_info</ItemText>
</Ww>
<Ww>
<count>2</count>
<WinNumber>1</WinNumber>
<ItemText>can,0x0A</ItemText>
<ItemText>dr16,0x0A</ItemText>
</Ww>
<Ww>
<count>3</count>
<WinNumber>1</WinNumber>
<ItemText>can_out,0x0A</ItemText>
<ItemText>cmd,0x0A</ItemText>
</Ww>
<Ww>
<count>4</count>
<WinNumber>1</WinNumber>
<ItemText>bbb,0x0A</ItemText>
<ItemText>rc_ctrl,0x0A</ItemText>
</Ww>
<Ww>
<count>5</count>
<WinNumber>1</WinNumber>
<ItemText>aaa,0x0A</ItemText>
</Ww>
<Ww>
<count>6</count>
<WinNumber>1</WinNumber>
<ItemText>CCC</ItemText>
</Ww>
<Ww>
<count>7</count>
<WinNumber>1</WinNumber>
<ItemText>GO_motor_info</ItemText>
<ItemText>aaa</ItemText>
</Ww>
</WatchWindow1>
<WatchWindow2>
@ -429,7 +419,7 @@
<Group>
<GroupName>Drivers/STM32F4xx_HAL_Driver</GroupName>
<tvExp>0</tvExp>
<tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
@ -701,7 +691,7 @@
<Group>
<GroupName>Drivers/CMSIS</GroupName>
<tvExp>0</tvExp>
<tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
@ -1116,18 +1106,6 @@
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>..\User\task\nuc_task.c</PathWithFileName>
<FilenameWithoutPath>nuc_task.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>68</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>..\User\task\error_detect_task.c</PathWithFileName>
<FilenameWithoutPath>error_detect_task.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
@ -1135,7 +1113,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>69</FileNumber>
<FileNumber>68</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1147,7 +1125,7 @@
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>70</FileNumber>
<FileNumber>69</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1157,6 +1135,30 @@
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>70</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>..\User\task\cmd_task.c</PathWithFileName>
<FilenameWithoutPath>cmd_task.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>71</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>..\User\task\nuc_task.c</PathWithFileName>
<FilenameWithoutPath>nuc_task.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
</Group>
<Group>
@ -1167,7 +1169,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>71</FileNumber>
<FileNumber>72</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1177,18 +1179,6 @@
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>72</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>..\User\task\cmd_task.c</PathWithFileName>
<FilenameWithoutPath>cmd_task.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>73</FileNumber>
@ -1249,18 +1239,6 @@
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>78</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>..\User\Algorithm\navi.c</PathWithFileName>
<FilenameWithoutPath>navi.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
</Group>
<Group>
@ -1271,7 +1249,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>79</FileNumber>
<FileNumber>78</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1283,7 +1261,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>80</FileNumber>
<FileNumber>79</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1295,7 +1273,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>81</FileNumber>
<FileNumber>80</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1307,7 +1285,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>82</FileNumber>
<FileNumber>81</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1319,7 +1297,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>83</FileNumber>
<FileNumber>82</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1331,7 +1309,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>84</FileNumber>
<FileNumber>83</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1343,7 +1321,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>85</FileNumber>
<FileNumber>84</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1363,7 +1341,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>11</GroupNumber>
<FileNumber>86</FileNumber>
<FileNumber>85</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1375,7 +1353,7 @@
</File>
<File>
<GroupNumber>11</GroupNumber>
<FileNumber>87</FileNumber>
<FileNumber>86</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1387,7 +1365,7 @@
</File>
<File>
<GroupNumber>11</GroupNumber>
<FileNumber>88</FileNumber>
<FileNumber>87</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1407,7 +1385,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>12</GroupNumber>
<FileNumber>89</FileNumber>
<FileNumber>88</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1427,7 +1405,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>13</GroupNumber>
<FileNumber>90</FileNumber>
<FileNumber>89</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1439,7 +1417,7 @@
</File>
<File>
<GroupNumber>13</GroupNumber>
<FileNumber>91</FileNumber>
<FileNumber>90</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1451,7 +1429,7 @@
</File>
<File>
<GroupNumber>13</GroupNumber>
<FileNumber>92</FileNumber>
<FileNumber>91</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1463,7 +1441,7 @@
</File>
<File>
<GroupNumber>13</GroupNumber>
<FileNumber>93</FileNumber>
<FileNumber>92</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>

76
MDK-ARM/AUTO_CHASSIS.uvprojx
View File

@ -1208,11 +1208,6 @@
<FileType>1</FileType>
<FilePath>..\User\task\can_task.c</FilePath>
</File>
<File>
<FileName>nuc_task.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\task\nuc_task.c</FilePath>
</File>
<File>
<FileName>error_detect_task.c</FileName>
<FileType>1</FileType>
@ -1228,6 +1223,67 @@
<FileType>1</FileType>
<FilePath>..\User\task\up_task.c</FilePath>
</File>
<File>
<FileName>cmd_task.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\task\cmd_task.c</FilePath>
<FileOption>
<CommonProperty>
<UseCPPCompiler>2</UseCPPCompiler>
<RVCTCodeConst>0</RVCTCodeConst>
<RVCTZI>0</RVCTZI>
<RVCTOtherData>0</RVCTOtherData>
<ModuleSelection>0</ModuleSelection>
<IncludeInBuild>1</IncludeInBuild>
<AlwaysBuild>2</AlwaysBuild>
<GenerateAssemblyFile>2</GenerateAssemblyFile>
<AssembleAssemblyFile>2</AssembleAssemblyFile>
<PublicsOnly>2</PublicsOnly>
<StopOnExitCode>11</StopOnExitCode>
<CustomArgument></CustomArgument>
<IncludeLibraryModules></IncludeLibraryModules>
<ComprImg>1</ComprImg>
</CommonProperty>
<FileArmAds>
<Cads>
<interw>2</interw>
<Optim>0</Optim>
<oTime>2</oTime>
<SplitLS>2</SplitLS>
<OneElfS>2</OneElfS>
<Strict>2</Strict>
<EnumInt>2</EnumInt>
<PlainCh>2</PlainCh>
<Ropi>2</Ropi>
<Rwpi>2</Rwpi>
<wLevel>0</wLevel>
<uThumb>2</uThumb>
<uSurpInc>2</uSurpInc>
<uC99>2</uC99>
<uGnu>2</uGnu>
<useXO>2</useXO>
<v6Lang>0</v6Lang>
<v6LangP>0</v6LangP>
<vShortEn>2</vShortEn>
<vShortWch>2</vShortWch>
<v6Lto>2</v6Lto>
<v6WtE>2</v6WtE>
<v6Rtti>2</v6Rtti>
<VariousControls>
<MiscControls></MiscControls>
<Define></Define>
<Undefine></Undefine>
<IncludePath></IncludePath>
</VariousControls>
</Cads>
</FileArmAds>
</FileOption>
</File>
<File>
<FileName>nuc_task.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\task\nuc_task.c</FilePath>
</File>
</Files>
</Group>
<Group>
@ -1238,11 +1294,6 @@
<FileType>1</FileType>
<FilePath>..\User\Algorithm\filter.c</FilePath>
</File>
<File>
<FileName>cmd_task.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\task\cmd_task.c</FilePath>
</File>
<File>
<FileName>ahrs.c</FileName>
<FileType>1</FileType>
@ -1268,11 +1319,6 @@
<FileType>1</FileType>
<FilePath>..\User\Algorithm\pid.c</FilePath>
</File>
<File>
<FileName>navi.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\Algorithm\navi.c</FilePath>
</File>
</Files>
</Group>
<Group>

BIN
MDK-ARM/ELE_CHASSIS/AUTO_CHASSIS.axf
View File

Binary file not shown.

21
User/Algorithm/user_math.c
View File

@ -214,6 +214,14 @@ void abs_limit_fp(fp32 *num, fp32 Limit)
}
}
void limit(double *a, double b, double c) {
if (*a < b) {
*a = b; // 如果 a 小于下限 b，将 a 设置为 b
} else if (*a > c) {
*a = c; // 如果 a 大于上限 c，将 a 设置为 c
}
// 如果 a 在范围 [b, c] 内，不做任何修改
}
/* 移动向量 */
MoveVector_t *mv;
@ -309,3 +317,16 @@ uint8_t average(uint8_t arr[], uint8_t n) {
}
return (float) sum / n;
}
bool is_arrived(float target, float current, float mistake) {
// 计算当前值与目标值的差值的绝对值
float diff = fabs(target - current);
// 判断是否在误差范围内
if (diff <= mistake) {
return true; // 在误差范围内
} else {
return false; // 不在误差范围内
}
}

4
User/Algorithm/user_math.h
View File

@ -157,4 +157,8 @@ PolarCoordinate_t addPolarVectors(PolarCoordinate_t v1, PolarCoordinate_t v2);
/// @param n 元素数
/// @return 平均值
uint8_t average(uint8_t arr[], uint8_t n);
/*判断是否在误差内*/
bool is_arrived(float target, float current, float mistake) ;
void limit(double *a, double b, double c) ;
#endif

231
User/Module/Chassis.c
View File

@ -1,231 +0,0 @@
#include "Chassis.h"
#include "gpio.h"
#include "Action.h"
#include "user_math.h"
#include "bsp_buzzer.h"
#include "bsp_delay.h"
/*机器人坐标系，向前x，右y，上yaw
不同于nuc (前x，左y，上yaw) */
/*
x
|
--y
*/
static int8_t Chassis_SetCtrl(Chassis_t *c,CMD_t *ctrl){
if (c == NULL) return CHASSIS_ERR_NULL; /*主结构体不能为空 */
if (ctrl->C_cmd.type== c->ctrl && ctrl->C_cmd.mode== c->mode) return CHASSIS_OK; /*模式未改变直接返回*/
//此处源代码处做了pid的reset 待添加
c->ctrl =ctrl->C_cmd.type;
c->mode =ctrl->C_cmd.mode;
return CHASSIS_OK;
} //设置控制模式
/*该函数用来更新can任务获得的电机反馈值*/
int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can) {
if (c == NULL) return CHASSIS_ERR_NULL;
if (can == NULL) return CHASSIS_ERR_NULL;
for (uint8_t i = 0; i < 4; i++)
{
c->motorfeedback.rotor_rpm3508[i] = can->motor.motor3508.as_array[i].rotor_speed;
c->motorfeedback.rotor_current3508[i] = can->motor.motor3508.as_array[i].torque_current;
}
c->motorfeedback.rotor_pit6020angle = can->motor.pit6020.as_array[0].rotor_ecd;
c->motorfeedback.rotor_pit6020rpm = can->motor.pit6020.as_array[0].rotor_speed;
c->motorfeedback.rotor_gimbal_yawangle = can->motor.chassis6020.as_array[0].rotor_ecd;
c->motorfeedback.rotor_gimbal_yawrpm = can->motor.chassis6020.as_array[0].rotor_speed;
c->motorfeedback.rotor_gimbal_pitchangle = can->motor.chassis6020.as_array[1].rotor_ecd;
c->motorfeedback.rotor_gimbal_pitchrpm = can->motor.chassis6020.as_array[1].rotor_speed;
c->sick_dis[0] = can->sickfed.raw_dis[0];
c->sick_dis[1] = can->sickfed.raw_dis[1];
c->sick_dis[2] = can->sickfed.raw_dis[2];
c->sick_dis[3] = can->sickfed.raw_dis[3];
return CHASSIS_OK;
}
int8_t Chassis_init(Chassis_t *c,const Chassis_Param_t *param,float target_freq)
{
if (c == NULL) return CHASSIS_ERR_NULL;
c->param = param; /*初始化参数 */
for(int i =0 ; i < 4 ; i++)
{
PID_init(&(c->pid.chassis_3508VecPID[i]),PID_POSITION_D,&(c->param->M3508_param)); //带D项滤波
}
PID_init((&c->pid.chassis_pitAngle6020),PID_POSITION,&(c->param->C6020pitAngle_param));//尝试位置控制角度
PID_init((&c->pid.chassis_pitOmega6020),PID_POSITION,&(c->param->C6020pitOmega_param));
PID_init((&c->pid.chassis_gimbal_yawAnglePID),PID_POSITION,&(c->param->Gimbal_yawAngle_param));//尝试位置控制角度
PID_init((&c->pid.chassis_gimbal_yawOmegaPID),PID_POSITION,&(c->param->Gimbal_yawOmega_param));
PID_init((&c->pid.chassis_gimbal_pitchAnglePID),PID_POSITION,&(c->param->Gimbal_pitchAngle_param));//尝试位置控制角度
PID_init((&c->pid.chassis_gimbal_pitchOmegaPID),PID_POSITION,&(c->param->Gimbal_pitchOmega_param));
PID_init(&(c->pid.chassis_NaviVxPID),PID_POSITION,&(c->param->NaviVx_param));
PID_init(&(c->pid.chassis_NaviVyPID),PID_POSITION,&(c->param->NaviVy_param));
PID_init(&(c->pid.chassis_NaviWzPID),PID_POSITION,&(c->param->NaviVw_param));
PID_init(&(c->pid.sick_CaliforYPID),PID_POSITION,&(c->param->Sick_CaliYparam));
PID_init(&(c->pid.sick_CaliforXPID),PID_POSITION,&(c->param->Sick_CaliXparam));
LowPassFilter2p_Init(&(c->filled[0]),target_freq,80.0f); //给角加速度做滤波
LowPassFilter2p_Init(&(c->filled[1]),target_freq,80.0f); //给w 做滤波
LowPassFilter2p_Init(&(c->filled[2]),target_freq,80.0f); //给y做滤波
LowPassFilter2p_Init(&(c->filled[3]),target_freq,80.0f); //给x 做滤波
//
return CHASSIS_OK;
}
void Chassis_speed_calculate(Chassis_t *c,fp32 Vx,fp32 Vy,fp32 Vw) //底盘逆运动学的解算
{
c->hopemotorout.OmniSpeedOut[3] = -Vx+Vy+Vw;//右前
c->hopemotorout.OmniSpeedOut[0] = -Vx-Vy+Vw;//右后
c->hopemotorout.OmniSpeedOut[1] = Vx-Vy+Vw;//左后
c->hopemotorout.OmniSpeedOut[2] = Vx+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)
{
if(c ==NULL) return CHASSIS_ERR_NULL;
if(ctrl ==NULL) return CHASSIS_ERR_NULL;
Chassis_SetCtrl(c,ctrl);
//此处对imu加滤波做修正
c->pos088.bmi088.filtered_gyro.z =LowPassFilter2p_Apply(&(c->filled[0]),c->pos088.bmi088.gyro.z);
switch (c->ctrl)
{
case RC:
/*
在cmd里对数据进行处理 包括方向和油门
6000为全向轮的倍率,遥控器坐标系和机器人坐标系不同*/
c->move_vec.Vw = ctrl->Vw*6000;
c->move_vec.Vx = -ctrl->Vy*6000;
c->move_vec.Vy = ctrl->Vx*6000;
if(c->mode == RC_MODE1 ){
}
break;
case MID_NAVI:
// //这套是全向轮的方向,一定要注意这里的xy方向
c->move_vec.Vw =ctrl->C_navi.wz ;
c->move_vec.Vx =ctrl->C_navi.vy ;
c->move_vec.Vy =ctrl->C_navi.vx ;
c->move_vec.Vw =LowPassFilter2p_Apply(&(c->filled[1]),c->move_vec.Vw);
c->move_vec.Vy =LowPassFilter2p_Apply(&(c->filled[2]),c->move_vec.Vy);
c->move_vec.Vx =LowPassFilter2p_Apply(&(c->filled[3]),c->move_vec.Vx);
c->move_vec.Vw =PID_calc(&(c->pid.chassis_NaviWzPID),c->pos088.bmi088.gyro.z,c->move_vec.Vw);
c->move_vec.Vx =PID_calc(&(c->pid.chassis_NaviVxPID),c->pos088.bmi088.accl.y,c->move_vec.Vx);
c->move_vec.Vy =PID_calc(&(c->pid.chassis_NaviVyPID),c->pos088.bmi088.accl.x,c->move_vec.Vy);
if(ctrl->status[5] ==1)
{
c->move_vec.Vw = c->move_vec.Vw * 0.8f;
c->move_vec.Vx = c->move_vec.Vx * 0.5f;
c->move_vec.Vy = c->move_vec.Vy * 0.5f;
}
break;
}
/*怎么用*/
switch (c->mode)
{
case RELAXED:
c->move_vec.Vx =0;
c->move_vec.Vy =0;
c->move_vec.Vw =0;
break;
case NORMAL:
break;
case GYRO_STAY://陀螺仪yaw修正
c->move_vec.Vw = c->move_vec.Vw +c->pos088.bmi088.gyro.z *2000;
break;
}
//电机速度限幅
// abs_limit_fp(&c->move_vec.Vx,2000.0f);
// abs_limit_fp(&c->move_vec.Vy,2000.0f);
// abs_limit_fp(&c->move_vec.Vw,2000.0f);
Chassis_speed_calculate(c,c->move_vec.Vx,c->move_vec.Vy,c->move_vec.Vw);
for (uint8_t i = 0 ; i <4 ; i++)
{
c->final_out.final_3508out[i] = PID_calc(&(c->pid.chassis_3508VecPID[i]),c->motorfeedback.rotor_rpm3508[i],c->hopemotorout.OmniSpeedOut[i]);
out->motor3508.as_array[i] = c->final_out.final_3508out[i];
}
// c->vofa_send[0]=c->pos088.bmi088.gyro.x;
// c->vofa_send[1]=c->pos088.bmi088.gyro.y;
// c->vofa_send[2]=c->pos088.bmi088.gyro.z;
// c->vofa_send[3]=c->pos088.bmi088.accl.x;
// c->vofa_send[4]=c->pos088.bmi088.accl.y;
// c->vofa_send[5]=c->pos088.bmi088.accl.z;
return CHASSIS_OK;
}

232
User/Module/Chassis.h
View File

@ -1,232 +0,0 @@
#ifndef CHASSIS_H
/**
* @brief 底盘控制类头文件
*
* 本头文件定义了底盘控制类的接口和数据结构，用于控制车辆的底盘。
*
* @param ChassisImu_t 底盘的 IMU 数据结构体
* @param ops_t 操作数据结构体
* @param Chassis_t 底盘数据结构体
*
* @brief 底盘控制类的功能包括：
* 速度控制
* 方向控制
* PID 控制
* 速度和方向的综合控制
*
* @attention PID 控制器的配置需要在 config 文件中设置
*
*/
#define CHASSIS_H
#include "struct_typedef.h"
#include "pid.h"
#include "bmi088.h"
#include "map.h"
#include "user_math.h"
#include "ahrs.h"
#include "can_use.h"
#include "cmd.h"
#include "filter.h"
#include "Action.h"
#define CHASSIS_OK (0)
#define CHASSIS_ERR (-1)
#define CHASSIS_ERR_NULL (-2)
#define CHASSIS_ERR_MODE (-3) /*CMD_ChassisMode_t */
#define CHASSIS_ERR_TYPE (-4) /*Chassis_Type_t */
//m3508的电机转速转换为底盘的实际速度
#define M3508_MOTOR_RPM_TO_VECTOR 0.0008809748903494517209f
#define M6020_MOTOR_RPM_TO_VECTOR 0.003664f
#define PI 3.1415926535f
typedef struct {
BMI088_t bmi088;
/*可通过该枚举类型来决定Imu的数据量纲*/
enum {
IMU_DEGREE,//角度值（0-360）
IMU_RADIAN//弧度制（0-2pi)
}angle_mode;
AHRS_Eulr_t imu_eulr;//解算后存放欧拉角（弧度制）
}ChassisImu_t;
/*底盘的类型*/
typedef enum {
CHASSIS_TYPE_MECANUM, /* 麦轮 */
CHASSIS_TYPE_OMNI_CROSS, /* 全向轮*/
CHASSIS_TYPE_AGV, /* AGV舵轮 */
} Chassis_Type_e;
/*底盘的电机轮组*/
typedef enum {
DJI_M3508,
DJI_G6020,
AGV_Group,
}Chassis_Motortype_e;
/* 该结构体用于存取固定的一些参数 在config.c中更改后不再变化 */
typedef struct
{
Chassis_Type_e chassis_type; /* */
Chassis_Motortype_e motor_type; /**/
/*该部分决定PID的参数整定在config中修改*/
pid_param_t M3508_param;
pid_param_t AngleCor_param;
pid_param_t OmegaCor_param;
pid_param_t DisCamera_param;
pid_param_t ImuCor_param;
pid_param_t C6020pitAngle_param;
pid_param_t C6020pitOmega_param;
pid_param_t Gimbal_yawAngle_param;
pid_param_t Gimbal_yawOmega_param;
pid_param_t Gimbal_pitchAngle_param;
pid_param_t Gimbal_pitchOmega_param;
pid_param_t NaviVx_param;
pid_param_t NaviVy_param;
pid_param_t NaviVw_param;
pid_param_t Sick_CaliYparam;
pid_param_t Sick_CaliXparam;
}Chassis_Param_t;
/*该结构体用于底盘的期望运动向量*/
typedef struct
{
fp32 Vx;
fp32 Vy;
fp32 Vw;
fp32 mul;//油门倍率
}ChassisMove_Vec;
/**
* @brief
*
*/
typedef struct{
uint8_t chassis_task_run; //线程的运行
const Chassis_Param_t *param; //一些固定的参数
ChassisImu_t pos088; //088的实时姿态
Action_POS_t Action_pos;
CMD_Chassis_CtrlType_e ctrl;
CMD_Chassis_mode_e mode;
ChassisMove_Vec move_vec; //由控制任务决定
struct{
fp32 rotor_rpm3508[4];
fp32 rotor_current3508[4];
fp32 rotor_pit6020angle;
fp32 rotor_pit6020rpm;
fp32 rotor_gimbal_yawangle;
fp32 rotor_gimbal_yawrpm;
fp32 rotor_gimbal_pitchangle;
fp32 rotor_gimbal_pitchrpm;
}motorfeedback;
/*期望的底盘输出值*/
struct{
fp32 OmniSpeedOut[4];
}hopemotorout;
/*经PID计算后的实际发送给电机的实时输出值*/
struct
{
fp32 final_3508out[4];
fp32 final_pitchout;
fp32 final_gimbal_yawout;
fp32 final_gimbal_pitchout;
}final_out;
struct{
pid_type_def chassis_3508VecPID[4];
pid_type_def chassis_pitAngle6020;
pid_type_def chassis_pitOmega6020;
pid_type_def chassis_gimbal_yawAnglePID;
pid_type_def chassis_gimbal_yawOmegaPID;
pid_type_def chassis_gimbal_pitchAnglePID;
pid_type_def chassis_gimbal_pitchOmegaPID;
pid_type_def chassis_NaviVxPID;
pid_type_def chassis_NaviVyPID;
pid_type_def chassis_NaviWzPID;
pid_type_def sick_CaliforYPID;
pid_type_def sick_CaliforXPID;
pid_type_def Action_VxPID;
pid_type_def Action_VyPID;
pid_type_def Action_WzPID;
}pid;
fp32 vofa_send[8];
LowPassFilter2p_t filled[6]; /* 输出滤波器滤波器数组 */
int32_t sick_dis[4]; //获取到的sick激光值
}Chassis_t;
/**
* @brief
*
* @param c
* @param param
* @param mech_zero
* @param wheelPolar
* @return
*/
int8_t Chassis_init(Chassis_t *c,const Chassis_Param_t *param,float target_freq);
/**
* \brief
*/
int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can);
/**
* \brief
*/
int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out);
/// @brief
/// @param c
void vesc_current_detection(Chassis_t *c);
#endif

170
User/Module/config.c
View File

@ -7,18 +7,6 @@
#define DEG_TO_RAD(x) ((x) * (3.141592653 / 180.0)) //角度转弧度
//#ifdef DEBUG
//ConfigParam_t param_up ={
//#else
//static const ConfigParam_t param_up ={
//#endif
//
//
//
//
//};
#ifdef DEBUG
@ -32,6 +20,8 @@ static const ConfigParam_t param_chassis ={
#endif
.up={
/*上层pid参数*/
.M2006_angle_param = {
.p = 25.0f,
.i = 0.0f,
@ -46,6 +36,21 @@ static const ConfigParam_t param_chassis ={
.i_limit = 2000.0f,
.out_limit = 3000.0f,
},
.M3508_angle_param = {
.p = 10.0f,
.i = 0.5f,
.d = 0.0f,
.i_limit = 1000.0f,
.out_limit = 3000.0f,
},
.M3508_speed_param={
.p = 6.0f,
.i = 0.0f,
.d = 3.2f,
.i_limit = 200.0f,
.out_limit =6000.0f,
},
.UP_GM6020_angle_param={
.p = 30.0f,
.i = 20.0f,
@ -60,115 +65,52 @@ static const ConfigParam_t param_chassis ={
.i_limit = 200.0f,
.out_limit = 3000.0f
},
.M3508_speed_param={
.p = 15.1f,
.i = 0.02f,
.d = 3.2f,
.i_limit = 200.0f,
.out_limit =6000.0f,
},
.go_param={
.go_param[0]={
.rev = 0,
.T=0.1,
.T=0.79,
.W=0.1,
.K_P=0.1,
.K_P=0.2,
.K_W=0.1,
}
},
.go_param[1]={
.rev = 0,
.T=0,
.W=0,
.K_P=1.0f,
.K_W=0.1,
},
.go1_position_param={
.p =0.1f,
.i =0.1f,
.d =0.0f,
.i_limit = 2.0f,
.out_limit = 10.0f
},
/*上层其他参数*/
/*运球*/
.DribbleConfig_Config = {
.m3508_init_angle = 50,
.m3508_high_angle = 1200,
.go2_init_angle = 0,
.go2_flip_angle = -250,
.flip_timing = 200,
.go2_release_threshold = -550.0f,
},
/*投球*/
.PitchConfig_Config = {
.m2006_init_angle =-150,
.m2006_trigger_angle =0,
.go1_init_position = -500,
.go1_release_threshold =-1900,
.m2006_Screw_init=0
},
},
// .chassis = {/**/
// .C6020pitAngle_param = {
// .p = 15.0f,
// .i = 0.3f,
// .d =0.0f,
// .i_limit = 200.0f,
// .out_limit = 3000.0f,
// },
// .C6020pitOmega_param = {
// .p =30.0f,
// .i =0.3f,
// .d =0.0f,
// .i_limit = 200.0f,
// .out_limit = 3000.0f
// },
//
// .Gimbal_yawAngle_param = {
// .p =8.0f,
// .i =0.0f,
// .d =0.0f,
// .i_limit = 200.0f,
// .out_limit = 3000.0f
// },
//
// .Gimbal_yawOmega_param = {
// .p =18.0f,
// .i =0.0f,
// .d =0.0f,
// .i_limit = 200.0f,
// .out_limit = 3000.0f
// },
//
// .Gimbal_pitchAngle_param = {
// .p =8.0f,
// .i =0.0f,
// .d =0.0f,
// .i_limit = 200.0f,
// .out_limit = 3000.0f
// },
//
// .Gimbal_pitchOmega_param = {
// .p =18.0f,
// .i =0.0f,
// .d =0.0f,
// .i_limit = 200.0f,
// .out_limit = 3000.0f
// },
// .AngleCor_param = {
// .p =0.8f,
// .i =0.0f,
// .d =1.0f,
// .i_limit = 0.0f,
// .out_limit =5000.0f,
// },
//
// .OmegaCor_param = {
// .p =23.5f,
// .i =0.0f,
// .d =0.05f,
// .i_limit = 0.0f,
// .out_limit =5000.0f,
// },
//
// .ImuCor_param = {
// .p =95.0f,
// .i =0.0f,
// .d =0.0f,
// .i_limit = 0.0f,
// .out_limit =200.0f,
// },
//
// .DisCamera_param = {
// .p =80.0f,
// .i =0.1f,
// .d =0.0f,
// .i_limit = 0.0f,
// .out_limit =5000.0f,
// },
// .M3508_param = {
// .p = 15.1f,
// .i = 0.02f,
// .d = 3.2f,
// .i_limit = 200.0f,
// .out_limit =6000.0f,
// }
//
// },
.can = {
.pitch6020 = BSP_CAN_1,
.motor3508 = BSP_CAN_1,

2
User/Module/config.h
View File

@ -1,14 +1,12 @@
#ifndef _CONFIG_H
#define _CONFIG_H
#include "Chassis.h"
#include "can_use.h"
#include "ahrs.h"
#include "map.h"
#include "up.h"
typedef struct{
Chassis_Param_t chassis; /**/
UP_Param_t up;
CAN_Params_t can;
AHRS_Eulr_t mech_zero[4];

480
User/Module/up.c
View File

@ -4,102 +4,167 @@
#include "bsp_buzzer.h"
#include "bsp_delay.h"
#define GEAR_RATIO (36) // 2006减速比
#define CAN_MOTOR_ENC_RES 8191 // 编码器分辨率
#define MOTOR2006_ECD_TO_ANGLE (360.0 / 8191.0 / (GEAR_RATIO)) //2006编码值转轴角度
#define GEAR_RATIO_2006 (36) // 2006减速比
#define GEAR_RATIO_3508 (19)
#define CAN_MOTOR_ENC_RES 8191 // 编码器分辨率
#define MOTOR2006_ECD_TO_ANGLE (360.0 / 8191.0 / (GEAR_RATIO_2006)) //2006编码值转轴角度
#define MOTOR3508_ECD_TO_ANGLE (360.0 / 8191.0 / (GEAR_RATIO_3508)) //3508编码值转轴角度
// 定义继电器控制
#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)
int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
{
u->param = param; /*初始化参数 */
u->param = param; /*初始化参数 */
/*go电机初始化*/
GO_M8010_init();
/*pid初始化*/
PID_init (&u->pid.VESC_5065_M1 ,PID_POSITION ,&(u->param ->VESC_5065_M1_param ));
PID_init (&u->pid.VESC_5065_M2 ,PID_POSITION ,&(u->param ->VESC_5065_M2_param ));
PID_init (&u->pid .M2006_angle ,PID_POSITION ,&(u->param->M2006_angle_param ));
PID_init (&u->pid .M2006_speed ,PID_POSITION ,&(u->param->M2006_speed_param ));
PID_init (&u->pid .M3508_angle ,PID_POSITION ,&(u->param->M3508_angle_param ));
PID_init (&u->pid .M3508_speed ,PID_POSITION ,&(u->param->M3508_speed_param ));
PID_init (&u->pid .GM6020_speed,PID_POSITION ,&(u->param ->UP_GM6020_speed_param ));
PID_init (&u->pid .GM6020_angle,PID_POSITION ,&(u->param ->UP_GM6020_angle_param ));
u->M2006 .motor =M2006 ;
u->M3508 .motor =M3508 ;
PID_init (&u->pid .GO1_position ,PID_POSITION,&(u->param->go1_position_param ));
// for(int i=0;i<2;i++){ //go初始位置设置为0
// GO_M8010_send_data(BSP_UART_GetHandle(BSP_UART_RS485), i,u->param->go_param[i] .rev ,u->param->go_param[i] .T ,u->param->go_param[i] .W ,0,u->param->go_param [i].K_P ,u->param->go_param[i] .K_W );
// }
// 初始化上层状态机
if (!u->DribbleContext .is_initialized) { //检查是否为第一次运行状态机，运球
u->DribbleContext .DribbleConfig = u->param ->DribbleConfig_Config ;//赋值
u->DribbleContext .DribbleState = Dribble_PREPARE;
u->DribbleContext .is_initialized = 1;
}
if (!u->PitchContext .is_initialized) {
u->PitchContext .PitchConfig = u->param ->PitchConfig_Config ;//赋值
u->PitchContext .PitchState = PITCH_PREPARE; //状态更新,开始夹球
u->PitchContext .is_initialized = 1;
}
for(int i=0;i<3;i++){
PID_init (&u->pid .M3508_speed[i] ,PID_POSITION ,&(u->param ->M3508_speed_param ));
}
for(int i=0;i<2;i++){ //go初始位置设置为0
u->GO_motor_info[i] = getGoPoint(i);
GO_M8010_send_data(BSP_UART_GetHandle(BSP_UART_RS485), i,u->param->go_param .rev ,u->param->go_param .T ,u->param->go_param .W ,0,u->param->go_param .K_P ,u->param->go_param .K_W );
}
}
/*can，上层状态更新*/
int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *c) {
for(int i=0;i<2;i++){ //go初始位置设置为0
u->motorfeedback .GO_motor_info [i] = getGoPoint(i);
}
int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can) {
u->motorfeedback .M2006_angle=can ->motor .motor3508 .as_array [0].rotor_ecd ;
u->motorfeedback .M2006_rpm =can ->motor .motor3508 . as_array [0].rotor_speed ;
u->motorfeedback .M2006 .motor =M2006;
u->motorfeedback .M3508 .motor =M3508;
u->motorfeedback .VESC_5065_M1_rpm =can ->motor .chassis5065 .as_array [0].rotor_speed ;
u->motorfeedback .VESC_5065_M2_rpm =can ->motor .chassis5065 .as_array [1].rotor_speed ;
u->motorfeedback .rotor_pit6020ecd =can ->motor .chassis6020.as_array [2].rotor_ecd ;
u->motorfeedback .rotor_pit6020rpm =can ->motor .chassis6020.as_array [2].rotor_speed ;
for(int i=0;i<3;i++){
u->motorfeedback .M3508_speed[i] =can ->motor .motor3508 .as_array [i+1].rotor_speed ;
for(int i=0;i<4;i++){
u->motorfeedback .M3508_rpm[i] =can ->motor .motor3508 .as_array [i].rotor_speed ;
u->motorfeedback .M3508_angle [i]=can ->motor .motor3508 .as_array [i].rotor_ecd ;
}
u->cmd =c;
return 0;
}
int8_t cnt=0;
/*上层电机控制*/
int8_t UP_angle(UP_t *u, fp32 target_angle) {
/*上层大疆电机角度控制,使用can1的id1和2*/
int8_t UP_angle_control(UP_t *u, fp32 target_angle,MotorType_t motor) {
// 获取当前编码器角度
int8_t cnt=0;
fp32 angle ,delta;
// switch (u->)
float angle = u->motorfeedback.M2006_angle;
// 初始化阶段：前50次循环记录初始值
if (u->M2006.init_cnt < 50) {
u->M2006.orig_angle = angle; // 记录初始编码器值
u->M2006.last_angle = angle;
switch(motor)
{
case M2006 :
angle = u->motorfeedback.M3508_angle[0];
if (u->motorfeedback .M2006 .init_cnt < 50) {
u->motorfeedback .M2006.orig_angle = angle; // 记录初始编码器值
u->motorfeedback .M2006.last_angle = angle;
u->M2006.init_cnt++; // 初始化计数器递增
u->motorfeedback .M2006.init_cnt++; // 初始化计数器递增
return 0;
}
float delta = angle - u->M2006.last_angle;
delta = angle - u->motorfeedback .M2006.last_angle;
delta = angle - u->motorfeedback .M2006.last_angle;
if (delta > 4096) {
u->M2006.round_cnt--; // 逆时针跨圈
u->motorfeedback .M2006.round_cnt--; // 逆时针跨圈
} else if (delta < -4096) {
u->M2006.round_cnt++; // 顺时针跨圈
u->motorfeedback .M2006.round_cnt++; // 顺时针跨圈
}
u->M2006.last_angle = angle;
// 计算总角度
float total_angle = (u->M2006.round_cnt * 8191 + (angle - u->M2006.orig_angle)) * MOTOR2006_ECD_TO_ANGLE;
u->M2006 .total_angle =total_angle;
u->motorfeedback.M2006.last_angle = angle;
// 计算总角度
float total_angle = (u->motorfeedback.M2006 .round_cnt * 8191 + (angle - u->motorfeedback.M2006.orig_angle)) * MOTOR2006_ECD_TO_ANGLE;
u->motorfeedback.M2006.total_angle =total_angle;
float delta_angle = PID_calc(&u->pid.M2006_angle, total_angle, target_angle);
float delta_speed = PID_calc(&u->pid.M2006_speed, u->motorfeedback.M2006_rpm, delta_angle);
float delta_speed = PID_calc(&u->pid.M2006_speed, u->motorfeedback.M3508_rpm [0], delta_angle);
u->motor_target.M2006_angle = target_angle;
u->final_out .final_2006out =delta_speed;
u->final_out .final_3508out [0] =delta_speed;
break ;
case M3508 :
angle = u->motorfeedback.M3508_angle[1];
if (u->motorfeedback .M3508 .init_cnt < 50) {
u->motorfeedback .M3508.orig_angle = angle; // 记录初始编码器值
u->motorfeedback .M3508.last_angle = angle;
u->motorfeedback .M3508.init_cnt++; // 初始化计数器递增
return 0;
}
delta = angle - u->motorfeedback .M3508.last_angle;
delta = angle - u->motorfeedback .M3508.last_angle;
if (delta > 4096) {
u->motorfeedback .M3508.round_cnt--; // 逆时针跨圈
} else if (delta < -4096) {
u->motorfeedback .M3508.round_cnt++; // 顺时针跨圈
}
u->motorfeedback.M3508.last_angle = angle;
// 计算总角度
total_angle = (u->motorfeedback.M3508 .round_cnt * 8191 + (angle - u->motorfeedback.M3508.orig_angle)) * MOTOR2006_ECD_TO_ANGLE;
u->motorfeedback.M3508.total_angle =total_angle;
delta_angle = PID_calc(&u->pid.M3508_angle , total_angle, target_angle);
delta_speed = PID_calc(&u->pid.M3508_speed , u->motorfeedback.M3508_rpm [1], delta_angle);
u->motor_target.M3508_angle = target_angle;
u->final_out .final_3508out[1] =delta_speed;
break ;
}
return 0;
}
int8_t UP_M3508_speed(UP_t *u,fp32 speed)
{
u->motor_target .M3508_speed =speed;
for(int i=0;i<3;i++){
u->final_out .final_3508out [i] =
PID_calc (&(u->pid .M3508_speed[i] ),u->motorfeedback .M3508_speed [i],speed );
}
}
//int8_t UP_M3508_speed(UP_t *u,fp32 speed)
//{
// u->motor_target .M3508_speed [] =speed;
// for(int i=0;i<3;i++){
// u->final_out .final_3508out [i] =
// PID_calc (&(u->pid .M3508_speed[i] ),u->motorfeedback .M3508_speed [i],speed );
// }
//
//}
int8_t VESC_M5065_Control(UP_t *u,fp32 speed)
@ -107,61 +172,334 @@ int8_t VESC_M5065_Control(UP_t *u,fp32 speed)
u->motor_target .VESC_5065_M1_rpm =speed;
u->motor_target .VESC_5065_M2_rpm =speed;
u->final_out .final_VESC_5065_M1out =u->motor_target .VESC_5065_M1_rpm;
u->final_out .final_VESC_5065_M2out =-u->motor_target .VESC_5065_M2_rpm;
u->final_out .final_VESC_5065_M1out =-u->motor_target .VESC_5065_M1_rpm;
u->final_out .final_VESC_5065_M2out =u->motor_target .VESC_5065_M2_rpm;
return 0;
}
int8_t GM6020_control(UP_t *u,fp32 angle)
{
fp32 delat_speed;
Clip(&angle,90,270);
// Clip(&angle,90,270);
delat_speed =
PID_calc (&(u->pid .GM6020_angle ),u->motorfeedback .rotor_pit6020ecd ,(angle /360*8191));
u->final_out .final_pitchout =
PID_calc (&(u->pid .GM6020_speed ),u->motorfeedback.rotor_pit6020rpm ,delat_speed);
u->motor_target .rotor_pit6020angle =angle ;
return 0;
}
/*go电机控制*/
int8_t GO_SendData(UP_t *u,int id,float pos)
{
fp32 deal_pos;
GO_M8010_send_data(BSP_UART_GetHandle(BSP_UART_RS485), id,u->param->go_param .rev ,u->param->go_param .T ,u->param->go_param .W ,AngleChange(RADIAN,pos),u->param->go_param .K_P ,u->param->go_param .K_W );
// deal_pos = PID_calc (&u->pid .GO1_position ,u->motorfeedback .GO_motor_info [0]->Pos ,pos);
GO_M8010_send_data(BSP_UART_GetHandle(BSP_UART_RS485), id,u->param->go_param[id] .rev ,u->param->go_param[id].T ,u->param->go_param[id] .W ,pos,u->param->go_param [id].K_P ,u->param->go_param[id] .K_W );
return 0;
}
int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
{
//电机控制 ，传进can
out ->motor3508 .as_array [0]=u->final_out .final_2006out ;
for(int i=1;i<4;i++){
out ->motor3508 .as_array[i]=u->final_out.final_3508out [i-1] ;
UP_angle_control(u,u->motor_target .M2006_angle ,M2006);
UP_angle_control(u,u->motor_target .M3508_angle ,M3508 );
GO_SendData(u,0 ,u->motor_target .go_shoot );
for(int i=0;i<4;i++){
out ->motor3508 .as_array[i]=u->final_out.final_3508out [i] ;
}
out ->chassis5065 .erpm [0]=u->final_out .final_VESC_5065_M1out ;
out ->chassis5065 .erpm [1]=u->final_out .final_VESC_5065_M2out ;
out ->chassis5065 .erpm [0]= u->final_out .final_VESC_5065_M1out ;
out ->chassis5065 .erpm [1]= -u->final_out .final_VESC_5065_M2out ;
out ->chassis6020 .as_array [2]=u->final_out .final_pitchout ;
return 0;
}
int8_t UP_control(UP_t *u,CAN_Output_t *out)
int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
{
// if(u ==NULL) return 0;
// if(u ==NULL) return 0;
// UP_M2006_angle(u,2500);
switch (u->ctrl)
if(u ==NULL) return 0;
static int is_pitch=1;
switch (c->CMD_CtrlType )
{
case STATE_IDLE : break ;
case STATE_PRE_DRIBBLE : break ;
case STATE_POST_DRIBBLE : break ;
case STATE_PRE_LAUNCH : break ;
case STATE_POST_LAUNCH : break ;
case UP_RCcontrol: //在手动模式下
switch (c-> CMD_UP_mode )
{
case Normal :
/*投篮*/
if(is_pitch){
u->motor_target .go_shoot =u->PitchContext .PitchConfig .go1_init_position ;
is_pitch=0;
}
u->motor_target .M2006_angle =u->PitchContext .PitchConfig .m2006_init_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 (1);//夹球，0夹1开
// RELAY2_TOGGLE (0);//球，0接1收
//
break;
case Pitch :
if (u->PitchContext .PitchState ==PITCH_PREPARE) //首次启动
{
u->PitchContext .PitchState=PITCH_START;//置标志位用于启动投篮
}
Pitch_Process(u,out,c);
break ;
case Dribble:
{
/*夹球 -> 3508 升起 ，同时go2翻转 -> 到位置后，继电器开，放球，同时3508降，go2翻回->接球，收 */
if(u->DribbleContext.DribbleState== Dribble_PREPARE){
u->DribbleContext .DribbleState =STATE_GRAB_BALL;
}
Dribble_Process(u,out);
}break ;
case Dribbl_transfer:
Dribbl_transfer_a(u,out);
}
break;
default:
break;
}
return 0;
}
int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c)
{
static fp32 go1_position,M2006_Screw_position ;
static int is_initialize=1;
if(is_initialize)
{
go1_position=u->PitchContext .PitchConfig .go1_init_position ;
M2006_Screw_position=u->PitchContext .PitchConfig .m2006_Screw_init;
is_initialize=0;
}
switch(u->PitchContext .PitchState){
case PITCH_START:
u->motor_target .go_shoot = u->PitchContext .PitchConfig .go1_release_threshold;
if(u->motorfeedback .GO_motor_info[0]->Pos < -32.40){ //检测go位置到达最上面，这里的检测条件可以更改
u->motor_target .M2006_angle = u->PitchContext .PitchConfig .m2006_trigger_angle ;//设置2006角度，0
u->PitchContext .PitchState=PITCH_PULL_TRIGGER;
}//更改标志位
break ;
case PITCH_PULL_TRIGGER:
if(u->motorfeedback .M2006 .total_angle >-1) //当2006的总角度小于1，可以认为已经勾上,误差为1
{
u->motor_target .go_shoot = go1_position;//go下拉
u->motor_target .M3508_angle = M2006_Screw_position;//丝杠上的2006
go1_position = go1_position + c->Vx ;
M2006_Screw_position=M2006_Screw_position+ c->Vy;
}
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;
}
}

171
User/Module/up.h
View File

@ -5,25 +5,104 @@
#include "struct_typedef.h"
#include "pid.h"
#include "bmi088.h"
#include "map.h"
#include "user_math.h"
#include "ahrs.h"
#include "can_use.h"
#include "cmd.h"
#include "filter.h"
#include "Action.h"
#include "chassis.h"
#include "vofa.h"
#include "GO_M8010_6_Driver.h"
#include "bsp_usart.h"
/*
状态机
状态结构体，触发标志位结构体
配置层，-->config.c
|
中间层，-->up.h,统一UP_t
|
运行函数，switch(状态) 运行相应函数
*/
/* 投球状态定义 */
typedef enum {
STATE_IDLE, // 空闲状态
STATE_PRE_DRIBBLE, // 运球前
STATE_POST_DRIBBLE, // 运球后
STATE_PRE_LAUNCH, // 发射前
STATE_POST_LAUNCH // 发射后
} OperationState;
PITCH_PREPARE, // 准备阶段
PITCH_START, //启动，拉扳机
PITCH_PULL_TRIGGER,
PITCH_LAUNCHING, // 发射中
PITCH_COMPLETE // 完成
} PitchState_t;
/* 投球参数配置 */
typedef struct {
fp32 m2006_init_angle; // M2006初始角度-140
fp32 m2006_trigger_angle; // M2006触发角度0,用来合并扳机
fp32 go1_init_position; // GO电机触发位置,0,初始位置
fp32 go1_release_threshold; // go上升去合并扳机，扳机位置
fp32 m2006_Screw_init;
} PitchConfig_t;
/* 投球控制上下文 */
typedef struct {
PitchState_t PitchState;
PitchConfig_t PitchConfig;
uint8_t is_initialized ;
} PitchContext_t;
/*运球*/
typedef enum {
Dribble_PREPARE,
STATE_GRAB_BALL, // 夹球升起阶段
STATE_RELEASE_BALL, // 释放球体
STATE_CATCH_PREP, // 接球准备
STATE_CATCH_BALL, // 接球动作
STATE_TRANSFER, //转移球
STATE_CATCH_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
fp32 go2_release_threshold; // 释放球
} DribbleConfig_t;
/* 状态机上下文 */
typedef struct {
DribbleState_t DribbleState;
DribbleConfig_t DribbleConfig;
uint8_t is_initialized;
} DribbleContext_t;
typedef struct {
@ -52,7 +131,7 @@ typedef struct {
float K_W;
}GO_param_t;
/*角度环控制电机类型*/
typedef enum {
M2006 = 1,
M3508
@ -62,7 +141,7 @@ typedef enum {
typedef struct
{
/*该部分决定PID的参数整定在config中修改*/
pid_param_t VESC_5065_M1_param;
pid_param_t VESC_5065_M2_param;
@ -73,13 +152,19 @@ typedef struct
pid_param_t M2006_angle_param;
pid_param_t M3508_speed_param;
GO_param_t go_param;
pid_param_t M3508_angle_param;
GO_param_t go_param[2];
pid_param_t go1_position_param;
DribbleConfig_t DribbleConfig_Config;
PitchConfig_t PitchConfig_Config;
}UP_Param_t;
typedef struct
{
MotorType_t motor;
float orig_angle;
float last_angle;
int32_t round_cnt;
@ -93,25 +178,39 @@ typedef struct{
uint8_t up_task_run;
const UP_Param_t *param;
int state;
/*运球过程*/
DribbleContext_t DribbleContext;
/*投篮过程*/
PitchContext_t PitchContext;
UP_Imu_t pos088;
OperationState ctrl;
motor_angle_data M2006;
motor_angle_data M3508;
GO_Motorfield *GO_motor_info[GO_NUM];//存放go电机数据
CMD_t *cmd;
struct{
fp32 rotor_pit6020ecd;
fp32 rotor_pit6020rpm;
fp32 VESC_5065_M1_rpm;
fp32 VESC_5065_M2_rpm;
fp32 M2006_rpm;
fp32 M2006_angle;
motor_angle_data M2006;
motor_angle_data M3508;
fp32 M3508_speed[3];
GO_Motorfield *GO_motor_info[GO_NUM];//存放go电机数据
fp32 M3508_angle[4];
fp32 M3508_rpm [4];
int flag;
}motorfeedback;
@ -125,7 +224,10 @@ typedef struct{
fp32 VESC_5065_M2_rpm;
fp32 M2006_angle;
fp32 M3508_speed;
fp32 M3508_angle;
fp32 go_shoot;
fp32 go_spin;
}motor_target;
@ -133,33 +235,37 @@ typedef struct{
pid_type_def GM6020_speed;
pid_type_def GM6020_angle;
pid_type_def VESC_5065_M1;
pid_type_def VESC_5065_M2;
pid_type_def M2006_angle;
pid_type_def M2006_speed;
pid_type_def M3508_speed[3];
pid_type_def M3508_angle;
pid_type_def M3508_speed;
pid_type_def GO1_position;
}pid;
/*经PID计算后的实际发送给电机的实时输出值*/
struct
{
fp32 final_2006out;
fp32 final_3508out[3];
fp32 final_3508out[4];
fp32 final_pitchout;
fp32 final_VESC_5065_M1out;
fp32 final_VESC_5065_M2out;
}final_out;
LowPassFilter2p_t filled[6]; /* 输出滤波器滤波器数组 */
fp32 vofa_send[8];
fp32 vofa_send[8];
} UP_t;
@ -167,16 +273,19 @@ typedef struct{
int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq);
int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can) ;
int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *c) ;
int8_t GM6020_control(UP_t *u,fp32 angle);
int8_t VESC_M5065_Control(UP_t *u,fp32 speed);
int8_t GO_SendData(UP_t *u,int id,float pos);
int8_t UP_angle(UP_t *u, fp32 target_angle) ;
int8_t UP_control(UP_t *u,CAN_Output_t *out);
int8_t UP_angle_control(UP_t *u, fp32 target_angle,MotorType_t motor);
int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c);
int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out);
int8_t UP_M2006_angle(UP_t *u,fp32 target_angle);
int8_t UP_M3508_speed(UP_t *u,fp32 speed);
int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c);
int8_t Dribble_Process(UP_t *u, CAN_Output_t *out);
void Dribbl_transfer_a(UP_t *u, CAN_Output_t *out);
#endif

203
User/device/Action.c
View File

@ -1,203 +0,0 @@
/**
* 东北大学ACTION码盘驱动
*
* 本驱动采用ops_9定位系统，负责收发码盘的原始数据，并在任务中解析出相应的位置坐标。
* 解析后的位置坐标将被交给导航处理模块，生成期望的运动向量。
*
* 使用手册：
* 请参阅相关文档了解如何使用本驱动。
*
* 注意：
* 本驱动仅适用于东北大学ACTION码盘。
*删去Action_HandleOffline错误处理函数中对结构体清0的函数
*/
/* Includes ----------------------------------------------------------------- */
#include "tim.h"
#include "Action.h"
#include <string.h>
static osThreadId_t thread_alert;
uint8_t rxbuf[RS232_FRAME_LENGTH];
static void Ops10msTimerCallback(void *arg){
(void)arg;
osThreadFlagsSet(thread_alert,SIGNAL_OPSTIMER_REDY);
}
/* Private function -------------------------------------------------------- */
static void ACTION_IdleCallback(void) {
osThreadFlagsSet(thread_alert,SIGNAL_ACTION_RAW_REDY);
}
/* Exported functions ------------------------------------------------------- */
int8_t ACTIONRECV_Init(Action_POS_t *pos){
if(pos == NULL) return DEVICE_ERR_NULL;
pos->Action_ready =0;//码盘校准标志位初始化
if((thread_alert = osThreadGetId()) == NULL ) return DEVICE_ERR_NULL;
pos->action_timer_Id =
osTimerNew(Ops10msTimerCallback, osTimerPeriodic,NULL,NULL);
osTimerStart(pos->action_timer_Id,10);//此处ticks 理解为1个tick为1ms 此处为每10ms触发一次回调函数
BSP_UART_RegisterCallback(BSP_UART_ACTION,BSP_UART_IDLE_LINE_CB,
ACTION_IdleCallback);
return DEVICE_OK;
}
int8_t ACTION_StartReceiving() {
if (HAL_UARTEx_ReceiveToIdle_DMA(BSP_UART_GetHandle(BSP_UART_ACTION),
(uint8_t *)rxbuf,
sizeof(rxbuf)) == HAL_OK)
return DEVICE_OK;
return DEVICE_ERR;
}
bool_t ACTION_WaitDmaCplt(void) {
return(osThreadFlagsWait(SIGNAL_ACTION_RAW_REDY, osFlagsWaitAll,400) ==
SIGNAL_ACTION_RAW_REDY);
}
/* *
*/
int8_t ACTION_Parse(Action_POS_t *pos)
{
if (pos == NULL) return DEVICE_ERR_NULL;
static union
{
fp32 pos_data[3];
char rxbuff[12];
} pos_rxbuf;
if (rxbuf[0] == 0x0D && rxbuf[1] == 0x0A)
{
if (rxbuf[26] == 0x0A && rxbuf[27] == 0x0D)
{
pos_rxbuf.rxbuff[0]=rxbuf[2];
pos_rxbuf.rxbuff[1]=rxbuf[3];
pos_rxbuf.rxbuff[2]=rxbuf[4];
pos_rxbuf.rxbuff[3]=rxbuf[5];
pos_rxbuf.rxbuff[4]=rxbuf[14];
pos_rxbuf.rxbuff[5]=rxbuf[15];
pos_rxbuf.rxbuff[6]=rxbuf[16];
pos_rxbuf.rxbuff[7]=rxbuf[17];
pos_rxbuf.rxbuff[8]=rxbuf[18];
pos_rxbuf.rxbuff[9]=rxbuf[19];
pos_rxbuf.rxbuff[10]=rxbuf[20];
pos_rxbuf.rxbuff[11]=rxbuf[21];
// 数据解析
pos->pos_yaw = pos_rxbuf.pos_data[0];
// 按照安装方向决定正负号
pos->pos_x = pos_rxbuf.pos_data[1];
pos->pos_y = pos_rxbuf.pos_data[2];
pos ->Action_ready =1;//码盘校准完成
}
}
else
{
return -1;
}
return DEVICE_OK;
}
//该函数用来计算速度(利用获取的位置来计算相应的速度)
//此处获取的是真实的位置(mm)后每10ms做一次微分处理
int8_t ACTION_DataRefresh(Action_POS_t *pos){
if (pos == NULL) return DEVICE_ERR_NULL;
if (osThreadFlagsGet() & SIGNAL_OPSTIMER_REDY){
osThreadFlagsClear(SIGNAL_OPSTIMER_REDY);
//计算每毫秒的速度 该函数每10ms调用
pos->pos_Vx = (pos->pos_x - pos->pos_lastX) / 10;
pos->pos_Vy = (pos->pos_y - pos->pos_lastY) / 10;
pos->pos_lastX = pos->pos_x;
pos->pos_lastY = pos->pos_y;
}
return DEVICE_OK;
}
int8_t Action_HandleOffline(Action_POS_t *pos) {
if (pos == NULL) return DEVICE_ERR_NULL;
(void)pos;
// memset(pos, 0, sizeof(*pos));
return 0;
}
/* 将字符串拼接 */
void Strcat(char str1[], char str2[], uint8_t num)
{
int i = 0, j = 0;
while (str1[i] != '\0')
i++;
for (j = 0; j < num; j++)
{
str1[i++] = str2[j];
}
}
/* 该部分函数用于码盘的重定位系统 */
/* ----------------------------- */
/* 手动标定，用于校正 */
void ACT_Calibration(void)
{
HAL_UART_Transmit(&huart1, (uint8_t *)"ACTR", 4, 100);
}
/* 清零 */
/* 将当前位置设置为 (0, 0) */
void ACT_ZeroClear(void)
{
HAL_UART_Transmit(&huart1, (uint8_t *)"ACT0", 4, 100);
}
/* 更新 XY 坐标 */
/* 将当前位置更新为指定的 X 和 Y */
void ACT_UpdateXY(float pos_x, float pos_y)
{
char update_xy[12] = "ACTD"; // 命令前缀为 "ACTD"
static union
{
float XY[2]; // 两个浮点数表示 X 和 Y 坐标
char data[8]; // 将浮点数视为字节数据
} set;
set.XY[0] = pos_x;
set.XY[1] = pos_y;
Strcat(update_xy, set.data, 8);
HAL_UART_Transmit(&huart1, (uint8_t *)update_xy, sizeof(update_xy), 100);
}

59
User/device/Action.h
View File

@ -1,59 +0,0 @@
#ifndef _ACTION_H_
#define _ACTION_H_
#define RS232_FRAME_LENGTH 28u
#define REF_LEN_RX_BUFF 0xFF
#include <cmsis_os2.h>
#include "device.h"
#include "bsp_usart.h"
#include "cmd.h"
typedef struct
{
osTimerId_t action_timer_Id;
/*原始数据*/
fp32 pos_x;
fp32 pos_y;
fp32 pos_yaw; // 航向角速度
/*处理数据*/
fp32 pos_Vx;
fp32 pos_Vy;
fp32 pos_lastX;
fp32 pos_lastY;
int Action_ready;//码盘校准标志位
}Action_POS_t;
int8_t ACTIONRECV_Init(Action_POS_t*pos);
int8_t ACTION_StartReceiving(void);
bool_t ACTION_WaitDmaCplt(void);
int8_t ACTION_Parse(Action_POS_t*pos);
int8_t ACTION_DataRefresh(Action_POS_t*pos);
int8_t Action_HandleOffline(Action_POS_t *pos);
void ACT_UpdateXY(float pos_x,float pos_y);
void ACT_ZeroClear(void);
#endif

11
User/device/GO_M8010_6_Driver.c
View File

@ -18,7 +18,7 @@ void GO_M8010_init (void){
for (uint8_t id= 0; id <GO_NUM ;id++)
{
GO_motor_info[id].id = id;
GO_motor_info[id].mode = 1; //位置模式
GO_motor_info[id].mode = 1; //foc闭环
GO_motor_info[id].correct = 1;
GO_motor_info[id].MError = 0;
GO_motor_info[id].Temp = 0;
@ -158,9 +158,12 @@ void GO_M8010_recv_data(uint8_t* Temp_buffer)
int16_t W = Temp_buffer[6]<<8 | Temp_buffer[5];
motor->W = W;
motor->W = (motor->W * 6.28319f )/(256*6.33f);
int32_t Pos = Temp_buffer[10]<<24 | Temp_buffer[9]<<16 | Temp_buffer[8]<<8 | Temp_buffer[7];
motor->Pos = Pos;
motor->Pos = (motor->Pos * 6.28319f )/(32768*6.33f);
int32_t Pos = (Temp_buffer[7] << 0) |
(Temp_buffer[8] << 8) |
(Temp_buffer[9] << 16) |
(Temp_buffer[10] << 24);
if(Pos > 0x7FFFFFFF) Pos -= 0xFFFFFFFF; // 溢出修正
motor->Pos = ((float)Pos / 32768.0f) * (6.28319f / 6.33f);
int8_t Temp = Temp_buffer[11] & 0xFF;
motor->Temp = Temp;
motor->MError = Temp_buffer[12] & 0x7;

4
User/device/GO_M8010_6_Driver.h
View File

@ -8,6 +8,7 @@ extern "C"{
#include "usart.h"
#include "string.h"
#include <math.h>
#include "user_math.h"
#define GO_NUM 2
/**
* @brief
@ -63,10 +64,13 @@ typedef struct {
float T;
float W;
float Pos;
float angle;
int footForce;
uint8_t buffer[17];
uint8_t Rec_buffer[16];
ControlData_t motor_send_data;
}GO_Motorfield;

203
User/device/cmd.c
View File

@ -13,74 +13,27 @@
/* Private function -------------------------------------------------------- */
/*Export function --------------------------------------------------------------*/
int8_t CMD_Init(CMD_t *cmd){
/*若主结构体为空 自动返回错误 */
if(cmd == NULL) return-1;
/**/
cmd->C_cmd.type =RC;
cmd->C_cmd.mode =NORMAL;
return 0;
}
static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd) {
/* 将操纵杆的对应值转换为底盘的控制向量和云台变化的欧拉角 */
cmd->Vx = rc->ch_r_x;
cmd->Vy = -rc->ch_r_y;
cmd->Vw = rc->ch_l_x;
cmd->poscamear = rc->ch_l_y;
cmd->key_ctrl_l = rc->sw_l;
cmd->key_ctrl_r = rc->sw_r ;
}
/**
* @brief rc失控时机器人恢复放松模式
*
* @param cmd 主结构体
*/
static void CMD_RcLostLogic(CMD_t *cmd){
/* 机器人底盘运行模式恢复至放松模式 */
cmd->C_cmd.mode = RELAXED;
}
int8_t CMD_ParseRc(CMD_t *cmd,CMD_RC_t *rc){
if (cmd == NULL) return -1;
if (rc == NULL) return -1;
/*c当rc丢控时，恢复机器人至默认状态 */
if ((rc->sw_l == CMD_SW_ERR) || (rc->sw_r == CMD_SW_ERR)) {
CMD_RcLostLogic(cmd);
} else {
CMD_RcLogic(rc, cmd);
}
return 0;
}
/*nuc数据统一到cmd*/
int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n){
if(cmd == NULL) return -1;
if(n == NULL) return -1;
cmd->cmd_status = n->status_fromnuc;
cmd->raw_status = n->ctrl_status;
cmd->NAVI_t .cmd_status = n->status_fromnuc;
cmd->NAVI_t .raw_status = n->ctrl_status;
for (int i = 0; i < 7; ++i)
{ // 从最低位到最高位遍历
cmd->status[i] = ((cmd->raw_status) & (1 << i)) ? 1 : 0;
cmd->NAVI_t .status[i] = ((cmd->NAVI_t .raw_status) & (1 << i)) ? 1 : 0;
}
switch(cmd->cmd_status){
switch(cmd->NAVI_t .cmd_status){
case PICK :
cmd ->NAVI_t .pick .posx =n->pick .posx ;
cmd ->NAVI_t .pick .posy =n->pick .posy ;
cmd ->NAVI_t .pick .posw =n->pick .posw ;
case MID:
cmd->C_navi.vx = n->navi.vx;
cmd->C_navi.vy = n->navi.vy;
cmd->C_navi.wz = n->navi.wz;
break;
@ -88,108 +41,64 @@ int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n){
return 0;
}
/*遥控器，上下层通用，按键控制，统一到cmd*/
int8_t CMD_ParseRC(CMD_t *cmd,const CMD_RC_t *rc) {
/*
遥控器模式重新分配
这一部分没有设置具体的模式名，后期根据需要修改
遥控器模式(RC):
左按键 --- 右按键
mode1
up no_mode
mode2
mode3
down no_mode
mode4
mid auto_navi(0x09)雷达导航
*/
int8_t CMD_CtrlSet(CMD_t *cmd) {
if(cmd == NULL) return -1;
if(cmd->key_ctrl_l == CMD_SW_UP)//当左拨杆打到最上面时 强制使用遥控器控制
{
/*遥控器模式下，右按键三种状态分配*/
const CMD_CtrlType_t prev_CtrlType = cmd->CMD_CtrlType ;
const CMD_UP_mode_t prev_mode = cmd->CMD_UP_mode ;//保存旧状态
if(cmd->key_ctrl_r==CMD_SW_UP)
{
cmd->C_cmd.type = RC;
cmd->C_cmd.mode = RC_MODE1;
}
if(cmd->key_ctrl_r==CMD_SW_MID)
{
cmd->C_cmd.type = RC;
cmd->C_cmd.mode = RC_NO_MODE;
}
if(cmd->key_ctrl_r==CMD_SW_DOWN)//遥控器控制模式下，右拨杆上时启动
{
cmd->C_cmd.type = RC;
cmd->C_cmd.mode =RC_MODE2;
}
cmd->Vx = rc->ch_r_x;
cmd->Vy = rc->ch_r_y;
cmd->Vw = rc->ch_l_x;
}
else if(cmd->key_ctrl_l ==CMD_SW_DOWN)
{
if(cmd->key_ctrl_r==CMD_SW_UP)
{
cmd->C_cmd.type = RC;
cmd->C_cmd.mode = RC_MODE3;
}
if(cmd->key_ctrl_r==CMD_SW_MID)
{
cmd->C_cmd.type = RC;
cmd->C_cmd.mode = RC_NO_MODE;
}
if(cmd->key_ctrl_r==CMD_SW_DOWN)//遥控器控制模式下，右拨杆上时启动
{
cmd->C_cmd.type = RC;
cmd->C_cmd.mode = RC_MODE4;
}
cmd->poscamear = rc->ch_l_y;
}
else //左按键打到中间，自动模式
{
if( cmd->key_ctrl_l==CMD_SW_MID ){
cmd->C_cmd.type = MID_NAVI;
cmd->C_cmd.mode = RC_NO_MODE;
cmd->key_ctrl_l = rc->sw_l;
cmd->key_ctrl_r = rc->sw_r ;
switch(cmd->cmd_status)
{
case MID://雷达，视觉导航
cmd->C_cmd.type = MID_NAVI;
break;
if ((rc->sw_l == CMD_SW_ERR) || (rc->sw_r == CMD_SW_ERR)) {
cmd->CMD_CtrlType =RELAXED;
}
}
}
return 0;
}
//接收码盘导航的返回数据，传入cmd_t结构体
int8_t CMD_ParseAction(CMD_t *cmd,CMD_ACTION_t *act)
{
if(cmd == NULL) return -1;
if(act == NULL) return -1;
if(cmd->key_ctrl_l ==CMD_SW_MID )
else if(rc->sw_l==CMD_SW_UP)
{
if(cmd->key_ctrl_r == CMD_SW_UP)
act->flag =1;
if(cmd->key_ctrl_r == CMD_SW_DOWN )
act->flag =-1;
cmd ->CMD_CtrlType =UP_RCcontrol;
if(rc->sw_r ==CMD_SW_UP) cmd ->CMD_UP_mode =Pitch; //左上，右上，投篮，设置好的
if(rc->sw_r ==CMD_SW_MID) cmd ->CMD_UP_mode =Normal; //左上，右中，无模式
if(rc->sw_r ==CMD_SW_DOWN) cmd ->CMD_UP_mode =Pitch_HAND; //左上，右上，手动调整
}
cmd->C_navi .vx =act ->out .Vx ;
cmd ->C_navi .vy =act ->out .Vy ;
cmd ->C_navi .wz =act ->out .Vw ;
else if(rc->sw_l==CMD_SW_MID)
{
if(rc->sw_r ==CMD_SW_UP) cmd ->CMD_CtrlType =MID_NAVI;; //左中，右上，雷达
if(rc->sw_r ==CMD_SW_MID)
{
cmd ->CMD_CtrlType =UP_RCcontrol;
cmd ->CMD_UP_mode =Normal; //左中，右中，无模式
}
if(rc->sw_r ==CMD_SW_DOWN)
{
cmd ->CMD_UP_mode =Normal; //左中，右下，无模式
cmd ->CMD_CtrlType =UP_RCcontrol;
}
}
else if(rc->sw_l==CMD_SW_DOWN)
{
cmd ->CMD_CtrlType =UP_RCcontrol;
if(rc->sw_r ==CMD_SW_UP) cmd ->CMD_UP_mode =Normal; //左下，右上，投篮
if(rc->sw_r ==CMD_SW_MID) cmd ->CMD_UP_mode =Normal; //左下，右中，无模式
if(rc->sw_r ==CMD_SW_DOWN) cmd ->CMD_UP_mode =Normal; //左下，右上，无模式
}
if (prev_CtrlType!= cmd->CMD_CtrlType ) cmd ->last_CMD_CtrlType = prev_CtrlType;
if (prev_mode!=cmd->CMD_UP_mode ) cmd ->last_CMD_UP_mode =prev_mode;
return 0;
return 0;
}

179
User/device/cmd.h
View File

@ -1,19 +1,3 @@
/*
该任务用于接收来自各个不同的控制方式所期望的控制指令 将其集中统一化后分发给各个模块
*/
/*
按键控制逻辑
RC模式，左按键打到最上，右按键启用，中间无状态，上下各代表模式1、模式2
雷达导航，左按键打到中间，右按键禁用
左按键打到下面，保留，未启用
*/
#ifndef _CMD_H
@ -23,38 +7,48 @@
#define MID (0x09)
#define PICK (0x06)
typedef enum{
RC,//遥控器控制，左按键上
UP_RCcontrol,//遥控器控制，左按键上，控制上层
MID_NAVI,//雷达导航
}CMD_Chassis_CtrlType_e;
PICK_t,
RELAXED,//异常模式
}CMD_CtrlType_t;
typedef enum{
RELAXED,//异常模式
NORMAL,
GYRO_STAY,
RC_MODE1,
RC_NO_MODE,
RC_MODE2,
RC_MODE3,
RC_MODE4,
AUTO_NAVI
}CMD_Chassis_mode_e;
/*该结构体负责接收和解析地盘模块所需要的控制指令*/
typedef struct{
CMD_Chassis_CtrlType_e type;
CMD_Chassis_mode_e mode;
}CMD_Chassis_Ctrl_t;
Normal, //无模式
Dribble , //运球
Pitch, //投篮
/*视觉辅助下的投篮*/
Pitch_HAND,
Dribbl_transfer
}CMD_UP_mode_t;
typedef struct {
uint8_t status_fromnuc;
uint8_t ctrl_status; //取其中每一个二进制位用作通信
struct
{
fp32 vx;
fp32 vy;
fp32 wz;
}navi;
struct
{
fp32 posx;
fp32 posy;
fp32 posw;
}pick;
struct
{
fp32 angle;
}sick_cali;
} CMD_NUC_t;
/* 拨杆位置 */
typedef enum {
CMD_SW_ERR = 0,
@ -82,102 +76,45 @@ typedef struct {
} __attribute__((packed))CMD_RC_t;
typedef struct
{
fp32 Vx;
fp32 Vy;
fp32 Vw;
}OpsHopeVector_t;
typedef struct
{
fp32 x;
fp32 y;
fp32 z;
int cnt_point;//计数点
}ops_point;
typedef struct {
OpsHopeVector_t out;
int flag;//传递flag触发标志位
} CMD_ACTION_t;
typedef struct {
uint8_t status_fromnuc;
uint8_t ctrl_status; //取其中每一个二进制位用作通信
struct
{
fp32 vx;
fp32 vy;
fp32 wz;
}navi;
struct
{
fp32 posx;
fp32 posy;
fp32 posw;
}pick;
struct
{
fp32 angle;
}sick_cali;
} CMD_NUC_t;
typedef struct{
fp32 posy;
fp32 posx;
fp32 posw;
}CMD_FOR_PICK;
typedef struct {
fp32 vx;
fp32 vy;
fp32 wz;
}CMD_FOR_NAVI;
typedef struct {
uint8_t cmd_status;
uint8_t raw_status;
uint8_t status[7];
fp32 Vx;
fp32 Vy;
fp32 Vw;
fp32 poscamear;
fp32 key_ctrl_l;
fp32 key_ctrl_r;
fp32 Vx;
fp32 Vy;
fp32 Vw;
fp32 poscamear;
fp32 forsick_wz;
/*视觉*/
struct {
uint8_t cmd_status;
uint8_t raw_status;
uint8_t status[7];
struct
{
fp32 posx;
fp32 posy;
fp32 posw;
}pick;
CMD_Chassis_Ctrl_t C_cmd;
CMD_FOR_NAVI C_navi;
}NAVI_t;
CMD_UP_mode_t CMD_UP_mode;
CMD_CtrlType_t CMD_CtrlType;
CMD_CtrlType_t last_CMD_CtrlType;
CMD_UP_mode_t last_CMD_UP_mode;
} CMD_t;
int8_t CMD_Init(CMD_t *cmd);
int8_t CMD_ParseRc(CMD_t *cmd,CMD_RC_t *rc);
int8_t CMD_ParseAction(CMD_t *cmd,CMD_ACTION_t *act);
/*nuc数据统一到cmd*/
int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n);
int8_t CMD_ParseRC(CMD_t *cmd,const CMD_RC_t *rc) ;
int8_t CMD_SwitchStatus(CMD_t *cmd);
int8_t CMD_CtrlSet(CMD_t *cmd);
static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd) ;
#endif

75
User/task/action_task.c
View File

@ -1,75 +0,0 @@
/*
码盘解析任务
处理码盘测量的实际距离
将需要的数据共享给其他的线程
*/
#include "action_task.h"
#include "Action.h"
#include "user_task.h"
#ifdef DEBUG
Action_POS_t pos;
#else
static Action_POS pos;
#endif
void Task_action(void *argument)
{
(void)argument;
// osDelay(TASK_INIT_DELAY_ACTION);
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_ACTION;
ACTIONRECV_Init(&pos);
uint32_t tick = osKernelGetTickCount(); /* 获取当前控制任务运行频率的tick*/
while (1)
{
#ifdef DEBUG
/* 记录任务使用的的栈空闄*/
task_runtime.stack_water_mark.action =
osThreadGetStackSpace(osThreadGetId());
#endif
ACTION_StartReceiving();
ACTION_DataRefresh(&pos);
/* 使用下面的错误处理函数遇到一些问题
每10ms置标志位， ACTION_WaitDmaCplt 判断此标志位，通过后会执行速度计算
根据计算，任务运行频率大于中断产生频率，每一次运行判断标志位不通过，会导致结构体内数据清0的操作
目前的处理方法：注释掉Action_HandleOffline函数中，对结构体数据清零的函数memset
*/
if(ACTION_WaitDmaCplt()){
ACTION_Parse(&pos);
}
else{
Action_HandleOffline(&pos);
}
//将解算后的码盘位置值放入消息队列供其他任务使用
osMessageQueueReset(task_runtime.msgq.cmd.raw.Action);
osMessageQueuePut(task_runtime.msgq.cmd.raw.Action,(&pos),0,0);
tick += delay_tick; /* 计算下一个唤醒时刻*/
osDelayUntil(tick);
}
}

1
User/task/action_task.h
View File

@ -1 +0,0 @@

94
User/task/chassis_task.c
View File

@ -1,94 +0,0 @@
/*
底盘控制任务
*/
/* Includes ----------------------------------------------------------------- */
#include "Chassis.h"
#include "user_task.h"
#include "can_use.h"
#include <cmsis_os2.h>
#include "up.h"
#include "vofa.h"
static CAN_t can;
#ifdef DEBUG
CAN_Output_t out;
Chassis_t chassis ;
CMD_t ctrl;
UP_t upp;
#else
static CAN_Output_t out;
static Chassis_t chassis;
static Chassis_Ctrl_t ctrl;
static UP_t upp;
#endif
/**
* \brief 底盘任务
*
* \param argument
*/
void Task_Chassis(void *argument)
{
(void)argument; /* 未使用argument，消除警告*/
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_UP;
uint32_t tick = osKernelGetTickCount();
//
//
//// HAL_GPIO_WritePin(FlagForUpper_GPIO_Port,FlagForUpper_Pin,GPIO_PIN_RESET); //拉低电平 避免未清除
//
while(1)
{
#ifdef DEBUG
task_runtime.stack_water_mark.chassis = osThreadGetStackSpace(osThreadGetId());
#endif
// /*imu数据获取*/
// osMessageQueueGet(task_runtime.msgq.imu.eulr, &chassis.pos088.imu_eulr, NULL, 0);
//
// osMessageQueueGet(task_runtime.msgq.imu.gyro, &chassis.pos088.bmi088.gyro,NULL, 0);
//
// osMessageQueueGet(task_runtime.msgq.imu.accl, &chassis.pos088.bmi088.accl,NULL, 0);
// /*can上设备数据获取*/
// osMessageQueueGet(task_runtime.msgq.can.feedback.CAN_feedback, &can, NULL, 0);
//
// osMessageQueueGet(task_runtime.msgq.cmd.raw.Action, &chassis.Action_pos , NULL, 0);
/*底盘控制信息获取,目前dji遥控器*/
osMessageQueueGet(task_runtime.msgq.cmd.chassis,&ctrl, NULL, 0);
/*锁定RTOS（实时操作系统）内核，防止任务切换，直到 osKernelUnlock() 被调用*/
// osKernelLock();
/*更新电机反馈 */
Chassis_UpdateFeedback(&chassis, &can);
/*底盘控制*/
Chassis_Control(&chassis,&ctrl,&out);
/*解锁*/
// osKernelUnlock();
tick += delay_tick;
osDelayUntil(tick);
}
}

23
User/task/chassis_task.h
View File

@ -1,23 +0,0 @@
#ifndef CHASSIS_TASK_H
#define CHASSIS_TASK_H
extern void Task_Chassis(void *argument);
#endif

25
User/task/cmd_task.c
View File

@ -4,16 +4,14 @@
#ifdef DEBUG
CMD_t cmd;
CMD_RC_t rc_ctrl;
CMD_NUC_t Nuc;
CMD_ACTION_t cmd_ops_out;
CMD_RC_t rc_ctrl;
#else
static CMD_t cmd;
static CMD_RC_t rc_ctrl;
static CMD_NUC_t Nuc;
static CMD_ACTION_t cmd_ops_out;
static CMD_RC_t rc_ctrl;
#endif
@ -26,7 +24,6 @@ void Task_cmd(void *argument){
/* 计算到下一次调度任务所需要的tick数 */
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CTRL_CMD;
/**/
CMD_Init(&cmd);
uint32_t tick = osKernelGetTickCount(); /*控制任务运行频率的计时 */
while(1){
@ -40,25 +37,17 @@ void Task_cmd(void *argument){
/*将各任务接收到的原始数据解析为通用的控制命令*/
/*注意，不能将nuc和码盘导航一块使用*/
if(osMessageQueueGet(task_runtime.msgq.cmd.raw.rc, &rc_ctrl, 0, 0) == osOK)//遥控器
CMD_ParseRC(&cmd, &rc_ctrl);
if(osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc, &Nuc,0 ,0) ==osOK) //nuc
CMD_ParseNuc(&cmd,&Nuc);
CMD_CtrlSet(&cmd);
if(osMessageQueueGet(task_runtime.msgq.cmd.raw.rc, &rc_ctrl, 0, 0) == osOK)//遥控器
CMD_ParseRc(&cmd, &rc_ctrl);
if(osMessageQueueGet(task_runtime.msgq.cmd.raw .ops_9_out , &cmd_ops_out, 0, 0) == osOK)//码盘
CMD_ParseAction (&cmd ,&cmd_ops_out);
osKernelUnlock(); /* 同上 解锁RTOS内核 */
/*将需要与其他任务共享的数据放到消息队列里 此处主要分享给底盘 后续会添加和上层机构的通信 */
osMessageQueueReset(task_runtime.msgq.cmd.chassis);
osMessageQueuePut(task_runtime.msgq.cmd.chassis,&cmd,0,0);
osMessageQueueReset(task_runtime.msgq.cmd.up_ctrl);
osMessageQueuePut(task_runtime.msgq.cmd.up_ctrl,&cmd,0,0);
tick += delay_tick; /*计算下一个唤醒时刻*/
osDelayUntil(tick); /*绝对延时 等待下一个唤醒时刻 */

3
User/task/init.c
View File

@ -11,7 +11,6 @@
#include "task\user_task.h"
#include "can_use.h"
#include "cmd.h"
#include "Action.h"
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
@ -86,6 +85,8 @@ void Task_Init(void *argument) {
osMessageQueueNew(3u, sizeof(CMD_RC_t), NULL);
task_runtime.msgq.cmd.raw.nuc =
osMessageQueueNew(3u,sizeof(CMD_NUC_t), NULL);
task_runtime.msgq.cmd.up_ctrl =
osMessageQueueNew(3u,sizeof(CMD_t), NULL);
osKernelUnlock();
osThreadTerminate(osThreadGetId()); /* 结束自身 */

68
User/task/navi_task.c
View File

@ -1,68 +0,0 @@
#include "user_task.h"
#include "navi.h"
#include "map.h"
#ifdef DEBUG
ops_t o;
Action_POS_t pos_get;
CMD_ACTION_t ops_out;//经过导航算法解算后输出的期望控制值
#else
static ops_t o;
static Action_POS_t pos_get;
static CMD_ACTION_t ops_out;
#endif
void Task_Navi(void *argument){
(void)argument;
const uint32_t delay_tick = osKernelGetTickFreq() /TASK_FREQ_NAVI;
Action_init(&o,&(task_runtime.config.chassis_config->ops),&pos_get);
uint32_t tick = osKernelGetTickCount(); /* 获取当前控制任务运行频率的tick*/
while (1)
{
#ifdef DEBUG
/* 记录任务使用的的栈空间 */
task_runtime.stack_water_mark.action =
osThreadGetStackSpace(osThreadGetId());
#endif
osMessageQueueGet(task_runtime.msgq.imu.gyro,&o.ops_gyro,NULL,0);
osMessageQueueGet(task_runtime.msgq.imu.eulr,&o.ops_imu_pos,NULL,0);
osMessageQueueGet(task_runtime.msgq.cmd .raw .Action ,&pos_get,NULL,0);
go_path(&o,&ops_out);
//将解算后的导航期望运动值放入消息队列供其他任务使用
osMessageQueueReset(task_runtime.msgq.cmd.raw.ops_9_out);
osMessageQueuePut(task_runtime.msgq.cmd.raw.ops_9_out,(&ops_out),0,0);
tick += delay_tick; /* 计算下一个唤醒时刻*/
osDelayUntil(tick);
}
}
//
//
//
//
//
//
//

1
User/task/navi_task.h
View File

@ -1 +0,0 @@
#include "navi.h"

2
User/task/nuc_task.c
View File

@ -6,6 +6,8 @@
NUC_t nuc_raw;
CMD_NUC_t cmd_fromnuc;
#else
static NUC_t nuc_raw;
static CMD_NUC_t cmd_fromnuc;
#endif

27
User/task/up_task.c
View File

@ -10,7 +10,7 @@
#include "vofa.h"
static CAN_t can;
static CAN_t can;
#ifdef DEBUG
@ -19,6 +19,7 @@ CAN_Output_t UP_CAN_out;
UP_t UP ;
CMD_t up_cmd;
#else
@ -27,11 +28,13 @@ static CAN_Output_t up_can_out;
static UP_t UP;
static CMD_t up_cmd;
#endif
float aaa=0;
float bbb=0;
float CCC=0;
float ddd=0;
/**
@ -53,21 +56,28 @@ void Task_up(void *argument)
#ifdef DEBUG
task_runtime.stack_water_mark.up = osThreadGetStackSpace(osThreadGetId());
#endif
UP_UpdateFeedback(&UP, &can) ;
UP_UpdateFeedback(&UP, &can,&up_cmd) ;
// GM6020_control(&UP, 100) ;
UP_angle(&UP, bbb);
// UP_M3508_speed(&UP, 500);
// UP_angle_control(&UP,0,M2006);
//
//
// VESC_M5065_Control(&UP, 20000);
//
GO_SendData(&UP, 1,CCC);
GO_SendData(&UP, 0,aaa);
// UP_control(&UP,&UP_CAN_out,&up_cmd);
UP.motor_target .go_shoot =aaa;
// UP.motor_target .M2006_angle =bbb ;
ALL_Motor_Control(&UP,&UP_CAN_out);
// UP.motor_target .M3508_angle =CCC;
// UP.motor_target .go_shoot =aaa;
// UP.motor_target .M2006_angle =bbb;
// UP.motor_target .go_spin =ddd;
//
ALL_Motor_Control(&UP,&UP_CAN_out);
osDelay(1);
@ -81,6 +91,7 @@ void Task_up(void *argument)
/*can上设备数据获取*/
osMessageQueueGet(task_runtime.msgq.can.feedback.CAN_feedback, &can, NULL, 0);
osMessageQueueGet(task_runtime.msgq.cmd .up_ctrl , &up_cmd, NULL, 0);
/*锁定RTOS（实时操作系统）内核，防止任务切换，直到 osKernelUnlock() 被调用*/
osKernelLock();

45
User/task/user_task.h
View File

@ -42,16 +42,12 @@ typedef struct {
/* 任务 */
struct {
osThreadId_t atti_esti;
// osThreadId_t chassis;
osThreadId_t dr16;
// osThreadId_t action_ops;
osThreadId_t up;
osThreadId_t nuc;
// osThreadId_t ai;
osThreadId_t can;
osThreadId_t cmd;
osThreadId_t nuc;
// osThreadId_t navi;
osThreadId_t error_detect;
} thread;
@ -66,14 +62,12 @@ typedef struct {
struct {
struct {
osMessageQueueId_t rc;
osMessageQueueId_t nuc;
osMessageQueueId_t eulr;/*姿态结算得到的欧拉角*/
osMessageQueueId_t Action;
osMessageQueueId_t ops_9_out;
osMessageQueueId_t nuc;
osMessageQueueId_t eulr;/*姿态结算得到的欧拉角*/
}raw;
osMessageQueueId_t chassis;
osMessageQueueId_t up_ctrl;
osMessageQueueId_t status;
} cmd;
/* can任务放入、读取，电机或电容的输入输出 */
@ -108,40 +102,29 @@ typedef struct {
UBaseType_t can;
UBaseType_t atti_esti;
UBaseType_t dr16;
UBaseType_t action;
UBaseType_t cmd;
UBaseType_t nuc;
UBaseType_t navi;
UBaseType_t error_detect;
UBaseType_t nuc;
UBaseType_t up;
} stack_water_mark; /* stack使用 */
struct {
// float chassis;
float can;
float atti_esti;
float r12ds;
float cmd;
float nuc;
// float action;
// float navi;
float error_detect;
float up;
} freq; /* 任务运行频率 */
struct {
// float chassis;
float can;
float atti_esti;
// float ai;
float r12ds;
float cmd;
float nuc;
// float navi;
// float action;
float error_detect;
float up;
@ -156,21 +139,15 @@ extern const osThreadAttr_t attr_init;
extern const osThreadAttr_t attr_atti_esti;
//extern const osThreadAttr_t attr_chassis;
extern const osThreadAttr_t attr_can;
extern const osThreadAttr_t attr_cmd;
//extern const osThreadAttr_t attr_ops_9pos;
extern const osThreadAttr_t attr_nuc;
extern const osThreadAttr_t attr_error_detect;
extern const osThreadAttr_t attr_dr16;
//extern const osThreadAttr_t attr_navi;
extern const osThreadAttr_t attr_nuc;
extern const osThreadAttr_t attr_up;
/* Exported functions prototypes -------------------------------------------- */
@ -178,20 +155,14 @@ void Task_Init(void *argument);
void Task_AttiEsti(void *argument);
//void Task_Chassis(void *argument);
void Task_can(void *argument);
void Task_cmd(void *argument);
void Task_nuc(void *argument);
//void Task_action(void *argument);
//void Task_Navi(void *argument);
void Task_up(void *argument);
void Task_nuc(void *argument);
void Task_error_detect(void *argument);
void Task_dr16(void *argument);