robofish/rm_balance
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Compare commits

base: robofish:20b386a53bfb69be48665f1627b9484a80b98323
Branches Tags
robofish:main
robofish:MC_02
robofish:神秘小轮腿
robofish:ai
robofish:单独底层纯转发
..
compare: robofish:39adf0a2b68ff0c2f9bcf3fda2d85564e10f1c6e
Branches Tags
robofish:MC_02
robofish:神秘小轮腿
robofish:ai
robofish:单独底层纯转发
robofish:main

15 Commits
20b386a53b ... 39adf0a2b6

Author SHA1 Message Date
Robofish
39adf0a2b6 云台好了 2026-01-12 01:19:16 +08:00
Robofish
8c44c6a274 优化云台 2026-01-11 23:22:12 +08:00
Robofish
6228cafada 准备 2026-01-11 19:31:31 +08:00
Robofish
501ea4f1c4 添加imu 2026-01-11 19:25:37 +08:00
Robofish
c54ff4f988 加error pid init 2026-01-11 18:30:25 +08:00
Robofish
25a930c90a nb 2026-01-11 17:08:52 +08:00
Robofish
9652f6c5cc 改极性 2026-01-11 16:52:19 +08:00
Robofish
4aab1a909d 站不起来了草 2026-01-11 16:24:35 +08:00
Robofish
93759801cc 修扩展can 2026-01-11 03:49:16 +08:00
Robofish
22cdb32157 寄 2026-01-11 02:39:43 +08:00
Robofish
1bdee0af4d 死里 2026-01-11 02:21:31 +08:00
Robofish
47b8a6ef1c 抽象 2026-01-11 02:15:59 +08:00
Robofish
401508ed08 改底盘 2026-01-11 01:03:27 +08:00
Robofish
62e187a743 添加keil 2026-01-11 00:21:42 +08:00
Robofish
7bb87644be 换keil 2026-01-10 21:35:57 +08:00
80 changed files with 19705 additions and 92 deletions
Show Stats Expand all files Collapse all files

39
.gitignore vendored
View File

@ -1,3 +1,36 @@
build
mx.scratch
!.settings
*.rar
*.o
*.d
*.crf
*.htm
*.dep
*.map
*.bak
*.lnp
*.lst
*.ini
*.iex
*.pyc
*.sct
*.scvd
*.uvguix
*.dbg*
*.uvguix.*
.mxproject
RTE/
Templates/
Examples/
!*.uvprojx
!*.h
!*.c
!*.ioc
!*.axf
!*.bin
!*.hex
build/
dist/
*.spec
*.exe

7
.mxproject
View File

File diff suppressed because one or more lines are too long

7
CMakeLists.txt
View File

@ -62,22 +62,27 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
User/component/error_detect.c
User/component/filter.c
User/component/freertos_cli.c
User/component/lqr.c
User/component/pid.c
User/component/user_math.c
User/component/vmc.c
# User/device sources
User/device/bmi088.c
User/device/buzzer.c
User/device/dr16.c
User/device/gimbal_imu.c
User/device/motor.c
User/device/motor_dm.c
User/device/motor_lk.c
User/device/motor_lz.c
User/device/motor_rm.c
# User/module sources
User/module/balance_chassis.c
User/module/config.c
User/module/shoot.c
User/module/gimbal.c
# User/task sources
User/task/atti_esit.c

12
Core/Src/fdcan.c
View File

@ -42,7 +42,7 @@ void MX_FDCAN1_Init(void)
hfdcan1.Instance = FDCAN1;
hfdcan1.Init.FrameFormat = FDCAN_FRAME_CLASSIC;
hfdcan1.Init.Mode = FDCAN_MODE_NORMAL;
hfdcan1.Init.AutoRetransmission = DISABLE;
hfdcan1.Init.AutoRetransmission = ENABLE;
hfdcan1.Init.TransmitPause = DISABLE;
hfdcan1.Init.ProtocolException = DISABLE;
hfdcan1.Init.NominalPrescaler = 24;
@ -90,7 +90,7 @@ void MX_FDCAN2_Init(void)
hfdcan2.Instance = FDCAN2;
hfdcan2.Init.FrameFormat = FDCAN_FRAME_CLASSIC;
hfdcan2.Init.Mode = FDCAN_MODE_NORMAL;
hfdcan2.Init.AutoRetransmission = DISABLE;
hfdcan2.Init.AutoRetransmission = ENABLE;
hfdcan2.Init.TransmitPause = DISABLE;
hfdcan2.Init.ProtocolException = DISABLE;
hfdcan2.Init.NominalPrescaler = 24;
@ -106,7 +106,7 @@ void MX_FDCAN2_Init(void)
hfdcan2.Init.ExtFiltersNbr = 0;
hfdcan2.Init.RxFifo0ElmtsNbr = 32;
hfdcan2.Init.RxFifo0ElmtSize = FDCAN_DATA_BYTES_8;
hfdcan2.Init.RxFifo1ElmtsNbr = 0;
hfdcan2.Init.RxFifo1ElmtsNbr = 32;
hfdcan2.Init.RxFifo1ElmtSize = FDCAN_DATA_BYTES_8;
hfdcan2.Init.RxBuffersNbr = 0;
hfdcan2.Init.RxBufferSize = FDCAN_DATA_BYTES_8;
@ -138,7 +138,7 @@ void MX_FDCAN3_Init(void)
hfdcan3.Instance = FDCAN3;
hfdcan3.Init.FrameFormat = FDCAN_FRAME_CLASSIC;
hfdcan3.Init.Mode = FDCAN_MODE_NORMAL;
hfdcan3.Init.AutoRetransmission = DISABLE;
hfdcan3.Init.AutoRetransmission = ENABLE;
hfdcan3.Init.TransmitPause = DISABLE;
hfdcan3.Init.ProtocolException = DISABLE;
hfdcan3.Init.NominalPrescaler = 24;
@ -151,10 +151,10 @@ void MX_FDCAN3_Init(void)
hfdcan3.Init.DataTimeSeg2 = 1;
hfdcan3.Init.MessageRAMOffset = 0x812;
hfdcan3.Init.StdFiltersNbr = 1;
hfdcan3.Init.ExtFiltersNbr = 0;
hfdcan3.Init.ExtFiltersNbr = 1;
hfdcan3.Init.RxFifo0ElmtsNbr = 32;
hfdcan3.Init.RxFifo0ElmtSize = FDCAN_DATA_BYTES_8;
hfdcan3.Init.RxFifo1ElmtsNbr = 0;
hfdcan3.Init.RxFifo1ElmtsNbr = 32;
hfdcan3.Init.RxFifo1ElmtSize = FDCAN_DATA_BYTES_8;
hfdcan3.Init.RxBuffersNbr = 0;
hfdcan3.Init.RxBufferSize = FDCAN_DATA_BYTES_8;

14
CtrBoard-H7_ALL.ioc
View File

@ -106,33 +106,39 @@ Dma.UART5_RX.3.SyncEnable=DISABLE
Dma.UART5_RX.3.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT
Dma.UART5_RX.3.SyncRequestNumber=1
Dma.UART5_RX.3.SyncSignalID=NONE
FDCAN1.AutoRetransmission=ENABLE
FDCAN1.CalculateBaudRateNominal=1000000
FDCAN1.CalculateTimeBitNominal=1000
FDCAN1.CalculateTimeQuantumNominal=200.0
FDCAN1.IPParameters=CalculateTimeQuantumNominal,CalculateTimeBitNominal,CalculateBaudRateNominal,RxFifo0ElmtsNbr,TxFifoQueueElmtsNbr,NominalPrescaler,StdFiltersNbr,NominalTimeSeg1
FDCAN1.IPParameters=CalculateTimeQuantumNominal,CalculateTimeBitNominal,CalculateBaudRateNominal,RxFifo0ElmtsNbr,TxFifoQueueElmtsNbr,NominalPrescaler,StdFiltersNbr,NominalTimeSeg1,AutoRetransmission
FDCAN1.NominalPrescaler=24
FDCAN1.NominalTimeSeg1=3
FDCAN1.RxFifo0ElmtsNbr=32
FDCAN1.StdFiltersNbr=1
FDCAN1.TxFifoQueueElmtsNbr=32
FDCAN2.AutoRetransmission=ENABLE
FDCAN2.CalculateBaudRateNominal=1000000
FDCAN2.CalculateTimeBitNominal=1000
FDCAN2.CalculateTimeQuantumNominal=200.0
FDCAN2.IPParameters=CalculateTimeQuantumNominal,CalculateTimeBitNominal,CalculateBaudRateNominal,MessageRAMOffset,RxFifo0ElmtsNbr,TxFifoQueueElmtsNbr,NominalPrescaler,StdFiltersNbr,NominalTimeSeg1
FDCAN2.IPParameters=CalculateTimeQuantumNominal,CalculateTimeBitNominal,CalculateBaudRateNominal,MessageRAMOffset,RxFifo0ElmtsNbr,TxFifoQueueElmtsNbr,NominalPrescaler,StdFiltersNbr,NominalTimeSeg1,AutoRetransmission,RxFifo1ElmtsNbr
FDCAN2.MessageRAMOffset=0x406
FDCAN2.NominalPrescaler=24
FDCAN2.NominalTimeSeg1=3
FDCAN2.RxFifo0ElmtsNbr=32
FDCAN2.RxFifo1ElmtsNbr=32
FDCAN2.StdFiltersNbr=1
FDCAN2.TxFifoQueueElmtsNbr=32
FDCAN3.AutoRetransmission=ENABLE
FDCAN3.CalculateBaudRateNominal=1000000
FDCAN3.CalculateTimeBitNominal=1000
FDCAN3.CalculateTimeQuantumNominal=200.0
FDCAN3.IPParameters=CalculateTimeQuantumNominal,CalculateTimeBitNominal,CalculateBaudRateNominal,MessageRAMOffset,RxFifo0ElmtsNbr,TxFifoQueueElmtsNbr,NominalPrescaler,StdFiltersNbr,NominalTimeSeg1
FDCAN3.ExtFiltersNbr=1
FDCAN3.IPParameters=CalculateTimeQuantumNominal,CalculateTimeBitNominal,CalculateBaudRateNominal,MessageRAMOffset,RxFifo0ElmtsNbr,TxFifoQueueElmtsNbr,NominalPrescaler,StdFiltersNbr,NominalTimeSeg1,AutoRetransmission,RxFifo1ElmtsNbr,ExtFiltersNbr
FDCAN3.MessageRAMOffset=0x812
FDCAN3.NominalPrescaler=24
FDCAN3.NominalTimeSeg1=3
FDCAN3.RxFifo0ElmtsNbr=32
FDCAN3.RxFifo1ElmtsNbr=32
FDCAN3.StdFiltersNbr=1
FDCAN3.TxFifoQueueElmtsNbr=32
FREERTOS.IPParameters=Tasks01,configTOTAL_HEAP_SIZE
@ -503,7 +509,7 @@ ProjectManager.ProjectName=CtrBoard-H7_ALL
ProjectManager.ProjectStructure=
ProjectManager.RegisterCallBack=
ProjectManager.StackSize=0x2000
ProjectManager.TargetToolchain=CMake
ProjectManager.TargetToolchain=MDK-ARM V5.32
ProjectManager.ToolChainLocation=
ProjectManager.UAScriptAfterPath=
ProjectManager.UAScriptBeforePath=

1544
MDK-ARM/CtrBoard-H7_ALL.uvoptx Normal file
View File

File diff suppressed because it is too large Load Diff

943
MDK-ARM/CtrBoard-H7_ALL.uvprojx Normal file
View File

@ -0,0 +1,943 @@
<?xml version="1.0" encoding="UTF-8" standalone="no" ?>
<Project xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="project_projx.xsd">
<SchemaVersion>2.1</SchemaVersion>
<Header>### uVision Project, (C) Keil Software</Header>
<Targets>
<Target>
<TargetName>CtrBoard-H7_ALL</TargetName>
<ToolsetNumber>0x4</ToolsetNumber>
<ToolsetName>ARM-ADS</ToolsetName>
<pArmCC>6160000::V6.16::ARMCLANG</pArmCC>
<pCCUsed>6160000::V6.16::ARMCLANG</pCCUsed>
<uAC6>1</uAC6>
<TargetOption>
<TargetCommonOption>
<Device>STM32H723VGTx</Device>
<Vendor>STMicroelectronics</Vendor>
<PackID>Keil.STM32H7xx_DFP.4.1.3</PackID>
<PackURL>https://www.keil.com/pack/</PackURL>
<Cpu>IRAM(0x20000000-0x2001FFFF) IRAM2(0x24000000-0x2404FFFF) IROM(0x8000000-0x80FFFFF) CLOCK(12000000) FPU3(DFPU) CPUTYPE("Cortex-M7") ELITTLE TZ</Cpu>
<FlashUtilSpec></FlashUtilSpec>
<StartupFile></StartupFile>
<FlashDriverDll></FlashDriverDll>
<DeviceId>0</DeviceId>
<RegisterFile></RegisterFile>
<MemoryEnv></MemoryEnv>
<Cmp></Cmp>
<Asm></Asm>
<Linker></Linker>
<OHString></OHString>
<InfinionOptionDll></InfinionOptionDll>
<SLE66CMisc></SLE66CMisc>
<SLE66AMisc></SLE66AMisc>
<SLE66LinkerMisc></SLE66LinkerMisc>
<SFDFile>$$Device:STM32H723VGTx$CMSIS\SVD\STM32H723.svd</SFDFile>
<bCustSvd>0</bCustSvd>
<UseEnv>0</UseEnv>
<BinPath></BinPath>
<IncludePath></IncludePath>
<LibPath></LibPath>
<RegisterFilePath></RegisterFilePath>
<DBRegisterFilePath></DBRegisterFilePath>
<TargetStatus>
<Error>0</Error>
<ExitCodeStop>0</ExitCodeStop>
<ButtonStop>0</ButtonStop>
<NotGenerated>0</NotGenerated>
<InvalidFlash>1</InvalidFlash>
</TargetStatus>
<OutputDirectory>CtrBoard-H7_ALL\</OutputDirectory>
<OutputName>CtrBoard-H7_ALL</OutputName>
<CreateExecutable>1</CreateExecutable>
<CreateLib>0</CreateLib>
<CreateHexFile>1</CreateHexFile>
<DebugInformation>1</DebugInformation>
<BrowseInformation>1</BrowseInformation>
<ListingPath></ListingPath>
<HexFormatSelection>1</HexFormatSelection>
<Merge32K>0</Merge32K>
<CreateBatchFile>0</CreateBatchFile>
<BeforeCompile>
<RunUserProg1>0</RunUserProg1>
<RunUserProg2>0</RunUserProg2>
<UserProg1Name></UserProg1Name>
<UserProg2Name></UserProg2Name>
<UserProg1Dos16Mode>0</UserProg1Dos16Mode>
<UserProg2Dos16Mode>0</UserProg2Dos16Mode>
<nStopU1X>0</nStopU1X>
<nStopU2X>0</nStopU2X>
</BeforeCompile>
<BeforeMake>
<RunUserProg1>0</RunUserProg1>
<RunUserProg2>0</RunUserProg2>
<UserProg1Name></UserProg1Name>
<UserProg2Name></UserProg2Name>
<UserProg1Dos16Mode>0</UserProg1Dos16Mode>
<UserProg2Dos16Mode>0</UserProg2Dos16Mode>
<nStopB1X>0</nStopB1X>
<nStopB2X>0</nStopB2X>
</BeforeMake>
<AfterMake>
<RunUserProg1>0</RunUserProg1>
<RunUserProg2>1</RunUserProg2>
<UserProg1Name></UserProg1Name>
<UserProg2Name></UserProg2Name>
<UserProg1Dos16Mode>0</UserProg1Dos16Mode>
<UserProg2Dos16Mode>0</UserProg2Dos16Mode>
<nStopA1X>0</nStopA1X>
<nStopA2X>0</nStopA2X>
</AfterMake>
<SelectedForBatchBuild>1</SelectedForBatchBuild>
<SVCSIdString></SVCSIdString>
</TargetCommonOption>
<CommonProperty>
<UseCPPCompiler>0</UseCPPCompiler>
<RVCTCodeConst>0</RVCTCodeConst>
<RVCTZI>0</RVCTZI>
<RVCTOtherData>0</RVCTOtherData>
<ModuleSelection>0</ModuleSelection>
<IncludeInBuild>1</IncludeInBuild>
<AlwaysBuild>0</AlwaysBuild>
<GenerateAssemblyFile>0</GenerateAssemblyFile>
<AssembleAssemblyFile>0</AssembleAssemblyFile>
<PublicsOnly>0</PublicsOnly>
<StopOnExitCode>3</StopOnExitCode>
<CustomArgument></CustomArgument>
<IncludeLibraryModules></IncludeLibraryModules>
<ComprImg>0</ComprImg>
</CommonProperty>
<DllOption>
<SimDllName>SARMCM3.DLL</SimDllName>
<SimDllArguments>-REMAP -MPU</SimDllArguments>
<SimDlgDll>DCM.DLL</SimDlgDll>
<SimDlgDllArguments>-pCM7</SimDlgDllArguments>
<TargetDllName>SARMCM3.DLL</TargetDllName>
<TargetDllArguments>-MPU</TargetDllArguments>
<TargetDlgDll>TCM.DLL</TargetDlgDll>
<TargetDlgDllArguments>-pCM7</TargetDlgDllArguments>
</DllOption>
<DebugOption>
<OPTHX>
<HexSelection>1</HexSelection>
<HexRangeLowAddress>0</HexRangeLowAddress>
<HexRangeHighAddress>0</HexRangeHighAddress>
<HexOffset>0</HexOffset>
<Oh166RecLen>16</Oh166RecLen>
</OPTHX>
</DebugOption>
<Utilities>
<Flash1>
<UseTargetDll>1</UseTargetDll>
<UseExternalTool>0</UseExternalTool>
<RunIndependent>0</RunIndependent>
<UpdateFlashBeforeDebugging>1</UpdateFlashBeforeDebugging>
<Capability>1</Capability>
<DriverSelection>4101</DriverSelection>
</Flash1>
<bUseTDR>1</bUseTDR>
<Flash2>BIN\UL2V8M.DLL</Flash2>
<Flash3></Flash3>
<Flash4></Flash4>
<pFcarmOut></pFcarmOut>
<pFcarmGrp></pFcarmGrp>
<pFcArmRoot></pFcArmRoot>
<FcArmLst>0</FcArmLst>
</Utilities>
<TargetArmAds>
<ArmAdsMisc>
<GenerateListings>0</GenerateListings>
<asHll>1</asHll>
<asAsm>1</asAsm>
<asMacX>1</asMacX>
<asSyms>1</asSyms>
<asFals>1</asFals>
<asDbgD>1</asDbgD>
<asForm>1</asForm>
<ldLst>0</ldLst>
<ldmm>1</ldmm>
<ldXref>1</ldXref>
<BigEnd>0</BigEnd>
<AdsALst>1</AdsALst>
<AdsACrf>1</AdsACrf>
<AdsANop>0</AdsANop>
<AdsANot>0</AdsANot>
<AdsLLst>1</AdsLLst>
<AdsLmap>1</AdsLmap>
<AdsLcgr>1</AdsLcgr>
<AdsLsym>1</AdsLsym>
<AdsLszi>1</AdsLszi>
<AdsLtoi>1</AdsLtoi>
<AdsLsun>1</AdsLsun>
<AdsLven>1</AdsLven>
<AdsLsxf>1</AdsLsxf>
<RvctClst>0</RvctClst>
<GenPPlst>0</GenPPlst>
<AdsCpuType>"Cortex-M7"</AdsCpuType>
<RvctDeviceName></RvctDeviceName>
<mOS>0</mOS>
<uocRom>0</uocRom>
<uocRam>0</uocRam>
<hadIROM>1</hadIROM>
<hadIRAM>1</hadIRAM>
<hadXRAM>0</hadXRAM>
<uocXRam>0</uocXRam>
<RvdsVP>3</RvdsVP>
<RvdsMve>0</RvdsMve>
<RvdsCdeCp>0</RvdsCdeCp>
<hadIRAM2>1</hadIRAM2>
<hadIROM2>0</hadIROM2>
<StupSel>8</StupSel>
<useUlib>0</useUlib>
<EndSel>0</EndSel>
<uLtcg>0</uLtcg>
<nSecure>0</nSecure>
<RoSelD>3</RoSelD>
<RwSelD>4</RwSelD>
<CodeSel>0</CodeSel>
<OptFeed>0</OptFeed>
<NoZi1>0</NoZi1>
<NoZi2>0</NoZi2>
<NoZi3>0</NoZi3>
<NoZi4>0</NoZi4>
<NoZi5>0</NoZi5>
<Ro1Chk>0</Ro1Chk>
<Ro2Chk>0</Ro2Chk>
<Ro3Chk>0</Ro3Chk>
<Ir1Chk>1</Ir1Chk>
<Ir2Chk>0</Ir2Chk>
<Ra1Chk>0</Ra1Chk>
<Ra2Chk>0</Ra2Chk>
<Ra3Chk>0</Ra3Chk>
<Im1Chk>0</Im1Chk>
<Im2Chk>1</Im2Chk>
<OnChipMemories>
<Ocm1>
<Type>0</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</Ocm1>
<Ocm2>
<Type>0</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</Ocm2>
<Ocm3>
<Type>0</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</Ocm3>
<Ocm4>
<Type>0</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</Ocm4>
<Ocm5>
<Type>0</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</Ocm5>
<Ocm6>
<Type>0</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</Ocm6>
<IRAM>
<Type>0</Type>
<StartAddress>0x20000000</StartAddress>
<Size>0x20000</Size>
</IRAM>
<IROM>
<Type>1</Type>
<StartAddress>0x8000000</StartAddress>
<Size>0x100000</Size>
</IROM>
<XRAM>
<Type>0</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</XRAM>
<OCR_RVCT1>
<Type>1</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</OCR_RVCT1>
<OCR_RVCT2>
<Type>1</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</OCR_RVCT2>
<OCR_RVCT3>
<Type>1</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</OCR_RVCT3>
<OCR_RVCT4>
<Type>1</Type>
<StartAddress>0x8000000</StartAddress>
<Size>0x100000</Size>
</OCR_RVCT4>
<OCR_RVCT5>
<Type>1</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</OCR_RVCT5>
<OCR_RVCT6>
<Type>0</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</OCR_RVCT6>
<OCR_RVCT7>
<Type>0</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</OCR_RVCT7>
<OCR_RVCT8>
<Type>0</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</OCR_RVCT8>
<OCR_RVCT9>
<Type>0</Type>
<StartAddress>0x20000000</StartAddress>
<Size>0x20000</Size>
</OCR_RVCT9>
<OCR_RVCT10>
<Type>0</Type>
<StartAddress>0x24000000</StartAddress>
<Size>0x50000</Size>
</OCR_RVCT10>
</OnChipMemories>
<RvctStartVector></RvctStartVector>
</ArmAdsMisc>
<Cads>
<interw>1</interw>
<Optim>1</Optim>
<oTime>0</oTime>
<SplitLS>0</SplitLS>
<OneElfS>1</OneElfS>
<Strict>0</Strict>
<EnumInt>0</EnumInt>
<PlainCh>0</PlainCh>
<Ropi>0</Ropi>
<Rwpi>0</Rwpi>
<wLevel>3</wLevel>
<uThumb>0</uThumb>
<uSurpInc>0</uSurpInc>
<uC99>1</uC99>
<uGnu>1</uGnu>
<useXO>0</useXO>
<v6Lang>3</v6Lang>
<v6LangP>3</v6LangP>
<vShortEn>1</vShortEn>
<vShortWch>1</vShortWch>
<v6Lto>0</v6Lto>
<v6WtE>0</v6WtE>
<v6Rtti>0</v6Rtti>
<VariousControls>
<MiscControls></MiscControls>
<Define>USE_PWR_LDO_SUPPLY,USE_HAL_DRIVER,STM32H723xx</Define>
<Undefine></Undefine>
<IncludePath>../Core/Inc;../Drivers/STM32H7xx_HAL_Driver/Inc;../Drivers/STM32H7xx_HAL_Driver/Inc/Legacy;../Middlewares/Third_Party/FreeRTOS/Source/include;../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2;../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F;../Drivers/CMSIS/Device/ST/STM32H7xx/Include;../Drivers/CMSIS/Include;../User</IncludePath>
</VariousControls>
</Cads>
<Aads>
<interw>1</interw>
<Ropi>0</Ropi>
<Rwpi>0</Rwpi>
<thumb>0</thumb>
<SplitLS>0</SplitLS>
<SwStkChk>0</SwStkChk>
<NoWarn>0</NoWarn>
<uSurpInc>0</uSurpInc>
<useXO>0</useXO>
<ClangAsOpt>1</ClangAsOpt>
<VariousControls>
<MiscControls></MiscControls>
<Define></Define>
<Undefine></Undefine>
<IncludePath>../Core/Inc;../Drivers/STM32H7xx_HAL_Driver/Inc;../Drivers/STM32H7xx_HAL_Driver/Inc/Legacy;../Middlewares/Third_Party/FreeRTOS/Source/include;../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2;../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F;../Drivers/CMSIS/Device/ST/STM32H7xx/Include;../Drivers/CMSIS/Include</IncludePath>
</VariousControls>
</Aads>
<LDads>
<umfTarg>1</umfTarg>
<Ropi>0</Ropi>
<Rwpi>0</Rwpi>
<noStLib>0</noStLib>
<RepFail>1</RepFail>
<useFile>0</useFile>
<TextAddressRange></TextAddressRange>
<DataAddressRange></DataAddressRange>
<pXoBase></pXoBase>
<ScatterFile></ScatterFile>
<IncludeLibs></IncludeLibs>
<IncludeLibsPath></IncludeLibsPath>
<Misc></Misc>
<LinkerInputFile></LinkerInputFile>
<DisabledWarnings></DisabledWarnings>
</LDads>
</TargetArmAds>
</TargetOption>
<Groups>
<Group>
<GroupName>Application/MDK-ARM</GroupName>
<Files>
<File>
<FileName>startup_stm32h723xx.s</FileName>
<FileType>2</FileType>
<FilePath>startup_stm32h723xx.s</FilePath>
</File>
</Files>
</Group>
<Group>
<GroupName>Application/User/Core</GroupName>
<Files>
<File>
<FileName>main.c</FileName>
<FileType>1</FileType>
<FilePath>../Core/Src/main.c</FilePath>
</File>
<File>
<FileName>gpio.c</FileName>
<FileType>1</FileType>
<FilePath>../Core/Src/gpio.c</FilePath>
</File>
<File>
<FileName>freertos.c</FileName>
<FileType>1</FileType>
<FilePath>../Core/Src/freertos.c</FilePath>
</File>
<File>
<FileName>adc.c</FileName>
<FileType>1</FileType>
<FilePath>../Core/Src/adc.c</FilePath>
</File>
<File>
<FileName>dma.c</FileName>
<FileType>1</FileType>
<FilePath>../Core/Src/dma.c</FilePath>
</File>
<File>
<FileName>fdcan.c</FileName>
<FileType>1</FileType>
<FilePath>../Core/Src/fdcan.c</FilePath>
</File>
<File>
<FileName>octospi.c</FileName>
<FileType>1</FileType>
<FilePath>../Core/Src/octospi.c</FilePath>
</File>
<File>
<FileName>spi.c</FileName>
<FileType>1</FileType>
<FilePath>../Core/Src/spi.c</FilePath>
</File>
<File>
<FileName>tim.c</FileName>
<FileType>1</FileType>
<FilePath>../Core/Src/tim.c</FilePath>
</File>
<File>
<FileName>usart.c</FileName>
<FileType>1</FileType>
<FilePath>../Core/Src/usart.c</FilePath>
</File>
<File>
<FileName>usb_otg.c</FileName>
<FileType>1</FileType>
<FilePath>../Core/Src/usb_otg.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_it.c</FileName>
<FileType>1</FileType>
<FilePath>../Core/Src/stm32h7xx_it.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_msp.c</FileName>
<FileType>1</FileType>
<FilePath>../Core/Src/stm32h7xx_hal_msp.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_timebase_tim.c</FileName>
<FileType>1</FileType>
<FilePath>../Core/Src/stm32h7xx_hal_timebase_tim.c</FilePath>
</File>
</Files>
</Group>
<Group>
<GroupName>Drivers/STM32H7xx_HAL_Driver</GroupName>
<Files>
<File>
<FileName>stm32h7xx_hal_tim.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_tim.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_tim_ex.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_tim_ex.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_adc.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_adc.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_adc_ex.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_adc_ex.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_rcc.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_rcc.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_rcc_ex.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_rcc_ex.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_flash.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_flash.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_flash_ex.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_flash_ex.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_gpio.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_gpio.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_hsem.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_hsem.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_dma.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_dma.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_dma_ex.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_dma_ex.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_mdma.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_mdma.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_pwr.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_pwr.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_pwr_ex.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_pwr_ex.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_cortex.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_cortex.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_i2c.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_i2c.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_i2c_ex.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_i2c_ex.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_exti.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_exti.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_fdcan.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_fdcan.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_ospi.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_ospi.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_spi.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_spi.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_spi_ex.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_spi_ex.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_uart.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_uart.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_uart_ex.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_uart_ex.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_pcd.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_pcd.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_hal_pcd_ex.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_pcd_ex.c</FilePath>
</File>
<File>
<FileName>stm32h7xx_ll_usb.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_ll_usb.c</FilePath>
</File>
</Files>
</Group>
<Group>
<GroupName>Drivers/CMSIS</GroupName>
<Files>
<File>
<FileName>system_stm32h7xx.c</FileName>
<FileType>1</FileType>
<FilePath>../Core/Src/system_stm32h7xx.c</FilePath>
</File>
</Files>
</Group>
<Group>
<GroupName>Middlewares/FreeRTOS</GroupName>
<Files>
<File>
<FileName>croutine.c</FileName>
<FileType>1</FileType>
<FilePath>../Middlewares/Third_Party/FreeRTOS/Source/croutine.c</FilePath>
</File>
<File>
<FileName>event_groups.c</FileName>
<FileType>1</FileType>
<FilePath>../Middlewares/Third_Party/FreeRTOS/Source/event_groups.c</FilePath>
</File>
<File>
<FileName>list.c</FileName>
<FileType>1</FileType>
<FilePath>../Middlewares/Third_Party/FreeRTOS/Source/list.c</FilePath>
</File>
<File>
<FileName>queue.c</FileName>
<FileType>1</FileType>
<FilePath>../Middlewares/Third_Party/FreeRTOS/Source/queue.c</FilePath>
</File>
<File>
<FileName>stream_buffer.c</FileName>
<FileType>1</FileType>
<FilePath>../Middlewares/Third_Party/FreeRTOS/Source/stream_buffer.c</FilePath>
</File>
<File>
<FileName>tasks.c</FileName>
<FileType>1</FileType>
<FilePath>../Middlewares/Third_Party/FreeRTOS/Source/tasks.c</FilePath>
</File>
<File>
<FileName>timers.c</FileName>
<FileType>1</FileType>
<FilePath>../Middlewares/Third_Party/FreeRTOS/Source/timers.c</FilePath>
</File>
<File>
<FileName>cmsis_os2.c</FileName>
<FileType>1</FileType>
<FilePath>../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os2.c</FilePath>
</File>
<File>
<FileName>heap_4.c</FileName>
<FileType>1</FileType>
<FilePath>../Middlewares/Third_Party/FreeRTOS/Source/portable/MemMang/heap_4.c</FilePath>
</File>
<File>
<FileName>port.c</FileName>
<FileType>1</FileType>
<FilePath>../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F/port.c</FilePath>
</File>
</Files>
</Group>
<Group>
<GroupName>User/bsp</GroupName>
<Files>
<File>
<FileName>fdcan.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\bsp\fdcan.c</FilePath>
</File>
<File>
<FileName>flash.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\bsp\flash.c</FilePath>
</File>
<File>
<FileName>gpio.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\bsp\gpio.c</FilePath>
</File>
<File>
<FileName>mm.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\bsp\mm.c</FilePath>
</File>
<File>
<FileName>pwm.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\bsp\pwm.c</FilePath>
</File>
<File>
<FileName>spi.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\bsp\spi.c</FilePath>
</File>
<File>
<FileName>time.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\bsp\time.c</FilePath>
</File>
<File>
<FileName>uart.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\bsp\uart.c</FilePath>
</File>
</Files>
</Group>
<Group>
<GroupName>User/component</GroupName>
<Files>
<File>
<FileName>ahrs.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\component\ahrs.c</FilePath>
</File>
<File>
<FileName>crc8.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\component\crc8.c</FilePath>
</File>
<File>
<FileName>crc16.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\component\crc16.c</FilePath>
</File>
<File>
<FileName>error_detect.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\component\error_detect.c</FilePath>
</File>
<File>
<FileName>filter.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\component\filter.c</FilePath>
</File>
<File>
<FileName>freertos_cli.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\component\freertos_cli.c</FilePath>
</File>
<File>
<FileName>lqr.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\component\lqr.c</FilePath>
</File>
<File>
<FileName>pid.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\component\pid.c</FilePath>
</File>
<File>
<FileName>user_math.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\component\user_math.c</FilePath>
</File>
<File>
<FileName>vmc.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\component\vmc.c</FilePath>
</File>
</Files>
</Group>
<Group>
<GroupName>User/device</GroupName>
<Files>
<File>
<FileName>bmi088.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\device\bmi088.c</FilePath>
</File>
<File>
<FileName>buzzer.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\device\buzzer.c</FilePath>
</File>
<File>
<FileName>dr16.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\device\dr16.c</FilePath>
</File>
<File>
<FileName>motor.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\device\motor.c</FilePath>
</File>
<File>
<FileName>motor_dm.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\device\motor_dm.c</FilePath>
</File>
<File>
<FileName>motor_lk.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\device\motor_lk.c</FilePath>
</File>
<File>
<FileName>motor_lz.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\device\motor_lz.c</FilePath>
</File>
<File>
<FileName>motor_rm.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\device\motor_rm.c</FilePath>
</File>
<File>
<FileName>gimbal_imu.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\device\gimbal_imu.c</FilePath>
</File>
</Files>
</Group>
<Group>
<GroupName>User/module</GroupName>
<Files>
<File>
<FileName>balance_chassis.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\module\balance_chassis.c</FilePath>
</File>
<File>
<FileName>config.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\module\config.c</FilePath>
</File>
<File>
<FileName>shoot.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\module\shoot.c</FilePath>
</File>
<File>
<FileName>gimbal.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\module\gimbal.c</FilePath>
</File>
</Files>
</Group>
<Group>
<GroupName>User/task</GroupName>
<Files>
<File>
<FileName>atti_esit.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\task\atti_esit.c</FilePath>
</File>
<File>
<FileName>blink.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\task\blink.c</FilePath>
</File>
<File>
<FileName>ctrl_chassis.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\task\ctrl_chassis.c</FilePath>
</File>
<File>
<FileName>ctrl_gimbal.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\task\ctrl_gimbal.c</FilePath>
</File>
<File>
<FileName>ctrl_shoot.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\task\ctrl_shoot.c</FilePath>
</File>
<File>
<FileName>init.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\task\init.c</FilePath>
</File>
<File>
<FileName>monitor.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\task\monitor.c</FilePath>
</File>
<File>
<FileName>rc.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\task\rc.c</FilePath>
</File>
<File>
<FileName>user_task.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\task\user_task.c</FilePath>
</File>
<File>
<FileName>user_task.h</FileName>
<FileType>5</FileType>
<FilePath>..\User\task\user_task.h</FilePath>
</File>
</Files>
</Group>
<Group>
<GroupName>::CMSIS</GroupName>
</Group>
</Groups>
</Target>
</Targets>
<RTE>
<apis/>
<components>
<component Cclass="CMSIS" Cgroup="CORE" Cvendor="ARM" Cversion="6.1.1" condition="ARMv6_7_8-M Device">
<package name="CMSIS" schemaVersion="1.7.40" url="https://www.keil.com/pack/" vendor="ARM" version="6.2.0"/>
<targetInfos>
<targetInfo name="CtrBoard-H7_ALL"/>
</targetInfos>
</component>
</components>
<files/>
</RTE>
<LayerInfo>
<Layers>
<Layer>
<LayName>CtrBoard-H7_ALL</LayName>
<LayPrjMark>1</LayPrjMark>
</Layer>
</Layers>
</LayerInfo>
</Project>

BIN
MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.axf Normal file
View File

Binary file not shown.

8566
MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.hex Normal file
View File

File diff suppressed because it is too large Load Diff

619
MDK-ARM/startup_stm32h723xx.s Normal file
View File

@ -0,0 +1,619 @@
;********************************************************************************
;* File Name : startup_stm32h723xx.s
;* @author MCD Application Team
;* Description : STM32H7xx devices vector table for MDK-ARM toolchain.
;* This module performs:
;* - Set the initial SP
;* - Set the initial PC == Reset_Handler
;* - Set the vector table entries with the exceptions ISR address
;* - Branches to __main in the C library (which eventually
;* calls main()).
;* After Reset the Cortex-M processor is in Thread mode,
;* priority is Privileged, and the Stack is set to Main.
;* <<< Use Configuration Wizard in Context Menu >>>
;******************************************************************************
;* @attention
;*
;* Copyright (c) 2019 STMicroelectronics.
;* All rights reserved.
;*
;* This software is licensed under terms that can be found in the LICENSE file
;* in the root directory of this software component.
;* If no LICENSE file comes with this software, it is provided AS-IS.
;*
;*******************************************************************************
; Amount of memory (in bytes) allocated for Stack
; Tailor this value to your application needs
; <h> Stack Configuration
; <o> Stack Size (in Bytes) <0x0-0xFFFFFFFF:8>
; </h>
Stack_Size EQU 0x2000
AREA STACK, NOINIT, READWRITE, ALIGN=3
Stack_Mem SPACE Stack_Size
__initial_sp
; <h> Heap Configuration
; <o> Heap Size (in Bytes) <0x0-0xFFFFFFFF:8>
; </h>
Heap_Size EQU 0x1000
AREA HEAP, NOINIT, READWRITE, ALIGN=3
__heap_base
Heap_Mem SPACE Heap_Size
__heap_limit
PRESERVE8
THUMB
; Vector Table Mapped to Address 0 at Reset
AREA RESET, DATA, READONLY
EXPORT __Vectors
EXPORT __Vectors_End
EXPORT __Vectors_Size
__Vectors DCD __initial_sp ; Top of Stack
DCD Reset_Handler ; Reset Handler
DCD NMI_Handler ; NMI Handler
DCD HardFault_Handler ; Hard Fault Handler
DCD MemManage_Handler ; MPU Fault Handler
DCD BusFault_Handler ; Bus Fault Handler
DCD UsageFault_Handler ; Usage Fault Handler
DCD 0 ; Reserved
DCD 0 ; Reserved
DCD 0 ; Reserved
DCD 0 ; Reserved
DCD SVC_Handler ; SVCall Handler
DCD DebugMon_Handler ; Debug Monitor Handler
DCD 0 ; Reserved
DCD PendSV_Handler ; PendSV Handler
DCD SysTick_Handler ; SysTick Handler
; External Interrupts
DCD WWDG_IRQHandler ; Window WatchDog interrupt ( wwdg1_it)
DCD PVD_AVD_IRQHandler ; PVD/AVD through EXTI Line detection
DCD TAMP_STAMP_IRQHandler ; Tamper and TimeStamps through the EXTI line
DCD RTC_WKUP_IRQHandler ; RTC Wakeup through the EXTI line
DCD FLASH_IRQHandler ; FLASH
DCD RCC_IRQHandler ; RCC
DCD EXTI0_IRQHandler ; EXTI Line0
DCD EXTI1_IRQHandler ; EXTI Line1
DCD EXTI2_IRQHandler ; EXTI Line2
DCD EXTI3_IRQHandler ; EXTI Line3
DCD EXTI4_IRQHandler ; EXTI Line4
DCD DMA1_Stream0_IRQHandler ; DMA1 Stream 0
DCD DMA1_Stream1_IRQHandler ; DMA1 Stream 1
DCD DMA1_Stream2_IRQHandler ; DMA1 Stream 2
DCD DMA1_Stream3_IRQHandler ; DMA1 Stream 3
DCD DMA1_Stream4_IRQHandler ; DMA1 Stream 4
DCD DMA1_Stream5_IRQHandler ; DMA1 Stream 5
DCD DMA1_Stream6_IRQHandler ; DMA1 Stream 6
DCD ADC_IRQHandler ; ADC1, ADC2
DCD FDCAN1_IT0_IRQHandler ; FDCAN1 interrupt line 0
DCD FDCAN2_IT0_IRQHandler ; FDCAN2 interrupt line 0
DCD FDCAN1_IT1_IRQHandler ; FDCAN1 interrupt line 1
DCD FDCAN2_IT1_IRQHandler ; FDCAN2 interrupt line 1
DCD EXTI9_5_IRQHandler ; External Line[9:5]s
DCD TIM1_BRK_IRQHandler ; TIM1 Break interrupt
DCD TIM1_UP_IRQHandler ; TIM1 Update Interrupt
DCD TIM1_TRG_COM_IRQHandler ; TIM1 Trigger and Commutation Interrupt
DCD TIM1_CC_IRQHandler ; TIM1 Capture Compare
DCD TIM2_IRQHandler ; TIM2
DCD TIM3_IRQHandler ; TIM3
DCD TIM4_IRQHandler ; TIM4
DCD I2C1_EV_IRQHandler ; I2C1 Event
DCD I2C1_ER_IRQHandler ; I2C1 Error
DCD I2C2_EV_IRQHandler ; I2C2 Event
DCD I2C2_ER_IRQHandler ; I2C2 Error
DCD SPI1_IRQHandler ; SPI1
DCD SPI2_IRQHandler ; SPI2
DCD USART1_IRQHandler ; USART1
DCD USART2_IRQHandler ; USART2
DCD USART3_IRQHandler ; USART3
DCD EXTI15_10_IRQHandler ; External Line[15:10]
DCD RTC_Alarm_IRQHandler ; RTC Alarm (A and B) through EXTI Line
DCD 0 ; Reserved
DCD TIM8_BRK_TIM12_IRQHandler ; TIM8 Break Interrupt and TIM12 global interrupt
DCD TIM8_UP_TIM13_IRQHandler ; TIM8 Update Interrupt and TIM13 global interrupt
DCD TIM8_TRG_COM_TIM14_IRQHandler ; TIM8 Trigger and Commutation Interrupt and TIM14 global interrupt
DCD TIM8_CC_IRQHandler ; TIM8 Capture Compare Interrupt
DCD DMA1_Stream7_IRQHandler ; DMA1 Stream7
DCD FMC_IRQHandler ; FMC
DCD SDMMC1_IRQHandler ; SDMMC1
DCD TIM5_IRQHandler ; TIM5
DCD SPI3_IRQHandler ; SPI3
DCD UART4_IRQHandler ; UART4
DCD UART5_IRQHandler ; UART5
DCD TIM6_DAC_IRQHandler ; TIM6 and DAC1&2 underrun errors
DCD TIM7_IRQHandler ; TIM7
DCD DMA2_Stream0_IRQHandler ; DMA2 Stream 0
DCD DMA2_Stream1_IRQHandler ; DMA2 Stream 1
DCD DMA2_Stream2_IRQHandler ; DMA2 Stream 2
DCD DMA2_Stream3_IRQHandler ; DMA2 Stream 3
DCD DMA2_Stream4_IRQHandler ; DMA2 Stream 4
DCD ETH_IRQHandler ; Ethernet
DCD ETH_WKUP_IRQHandler ; Ethernet Wakeup through EXTI line
DCD FDCAN_CAL_IRQHandler ; FDCAN calibration unit interrupt
DCD 0 ; Reserved
DCD 0 ; Reserved
DCD 0 ; Reserved
DCD 0 ; Reserved
DCD DMA2_Stream5_IRQHandler ; DMA2 Stream 5
DCD DMA2_Stream6_IRQHandler ; DMA2 Stream 6
DCD DMA2_Stream7_IRQHandler ; DMA2 Stream 7
DCD USART6_IRQHandler ; USART6
DCD I2C3_EV_IRQHandler ; I2C3 event
DCD I2C3_ER_IRQHandler ; I2C3 error
DCD OTG_HS_EP1_OUT_IRQHandler ; USB OTG HS End Point 1 Out
DCD OTG_HS_EP1_IN_IRQHandler ; USB OTG HS End Point 1 In
DCD OTG_HS_WKUP_IRQHandler ; USB OTG HS Wakeup through EXTI
DCD OTG_HS_IRQHandler ; USB OTG HS
DCD DCMI_PSSI_IRQHandler ; DCMI, PSSI
DCD 0 ; Reserved
DCD RNG_IRQHandler ; Rng
DCD FPU_IRQHandler ; FPU
DCD UART7_IRQHandler ; UART7
DCD UART8_IRQHandler ; UART8
DCD SPI4_IRQHandler ; SPI4
DCD SPI5_IRQHandler ; SPI5
DCD SPI6_IRQHandler ; SPI6
DCD SAI1_IRQHandler ; SAI1
DCD LTDC_IRQHandler ; LTDC
DCD LTDC_ER_IRQHandler ; LTDC error
DCD DMA2D_IRQHandler ; DMA2D
DCD 0 ; Reserved
DCD OCTOSPI1_IRQHandler ; OCTOSPI1
DCD LPTIM1_IRQHandler ; LPTIM1
DCD CEC_IRQHandler ; HDMI_CEC
DCD I2C4_EV_IRQHandler ; I2C4 Event
DCD I2C4_ER_IRQHandler ; I2C4 Error
DCD SPDIF_RX_IRQHandler ; SPDIF_RX
DCD 0 ; Reserved
DCD 0 ; Reserved
DCD 0 ; Reserved
DCD 0 ; Reserved
DCD DMAMUX1_OVR_IRQHandler ; DMAMUX1 Overrun interrupt
DCD 0 ; Reserved
DCD 0 ; Reserved
DCD 0 ; Reserved
DCD 0 ; Reserved
DCD 0 ; Reserved
DCD 0 ; Reserved
DCD 0 ; Reserved
DCD DFSDM1_FLT0_IRQHandler ; DFSDM Filter0 Interrupt
DCD DFSDM1_FLT1_IRQHandler ; DFSDM Filter1 Interrupt
DCD DFSDM1_FLT2_IRQHandler ; DFSDM Filter2 Interrupt
DCD DFSDM1_FLT3_IRQHandler ; DFSDM Filter3 Interrupt
DCD 0 ; Reserved
DCD SWPMI1_IRQHandler ; Serial Wire Interface 1 global interrupt
DCD TIM15_IRQHandler ; TIM15 global Interrupt
DCD TIM16_IRQHandler ; TIM16 global Interrupt
DCD TIM17_IRQHandler ; TIM17 global Interrupt
DCD MDIOS_WKUP_IRQHandler ; MDIOS Wakeup Interrupt
DCD MDIOS_IRQHandler ; MDIOS global Interrupt
DCD 0 ; Reserved
DCD MDMA_IRQHandler ; MDMA global Interrupt
DCD 0 ; Reserved
DCD SDMMC2_IRQHandler ; SDMMC2 global Interrupt
DCD HSEM1_IRQHandler ; HSEM1 global Interrupt
DCD 0 ; Reserved
DCD ADC3_IRQHandler ; ADC3 global Interrupt
DCD DMAMUX2_OVR_IRQHandler ; DMAMUX Overrun interrupt
DCD BDMA_Channel0_IRQHandler ; BDMA Channel 0 global Interrupt
DCD BDMA_Channel1_IRQHandler ; BDMA Channel 1 global Interrupt
DCD BDMA_Channel2_IRQHandler ; BDMA Channel 2 global Interrupt
DCD BDMA_Channel3_IRQHandler ; BDMA Channel 3 global Interrupt
DCD BDMA_Channel4_IRQHandler ; BDMA Channel 4 global Interrupt
DCD BDMA_Channel5_IRQHandler ; BDMA Channel 5 global Interrupt
DCD BDMA_Channel6_IRQHandler ; BDMA Channel 6 global Interrupt
DCD BDMA_Channel7_IRQHandler ; BDMA Channel 7 global Interrupt
DCD COMP1_IRQHandler ; COMP1 global Interrupt
DCD LPTIM2_IRQHandler ; LP TIM2 global interrupt
DCD LPTIM3_IRQHandler ; LP TIM3 global interrupt
DCD LPTIM4_IRQHandler ; LP TIM4 global interrupt
DCD LPTIM5_IRQHandler ; LP TIM5 global interrupt
DCD LPUART1_IRQHandler ; LP UART1 interrupt
DCD 0 ; Reserved
DCD CRS_IRQHandler ; Clock Recovery Global Interrupt
DCD ECC_IRQHandler ; ECC diagnostic Global Interrupt
DCD SAI4_IRQHandler ; SAI4 global interrupt
DCD DTS_IRQHandler ; DTS interrupt
DCD 0 ; Reserved
DCD WAKEUP_PIN_IRQHandler ; Interrupt for all 6 wake-up pins
DCD OCTOSPI2_IRQHandler ; OCTOSPI2 Interrupt
DCD 0 ; Reserved
DCD 0 ; Reserved
DCD FMAC_IRQHandler ; FMAC Interrupt
DCD CORDIC_IRQHandler ; CORDIC Interrupt
DCD UART9_IRQHandler ; UART9 Interrupt
DCD USART10_IRQHandler ; UART10 Interrupt
DCD I2C5_EV_IRQHandler ; I2C5 Event Interrupt
DCD I2C5_ER_IRQHandler ; I2C5 Error Interrupt
DCD FDCAN3_IT0_IRQHandler ; FDCAN3 interrupt line 0
DCD FDCAN3_IT1_IRQHandler ; FDCAN3 interrupt line 1
DCD TIM23_IRQHandler ; TIM23 global interrupt
DCD TIM24_IRQHandler ; TIM24 global interrupt
__Vectors_End
__Vectors_Size EQU __Vectors_End - __Vectors
AREA |.text|, CODE, READONLY
; Reset handler
Reset_Handler PROC
EXPORT Reset_Handler [WEAK]
IMPORT ExitRun0Mode
IMPORT SystemInit
IMPORT __main
LDR R0, =ExitRun0Mode
BLX R0
LDR R0, =SystemInit
BLX R0
LDR R0, =__main
BX R0
ENDP
; Dummy Exception Handlers (infinite loops which can be modified)
NMI_Handler PROC
EXPORT NMI_Handler [WEAK]
B .
ENDP
HardFault_Handler\
PROC
EXPORT HardFault_Handler [WEAK]
B .
ENDP
MemManage_Handler\
PROC
EXPORT MemManage_Handler [WEAK]
B .
ENDP
BusFault_Handler\
PROC
EXPORT BusFault_Handler [WEAK]
B .
ENDP
UsageFault_Handler\
PROC
EXPORT UsageFault_Handler [WEAK]
B .
ENDP
SVC_Handler PROC
EXPORT SVC_Handler [WEAK]
B .
ENDP
DebugMon_Handler\
PROC
EXPORT DebugMon_Handler [WEAK]
B .
ENDP
PendSV_Handler PROC
EXPORT PendSV_Handler [WEAK]
B .
ENDP
SysTick_Handler PROC
EXPORT SysTick_Handler [WEAK]
B .
ENDP
Default_Handler PROC
EXPORT WWDG_IRQHandler [WEAK]
EXPORT PVD_AVD_IRQHandler [WEAK]
EXPORT TAMP_STAMP_IRQHandler [WEAK]
EXPORT RTC_WKUP_IRQHandler [WEAK]
EXPORT FLASH_IRQHandler [WEAK]
EXPORT RCC_IRQHandler [WEAK]
EXPORT EXTI0_IRQHandler [WEAK]
EXPORT EXTI1_IRQHandler [WEAK]
EXPORT EXTI2_IRQHandler [WEAK]
EXPORT EXTI3_IRQHandler [WEAK]
EXPORT EXTI4_IRQHandler [WEAK]
EXPORT DMA1_Stream0_IRQHandler [WEAK]
EXPORT DMA1_Stream1_IRQHandler [WEAK]
EXPORT DMA1_Stream2_IRQHandler [WEAK]
EXPORT DMA1_Stream3_IRQHandler [WEAK]
EXPORT DMA1_Stream4_IRQHandler [WEAK]
EXPORT DMA1_Stream5_IRQHandler [WEAK]
EXPORT DMA1_Stream6_IRQHandler [WEAK]
EXPORT DMA1_Stream7_IRQHandler [WEAK]
EXPORT ADC_IRQHandler [WEAK]
EXPORT FDCAN1_IT0_IRQHandler [WEAK]
EXPORT FDCAN2_IT0_IRQHandler [WEAK]
EXPORT FDCAN1_IT1_IRQHandler [WEAK]
EXPORT FDCAN2_IT1_IRQHandler [WEAK]
EXPORT EXTI9_5_IRQHandler [WEAK]
EXPORT TIM1_BRK_IRQHandler [WEAK]
EXPORT TIM1_UP_IRQHandler [WEAK]
EXPORT TIM1_TRG_COM_IRQHandler [WEAK]
EXPORT TIM1_CC_IRQHandler [WEAK]
EXPORT TIM2_IRQHandler [WEAK]
EXPORT TIM3_IRQHandler [WEAK]
EXPORT TIM4_IRQHandler [WEAK]
EXPORT I2C1_EV_IRQHandler [WEAK]
EXPORT I2C1_ER_IRQHandler [WEAK]
EXPORT I2C2_EV_IRQHandler [WEAK]
EXPORT I2C2_ER_IRQHandler [WEAK]
EXPORT SPI1_IRQHandler [WEAK]
EXPORT SPI2_IRQHandler [WEAK]
EXPORT USART1_IRQHandler [WEAK]
EXPORT USART2_IRQHandler [WEAK]
EXPORT USART3_IRQHandler [WEAK]
EXPORT EXTI15_10_IRQHandler [WEAK]
EXPORT RTC_Alarm_IRQHandler [WEAK]
EXPORT TIM8_BRK_TIM12_IRQHandler [WEAK]
EXPORT TIM8_UP_TIM13_IRQHandler [WEAK]
EXPORT TIM8_TRG_COM_TIM14_IRQHandler [WEAK]
EXPORT TIM8_CC_IRQHandler [WEAK]
EXPORT DMA1_Stream7_IRQHandler [WEAK]
EXPORT FMC_IRQHandler [WEAK]
EXPORT SDMMC1_IRQHandler [WEAK]
EXPORT TIM5_IRQHandler [WEAK]
EXPORT SPI3_IRQHandler [WEAK]
EXPORT UART4_IRQHandler [WEAK]
EXPORT UART5_IRQHandler [WEAK]
EXPORT TIM6_DAC_IRQHandler [WEAK]
EXPORT TIM7_IRQHandler [WEAK]
EXPORT DMA2_Stream0_IRQHandler [WEAK]
EXPORT DMA2_Stream1_IRQHandler [WEAK]
EXPORT DMA2_Stream2_IRQHandler [WEAK]
EXPORT DMA2_Stream3_IRQHandler [WEAK]
EXPORT DMA2_Stream4_IRQHandler [WEAK]
EXPORT ETH_IRQHandler [WEAK]
EXPORT ETH_WKUP_IRQHandler [WEAK]
EXPORT FDCAN_CAL_IRQHandler [WEAK]
EXPORT DMA2_Stream5_IRQHandler [WEAK]
EXPORT DMA2_Stream6_IRQHandler [WEAK]
EXPORT DMA2_Stream7_IRQHandler [WEAK]
EXPORT USART6_IRQHandler [WEAK]
EXPORT I2C3_EV_IRQHandler [WEAK]
EXPORT I2C3_ER_IRQHandler [WEAK]
EXPORT OTG_HS_EP1_OUT_IRQHandler [WEAK]
EXPORT OTG_HS_EP1_IN_IRQHandler [WEAK]
EXPORT OTG_HS_WKUP_IRQHandler [WEAK]
EXPORT OTG_HS_IRQHandler [WEAK]
EXPORT DCMI_PSSI_IRQHandler [WEAK]
EXPORT RNG_IRQHandler [WEAK]
EXPORT FPU_IRQHandler [WEAK]
EXPORT UART7_IRQHandler [WEAK]
EXPORT UART8_IRQHandler [WEAK]
EXPORT SPI4_IRQHandler [WEAK]
EXPORT SPI5_IRQHandler [WEAK]
EXPORT SPI6_IRQHandler [WEAK]
EXPORT SAI1_IRQHandler [WEAK]
EXPORT LTDC_IRQHandler [WEAK]
EXPORT LTDC_ER_IRQHandler [WEAK]
EXPORT DMA2D_IRQHandler [WEAK]
EXPORT OCTOSPI1_IRQHandler [WEAK]
EXPORT LPTIM1_IRQHandler [WEAK]
EXPORT CEC_IRQHandler [WEAK]
EXPORT I2C4_EV_IRQHandler [WEAK]
EXPORT I2C4_ER_IRQHandler [WEAK]
EXPORT SPDIF_RX_IRQHandler [WEAK]
EXPORT DMAMUX1_OVR_IRQHandler [WEAK]
EXPORT DFSDM1_FLT0_IRQHandler [WEAK]
EXPORT DFSDM1_FLT1_IRQHandler [WEAK]
EXPORT DFSDM1_FLT2_IRQHandler [WEAK]
EXPORT DFSDM1_FLT3_IRQHandler [WEAK]
EXPORT SWPMI1_IRQHandler [WEAK]
EXPORT TIM15_IRQHandler [WEAK]
EXPORT TIM16_IRQHandler [WEAK]
EXPORT TIM17_IRQHandler [WEAK]
EXPORT MDIOS_WKUP_IRQHandler [WEAK]
EXPORT MDIOS_IRQHandler [WEAK]
EXPORT MDMA_IRQHandler [WEAK]
EXPORT SDMMC2_IRQHandler [WEAK]
EXPORT HSEM1_IRQHandler [WEAK]
EXPORT ADC3_IRQHandler [WEAK]
EXPORT DMAMUX2_OVR_IRQHandler [WEAK]
EXPORT BDMA_Channel0_IRQHandler [WEAK]
EXPORT BDMA_Channel1_IRQHandler [WEAK]
EXPORT BDMA_Channel2_IRQHandler [WEAK]
EXPORT BDMA_Channel3_IRQHandler [WEAK]
EXPORT BDMA_Channel4_IRQHandler [WEAK]
EXPORT BDMA_Channel5_IRQHandler [WEAK]
EXPORT BDMA_Channel6_IRQHandler [WEAK]
EXPORT BDMA_Channel7_IRQHandler [WEAK]
EXPORT COMP1_IRQHandler [WEAK]
EXPORT LPTIM2_IRQHandler [WEAK]
EXPORT LPTIM3_IRQHandler [WEAK]
EXPORT LPTIM4_IRQHandler [WEAK]
EXPORT LPTIM5_IRQHandler [WEAK]
EXPORT LPUART1_IRQHandler [WEAK]
EXPORT CRS_IRQHandler [WEAK]
EXPORT ECC_IRQHandler [WEAK]
EXPORT SAI4_IRQHandler [WEAK]
EXPORT DTS_IRQHandler [WEAK]
EXPORT WAKEUP_PIN_IRQHandler [WEAK]
EXPORT OCTOSPI2_IRQHandler [WEAK]
EXPORT FMAC_IRQHandler [WEAK]
EXPORT CORDIC_IRQHandler [WEAK]
EXPORT UART9_IRQHandler [WEAK]
EXPORT USART10_IRQHandler [WEAK]
EXPORT I2C5_EV_IRQHandler [WEAK]
EXPORT I2C5_ER_IRQHandler [WEAK]
EXPORT FDCAN3_IT0_IRQHandler [WEAK]
EXPORT FDCAN3_IT1_IRQHandler [WEAK]
EXPORT TIM23_IRQHandler [WEAK]
EXPORT TIM24_IRQHandler [WEAK]
WWDG_IRQHandler
PVD_AVD_IRQHandler
TAMP_STAMP_IRQHandler
RTC_WKUP_IRQHandler
FLASH_IRQHandler
RCC_IRQHandler
EXTI0_IRQHandler
EXTI1_IRQHandler
EXTI2_IRQHandler
EXTI3_IRQHandler
EXTI4_IRQHandler
DMA1_Stream0_IRQHandler
DMA1_Stream1_IRQHandler
DMA1_Stream2_IRQHandler
DMA1_Stream3_IRQHandler
DMA1_Stream4_IRQHandler
DMA1_Stream5_IRQHandler
DMA1_Stream6_IRQHandler
ADC_IRQHandler
FDCAN1_IT0_IRQHandler
FDCAN2_IT0_IRQHandler
FDCAN1_IT1_IRQHandler
FDCAN2_IT1_IRQHandler
EXTI9_5_IRQHandler
TIM1_BRK_IRQHandler
TIM1_UP_IRQHandler
TIM1_TRG_COM_IRQHandler
TIM1_CC_IRQHandler
TIM2_IRQHandler
TIM3_IRQHandler
TIM4_IRQHandler
I2C1_EV_IRQHandler
I2C1_ER_IRQHandler
I2C2_EV_IRQHandler
I2C2_ER_IRQHandler
SPI1_IRQHandler
SPI2_IRQHandler
USART1_IRQHandler
USART2_IRQHandler
USART3_IRQHandler
EXTI15_10_IRQHandler
RTC_Alarm_IRQHandler
TIM8_BRK_TIM12_IRQHandler
TIM8_UP_TIM13_IRQHandler
TIM8_TRG_COM_TIM14_IRQHandler
TIM8_CC_IRQHandler
DMA1_Stream7_IRQHandler
FMC_IRQHandler
SDMMC1_IRQHandler
TIM5_IRQHandler
SPI3_IRQHandler
UART4_IRQHandler
UART5_IRQHandler
TIM6_DAC_IRQHandler
TIM7_IRQHandler
DMA2_Stream0_IRQHandler
DMA2_Stream1_IRQHandler
DMA2_Stream2_IRQHandler
DMA2_Stream3_IRQHandler
DMA2_Stream4_IRQHandler
ETH_IRQHandler
ETH_WKUP_IRQHandler
FDCAN_CAL_IRQHandler
DMA2_Stream5_IRQHandler
DMA2_Stream6_IRQHandler
DMA2_Stream7_IRQHandler
USART6_IRQHandler
I2C3_EV_IRQHandler
I2C3_ER_IRQHandler
OTG_HS_EP1_OUT_IRQHandler
OTG_HS_EP1_IN_IRQHandler
OTG_HS_WKUP_IRQHandler
OTG_HS_IRQHandler
DCMI_PSSI_IRQHandler
RNG_IRQHandler
FPU_IRQHandler
UART7_IRQHandler
UART8_IRQHandler
SPI4_IRQHandler
SPI5_IRQHandler
SPI6_IRQHandler
SAI1_IRQHandler
LTDC_IRQHandler
LTDC_ER_IRQHandler
DMA2D_IRQHandler
OCTOSPI1_IRQHandler
LPTIM1_IRQHandler
CEC_IRQHandler
I2C4_EV_IRQHandler
I2C4_ER_IRQHandler
SPDIF_RX_IRQHandler
DMAMUX1_OVR_IRQHandler
DFSDM1_FLT0_IRQHandler
DFSDM1_FLT1_IRQHandler
DFSDM1_FLT2_IRQHandler
DFSDM1_FLT3_IRQHandler
SWPMI1_IRQHandler
TIM15_IRQHandler
TIM16_IRQHandler
TIM17_IRQHandler
MDIOS_WKUP_IRQHandler
MDIOS_IRQHandler
MDMA_IRQHandler
SDMMC2_IRQHandler
HSEM1_IRQHandler
ADC3_IRQHandler
DMAMUX2_OVR_IRQHandler
BDMA_Channel0_IRQHandler
BDMA_Channel1_IRQHandler
BDMA_Channel2_IRQHandler
BDMA_Channel3_IRQHandler
BDMA_Channel4_IRQHandler
BDMA_Channel5_IRQHandler
BDMA_Channel6_IRQHandler
BDMA_Channel7_IRQHandler
COMP1_IRQHandler
LPTIM2_IRQHandler
LPTIM3_IRQHandler
LPTIM4_IRQHandler
LPTIM5_IRQHandler
LPUART1_IRQHandler
CRS_IRQHandler
ECC_IRQHandler
SAI4_IRQHandler
DTS_IRQHandler
WAKEUP_PIN_IRQHandler
OCTOSPI2_IRQHandler
FMAC_IRQHandler
CORDIC_IRQHandler
UART9_IRQHandler
USART10_IRQHandler
I2C5_EV_IRQHandler
I2C5_ER_IRQHandler
FDCAN3_IT0_IRQHandler
FDCAN3_IT1_IRQHandler
TIM23_IRQHandler
TIM24_IRQHandler
B .
ENDP
ALIGN
;*******************************************************************************
; User Stack and Heap initialization
;*******************************************************************************
IF :DEF:__MICROLIB
EXPORT __initial_sp
EXPORT __heap_base
EXPORT __heap_limit
ELSE
IMPORT __use_two_region_memory
EXPORT __user_initial_stackheap
__user_initial_stackheap
LDR R0, = Heap_Mem
LDR R1, =(Stack_Mem + Stack_Size)
LDR R2, = (Heap_Mem + Heap_Size)
LDR R3, = Stack_Mem
BX LR
ALIGN
ENDIF
END

775
Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F/port.c vendored Normal file
View File

@ -0,0 +1,775 @@
/*
* FreeRTOS Kernel V10.3.1
* Copyright (C) 2020 Amazon.com, Inc. or its affiliates. All Rights Reserved.
*
* Permission is hereby granted, free of charge, to any person obtaining a copy of
* this software and associated documentation files (the "Software"), to deal in
* the Software without restriction, including without limitation the rights to
* use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
* the Software, and to permit persons to whom the Software is furnished to do so,
* subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
* FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
* COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
* IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*
* http://www.FreeRTOS.org
* http://aws.amazon.com/freertos
*
* 1 tab == 4 spaces!
*/
/*-----------------------------------------------------------
* Implementation of functions defined in portable.h for the ARM CM4F port.
*----------------------------------------------------------*/
/* Scheduler includes. */
#include "FreeRTOS.h"
#include "task.h"
#ifndef __VFP_FP__
#error This port can only be used when the project options are configured to enable hardware floating point support.
#endif
#ifndef configSYSTICK_CLOCK_HZ
#define configSYSTICK_CLOCK_HZ configCPU_CLOCK_HZ
/* Ensure the SysTick is clocked at the same frequency as the core. */
#define portNVIC_SYSTICK_CLK_BIT ( 1UL << 2UL )
#else
/* The way the SysTick is clocked is not modified in case it is not the same
as the core. */
#define portNVIC_SYSTICK_CLK_BIT ( 0 )
#endif
/* Constants required to manipulate the core. Registers first... */
#define portNVIC_SYSTICK_CTRL_REG ( * ( ( volatile uint32_t * ) 0xe000e010 ) )
#define portNVIC_SYSTICK_LOAD_REG ( * ( ( volatile uint32_t * ) 0xe000e014 ) )
#define portNVIC_SYSTICK_CURRENT_VALUE_REG ( * ( ( volatile uint32_t * ) 0xe000e018 ) )
#define portNVIC_SYSPRI2_REG ( * ( ( volatile uint32_t * ) 0xe000ed20 ) )
/* ...then bits in the registers. */
#define portNVIC_SYSTICK_INT_BIT ( 1UL << 1UL )
#define portNVIC_SYSTICK_ENABLE_BIT ( 1UL << 0UL )
#define portNVIC_SYSTICK_COUNT_FLAG_BIT ( 1UL << 16UL )
#define portNVIC_PENDSVCLEAR_BIT ( 1UL << 27UL )
#define portNVIC_PEND_SYSTICK_CLEAR_BIT ( 1UL << 25UL )
/* Constants used to detect a Cortex-M7 r0p1 core, which should use the ARM_CM7
r0p1 port. */
#define portCPUID ( * ( ( volatile uint32_t * ) 0xE000ed00 ) )
#define portCORTEX_M7_r0p1_ID ( 0x410FC271UL )
#define portCORTEX_M7_r0p0_ID ( 0x410FC270UL )
#define portNVIC_PENDSV_PRI ( ( ( uint32_t ) configKERNEL_INTERRUPT_PRIORITY ) << 16UL )
#define portNVIC_SYSTICK_PRI ( ( ( uint32_t ) configKERNEL_INTERRUPT_PRIORITY ) << 24UL )
/* Constants required to check the validity of an interrupt priority. */
#define portFIRST_USER_INTERRUPT_NUMBER ( 16 )
#define portNVIC_IP_REGISTERS_OFFSET_16 ( 0xE000E3F0 )
#define portAIRCR_REG ( * ( ( volatile uint32_t * ) 0xE000ED0C ) )
#define portMAX_8_BIT_VALUE ( ( uint8_t ) 0xff )
#define portTOP_BIT_OF_BYTE ( ( uint8_t ) 0x80 )
#define portMAX_PRIGROUP_BITS ( ( uint8_t ) 7 )
#define portPRIORITY_GROUP_MASK ( 0x07UL << 8UL )
#define portPRIGROUP_SHIFT ( 8UL )
/* Masks off all bits but the VECTACTIVE bits in the ICSR register. */
#define portVECTACTIVE_MASK ( 0xFFUL )
/* Constants required to manipulate the VFP. */
#define portFPCCR ( ( volatile uint32_t * ) 0xe000ef34 ) /* Floating point context control register. */
#define portASPEN_AND_LSPEN_BITS ( 0x3UL << 30UL )
/* Constants required to set up the initial stack. */
#define portINITIAL_XPSR ( 0x01000000 )
#define portINITIAL_EXC_RETURN ( 0xfffffffd )
/* The systick is a 24-bit counter. */
#define portMAX_24_BIT_NUMBER ( 0xffffffUL )
/* For strict compliance with the Cortex-M spec the task start address should
have bit-0 clear, as it is loaded into the PC on exit from an ISR. */
#define portSTART_ADDRESS_MASK ( ( StackType_t ) 0xfffffffeUL )
/* A fiddle factor to estimate the number of SysTick counts that would have
occurred while the SysTick counter is stopped during tickless idle
calculations. */
#define portMISSED_COUNTS_FACTOR ( 45UL )
/* Let the user override the pre-loading of the initial LR with the address of
prvTaskExitError() in case it messes up unwinding of the stack in the
debugger. */
#ifdef configTASK_RETURN_ADDRESS
#define portTASK_RETURN_ADDRESS configTASK_RETURN_ADDRESS
#else
#define portTASK_RETURN_ADDRESS prvTaskExitError
#endif
/*
* Setup the timer to generate the tick interrupts. The implementation in this
* file is weak to allow application writers to change the timer used to
* generate the tick interrupt.
*/
void vPortSetupTimerInterrupt( void );
/*
* Exception handlers.
*/
void xPortPendSVHandler( void ) __attribute__ (( naked ));
void xPortSysTickHandler( void );
void vPortSVCHandler( void ) __attribute__ (( naked ));
/*
* Start first task is a separate function so it can be tested in isolation.
*/
static void prvPortStartFirstTask( void ) __attribute__ (( naked ));
/*
* Function to enable the VFP.
*/
static void vPortEnableVFP( void ) __attribute__ (( naked ));
/*
* Used to catch tasks that attempt to return from their implementing function.
*/
static void prvTaskExitError( void );
/*-----------------------------------------------------------*/
/* Each task maintains its own interrupt status in the critical nesting
variable. */
static UBaseType_t uxCriticalNesting = 0xaaaaaaaa;
/*
* The number of SysTick increments that make up one tick period.
*/
#if( configUSE_TICKLESS_IDLE == 1 )
static uint32_t ulTimerCountsForOneTick = 0;
#endif /* configUSE_TICKLESS_IDLE */
/*
* The maximum number of tick periods that can be suppressed is limited by the
* 24 bit resolution of the SysTick timer.
*/
#if( configUSE_TICKLESS_IDLE == 1 )
static uint32_t xMaximumPossibleSuppressedTicks = 0;
#endif /* configUSE_TICKLESS_IDLE */
/*
* Compensate for the CPU cycles that pass while the SysTick is stopped (low
* power functionality only.
*/
#if( configUSE_TICKLESS_IDLE == 1 )
static uint32_t ulStoppedTimerCompensation = 0;
#endif /* configUSE_TICKLESS_IDLE */
/*
* Used by the portASSERT_IF_INTERRUPT_PRIORITY_INVALID() macro to ensure
* FreeRTOS API functions are not called from interrupts that have been assigned
* a priority above configMAX_SYSCALL_INTERRUPT_PRIORITY.
*/
#if( configASSERT_DEFINED == 1 )
static uint8_t ucMaxSysCallPriority = 0;
static uint32_t ulMaxPRIGROUPValue = 0;
static const volatile uint8_t * const pcInterruptPriorityRegisters = ( const volatile uint8_t * const ) portNVIC_IP_REGISTERS_OFFSET_16;
#endif /* configASSERT_DEFINED */
/*-----------------------------------------------------------*/
/*
* See header file for description.
*/
StackType_t *pxPortInitialiseStack( StackType_t *pxTopOfStack, TaskFunction_t pxCode, void *pvParameters )
{
/* Simulate the stack frame as it would be created by a context switch
interrupt. */
/* Offset added to account for the way the MCU uses the stack on entry/exit
of interrupts, and to ensure alignment. */
pxTopOfStack--;
*pxTopOfStack = portINITIAL_XPSR; /* xPSR */
pxTopOfStack--;
*pxTopOfStack = ( ( StackType_t ) pxCode ) & portSTART_ADDRESS_MASK; /* PC */
pxTopOfStack--;
*pxTopOfStack = ( StackType_t ) portTASK_RETURN_ADDRESS; /* LR */
/* Save code space by skipping register initialisation. */
pxTopOfStack -= 5; /* R12, R3, R2 and R1. */
*pxTopOfStack = ( StackType_t ) pvParameters; /* R0 */
/* A save method is being used that requires each task to maintain its
own exec return value. */
pxTopOfStack--;
*pxTopOfStack = portINITIAL_EXC_RETURN;
pxTopOfStack -= 8; /* R11, R10, R9, R8, R7, R6, R5 and R4. */
return pxTopOfStack;
}
/*-----------------------------------------------------------*/
static void prvTaskExitError( void )
{
volatile uint32_t ulDummy = 0;
/* A function that implements a task must not exit or attempt to return to
its caller as there is nothing to return to. If a task wants to exit it
should instead call vTaskDelete( NULL ).
Artificially force an assert() to be triggered if configASSERT() is
defined, then stop here so application writers can catch the error. */
configASSERT( uxCriticalNesting == ~0UL );
portDISABLE_INTERRUPTS();
while( ulDummy == 0 )
{
/* This file calls prvTaskExitError() after the scheduler has been
started to remove a compiler warning about the function being defined
but never called. ulDummy is used purely to quieten other warnings
about code appearing after this function is called - making ulDummy
volatile makes the compiler think the function could return and
therefore not output an 'unreachable code' warning for code that appears
after it. */
}
}
/*-----------------------------------------------------------*/
void vPortSVCHandler( void )
{
__asm volatile (
" ldr r3, pxCurrentTCBConst2 \n" /* Restore the context. */
" ldr r1, [r3] \n" /* Use pxCurrentTCBConst to get the pxCurrentTCB address. */
" ldr r0, [r1] \n" /* The first item in pxCurrentTCB is the task top of stack. */
" ldmia r0!, {r4-r11, r14} \n" /* Pop the registers that are not automatically saved on exception entry and the critical nesting count. */
" msr psp, r0 \n" /* Restore the task stack pointer. */
" isb \n"
" mov r0, #0 \n"
" msr basepri, r0 \n"
" bx r14 \n"
" \n"
" .align 4 \n"
"pxCurrentTCBConst2: .word pxCurrentTCB \n"
);
}
/*-----------------------------------------------------------*/
static void prvPortStartFirstTask( void )
{
/* Start the first task. This also clears the bit that indicates the FPU is
in use in case the FPU was used before the scheduler was started - which
would otherwise result in the unnecessary leaving of space in the SVC stack
for lazy saving of FPU registers. */
__asm volatile(
" ldr r0, =0xE000ED08 \n" /* Use the NVIC offset register to locate the stack. */
" ldr r0, [r0] \n"
" ldr r0, [r0] \n"
" msr msp, r0 \n" /* Set the msp back to the start of the stack. */
" mov r0, #0 \n" /* Clear the bit that indicates the FPU is in use, see comment above. */
" msr control, r0 \n"
" cpsie i \n" /* Globally enable interrupts. */
" cpsie f \n"
" dsb \n"
" isb \n"
" svc 0 \n" /* System call to start first task. */
" nop \n"
);
}
/*-----------------------------------------------------------*/
/*
* See header file for description.
*/
BaseType_t xPortStartScheduler( void )
{
/* configMAX_SYSCALL_INTERRUPT_PRIORITY must not be set to 0.
See http://www.FreeRTOS.org/RTOS-Cortex-M3-M4.html */
configASSERT( configMAX_SYSCALL_INTERRUPT_PRIORITY );
/* This port can be used on all revisions of the Cortex-M7 core other than
the r0p1 parts. r0p1 parts should use the port from the
/source/portable/GCC/ARM_CM7/r0p1 directory. */
configASSERT( portCPUID != portCORTEX_M7_r0p1_ID );
configASSERT( portCPUID != portCORTEX_M7_r0p0_ID );
#if( configASSERT_DEFINED == 1 )
{
volatile uint32_t ulOriginalPriority;
volatile uint8_t * const pucFirstUserPriorityRegister = ( volatile uint8_t * const ) ( portNVIC_IP_REGISTERS_OFFSET_16 + portFIRST_USER_INTERRUPT_NUMBER );
volatile uint8_t ucMaxPriorityValue;
/* Determine the maximum priority from which ISR safe FreeRTOS API
functions can be called. ISR safe functions are those that end in
"FromISR". FreeRTOS maintains separate thread and ISR API functions to
ensure interrupt entry is as fast and simple as possible.
Save the interrupt priority value that is about to be clobbered. */
ulOriginalPriority = *pucFirstUserPriorityRegister;
/* Determine the number of priority bits available. First write to all
possible bits. */
*pucFirstUserPriorityRegister = portMAX_8_BIT_VALUE;
/* Read the value back to see how many bits stuck. */
ucMaxPriorityValue = *pucFirstUserPriorityRegister;
/* Use the same mask on the maximum system call priority. */
ucMaxSysCallPriority = configMAX_SYSCALL_INTERRUPT_PRIORITY & ucMaxPriorityValue;
/* Calculate the maximum acceptable priority group value for the number
of bits read back. */
ulMaxPRIGROUPValue = portMAX_PRIGROUP_BITS;
while( ( ucMaxPriorityValue & portTOP_BIT_OF_BYTE ) == portTOP_BIT_OF_BYTE )
{
ulMaxPRIGROUPValue--;
ucMaxPriorityValue <<= ( uint8_t ) 0x01;
}
#ifdef __NVIC_PRIO_BITS
{
/* Check the CMSIS configuration that defines the number of
priority bits matches the number of priority bits actually queried
from the hardware. */
configASSERT( ( portMAX_PRIGROUP_BITS - ulMaxPRIGROUPValue ) == __NVIC_PRIO_BITS );
}
#endif
#ifdef configPRIO_BITS
{
/* Check the FreeRTOS configuration that defines the number of
priority bits matches the number of priority bits actually queried
from the hardware. */
configASSERT( ( portMAX_PRIGROUP_BITS - ulMaxPRIGROUPValue ) == configPRIO_BITS );
}
#endif
/* Shift the priority group value back to its position within the AIRCR
register. */
ulMaxPRIGROUPValue <<= portPRIGROUP_SHIFT;
ulMaxPRIGROUPValue &= portPRIORITY_GROUP_MASK;
/* Restore the clobbered interrupt priority register to its original
value. */
*pucFirstUserPriorityRegister = ulOriginalPriority;
}
#endif /* conifgASSERT_DEFINED */
/* Make PendSV and SysTick the lowest priority interrupts. */
portNVIC_SYSPRI2_REG |= portNVIC_PENDSV_PRI;
portNVIC_SYSPRI2_REG |= portNVIC_SYSTICK_PRI;
/* Start the timer that generates the tick ISR. Interrupts are disabled
here already. */
vPortSetupTimerInterrupt();
/* Initialise the critical nesting count ready for the first task. */
uxCriticalNesting = 0;
/* Ensure the VFP is enabled - it should be anyway. */
vPortEnableVFP();
/* Lazy save always. */
*( portFPCCR ) |= portASPEN_AND_LSPEN_BITS;
/* Start the first task. */
prvPortStartFirstTask();
/* Should never get here as the tasks will now be executing! Call the task
exit error function to prevent compiler warnings about a static function
not being called in the case that the application writer overrides this
functionality by defining configTASK_RETURN_ADDRESS. Call
vTaskSwitchContext() so link time optimisation does not remove the
symbol. */
vTaskSwitchContext();
prvTaskExitError();
/* Should not get here! */
return 0;
}
/*-----------------------------------------------------------*/
void vPortEndScheduler( void )
{
/* Not implemented in ports where there is nothing to return to.
Artificially force an assert. */
configASSERT( uxCriticalNesting == 1000UL );
}
/*-----------------------------------------------------------*/
void vPortEnterCritical( void )
{
portDISABLE_INTERRUPTS();
uxCriticalNesting++;
/* This is not the interrupt safe version of the enter critical function so
assert() if it is being called from an interrupt context. Only API
functions that end in "FromISR" can be used in an interrupt. Only assert if
the critical nesting count is 1 to protect against recursive calls if the
assert function also uses a critical section. */
if( uxCriticalNesting == 1 )
{
configASSERT( ( portNVIC_INT_CTRL_REG & portVECTACTIVE_MASK ) == 0 );
}
}
/*-----------------------------------------------------------*/
void vPortExitCritical( void )
{
configASSERT( uxCriticalNesting );
uxCriticalNesting--;
if( uxCriticalNesting == 0 )
{
portENABLE_INTERRUPTS();
}
}
/*-----------------------------------------------------------*/
void xPortPendSVHandler( void )
{
/* This is a naked function. */
__asm volatile
(
" mrs r0, psp \n"
" isb \n"
" \n"
" ldr r3, pxCurrentTCBConst \n" /* Get the location of the current TCB. */
" ldr r2, [r3] \n"
" \n"
" tst r14, #0x10 \n" /* Is the task using the FPU context? If so, push high vfp registers. */
" it eq \n"
" vstmdbeq r0!, {s16-s31} \n"
" \n"
" stmdb r0!, {r4-r11, r14} \n" /* Save the core registers. */
" str r0, [r2] \n" /* Save the new top of stack into the first member of the TCB. */
" \n"
" stmdb sp!, {r0, r3} \n"
" mov r0, %0 \n"
" msr basepri, r0 \n"
" dsb \n"
" isb \n"
" bl vTaskSwitchContext \n"
" mov r0, #0 \n"
" msr basepri, r0 \n"
" ldmia sp!, {r0, r3} \n"
" \n"
" ldr r1, [r3] \n" /* The first item in pxCurrentTCB is the task top of stack. */
" ldr r0, [r1] \n"
" \n"
" ldmia r0!, {r4-r11, r14} \n" /* Pop the core registers. */
" \n"
" tst r14, #0x10 \n" /* Is the task using the FPU context? If so, pop the high vfp registers too. */
" it eq \n"
" vldmiaeq r0!, {s16-s31} \n"
" \n"
" msr psp, r0 \n"
" isb \n"
" \n"
#ifdef WORKAROUND_PMU_CM001 /* XMC4000 specific errata workaround. */
#if WORKAROUND_PMU_CM001 == 1
" push { r14 } \n"
" pop { pc } \n"
#endif
#endif
" \n"
" bx r14 \n"
" \n"
" .align 4 \n"
"pxCurrentTCBConst: .word pxCurrentTCB \n"
::"i"(configMAX_SYSCALL_INTERRUPT_PRIORITY)
);
}
/*-----------------------------------------------------------*/
void xPortSysTickHandler( void )
{
/* The SysTick runs at the lowest interrupt priority, so when this interrupt
executes all interrupts must be unmasked. There is therefore no need to
save and then restore the interrupt mask value as its value is already
known. */
portDISABLE_INTERRUPTS();
{
/* Increment the RTOS tick. */
if( xTaskIncrementTick() != pdFALSE )
{
/* A context switch is required. Context switching is performed in
the PendSV interrupt. Pend the PendSV interrupt. */
portNVIC_INT_CTRL_REG = portNVIC_PENDSVSET_BIT;
}
}
portENABLE_INTERRUPTS();
}
/*-----------------------------------------------------------*/
#if( configUSE_TICKLESS_IDLE == 1 )
__attribute__((weak)) void vPortSuppressTicksAndSleep( TickType_t xExpectedIdleTime )
{
uint32_t ulReloadValue, ulCompleteTickPeriods, ulCompletedSysTickDecrements;
TickType_t xModifiableIdleTime;
/* Make sure the SysTick reload value does not overflow the counter. */
if( xExpectedIdleTime > xMaximumPossibleSuppressedTicks )
{
xExpectedIdleTime = xMaximumPossibleSuppressedTicks;
}
/* Stop the SysTick momentarily. The time the SysTick is stopped for
is accounted for as best it can be, but using the tickless mode will
inevitably result in some tiny drift of the time maintained by the
kernel with respect to calendar time. */
portNVIC_SYSTICK_CTRL_REG &= ~portNVIC_SYSTICK_ENABLE_BIT;
/* Calculate the reload value required to wait xExpectedIdleTime
tick periods. -1 is used because this code will execute part way
through one of the tick periods. */
ulReloadValue = portNVIC_SYSTICK_CURRENT_VALUE_REG + ( ulTimerCountsForOneTick * ( xExpectedIdleTime - 1UL ) );
if( ulReloadValue > ulStoppedTimerCompensation )
{
ulReloadValue -= ulStoppedTimerCompensation;
}
/* Enter a critical section but don't use the taskENTER_CRITICAL()
method as that will mask interrupts that should exit sleep mode. */
__asm volatile( "cpsid i" ::: "memory" );
__asm volatile( "dsb" );
__asm volatile( "isb" );
/* If a context switch is pending or a task is waiting for the scheduler
to be unsuspended then abandon the low power entry. */
if( eTaskConfirmSleepModeStatus() == eAbortSleep )
{
/* Restart from whatever is left in the count register to complete
this tick period. */
portNVIC_SYSTICK_LOAD_REG = portNVIC_SYSTICK_CURRENT_VALUE_REG;
/* Restart SysTick. */
portNVIC_SYSTICK_CTRL_REG |= portNVIC_SYSTICK_ENABLE_BIT;
/* Reset the reload register to the value required for normal tick
periods. */
portNVIC_SYSTICK_LOAD_REG = ulTimerCountsForOneTick - 1UL;
/* Re-enable interrupts - see comments above the cpsid instruction()
above. */
__asm volatile( "cpsie i" ::: "memory" );
}
else
{
/* Set the new reload value. */
portNVIC_SYSTICK_LOAD_REG = ulReloadValue;
/* Clear the SysTick count flag and set the count value back to
zero. */
portNVIC_SYSTICK_CURRENT_VALUE_REG = 0UL;
/* Restart SysTick. */
portNVIC_SYSTICK_CTRL_REG |= portNVIC_SYSTICK_ENABLE_BIT;
/* Sleep until something happens. configPRE_SLEEP_PROCESSING() can
set its parameter to 0 to indicate that its implementation contains
its own wait for interrupt or wait for event instruction, and so wfi
should not be executed again. However, the original expected idle
time variable must remain unmodified, so a copy is taken. */
xModifiableIdleTime = xExpectedIdleTime;
configPRE_SLEEP_PROCESSING( xModifiableIdleTime );
if( xModifiableIdleTime > 0 )
{
__asm volatile( "dsb" ::: "memory" );
__asm volatile( "wfi" );
__asm volatile( "isb" );
}
configPOST_SLEEP_PROCESSING( xExpectedIdleTime );
/* Re-enable interrupts to allow the interrupt that brought the MCU
out of sleep mode to execute immediately. see comments above
__disable_interrupt() call above. */
__asm volatile( "cpsie i" ::: "memory" );
__asm volatile( "dsb" );
__asm volatile( "isb" );
/* Disable interrupts again because the clock is about to be stopped
and interrupts that execute while the clock is stopped will increase
any slippage between the time maintained by the RTOS and calendar
time. */
__asm volatile( "cpsid i" ::: "memory" );
__asm volatile( "dsb" );
__asm volatile( "isb" );
/* Disable the SysTick clock without reading the
portNVIC_SYSTICK_CTRL_REG register to ensure the
portNVIC_SYSTICK_COUNT_FLAG_BIT is not cleared if it is set. Again,
the time the SysTick is stopped for is accounted for as best it can
be, but using the tickless mode will inevitably result in some tiny
drift of the time maintained by the kernel with respect to calendar
time*/
portNVIC_SYSTICK_CTRL_REG = ( portNVIC_SYSTICK_CLK_BIT | portNVIC_SYSTICK_INT_BIT );
/* Determine if the SysTick clock has already counted to zero and
been set back to the current reload value (the reload back being
correct for the entire expected idle time) or if the SysTick is yet
to count to zero (in which case an interrupt other than the SysTick
must have brought the system out of sleep mode). */
if( ( portNVIC_SYSTICK_CTRL_REG & portNVIC_SYSTICK_COUNT_FLAG_BIT ) != 0 )
{
uint32_t ulCalculatedLoadValue;
/* The tick interrupt is already pending, and the SysTick count
reloaded with ulReloadValue. Reset the
portNVIC_SYSTICK_LOAD_REG with whatever remains of this tick
period. */
ulCalculatedLoadValue = ( ulTimerCountsForOneTick - 1UL ) - ( ulReloadValue - portNVIC_SYSTICK_CURRENT_VALUE_REG );
/* Don't allow a tiny value, or values that have somehow
underflowed because the post sleep hook did something
that took too long. */
if( ( ulCalculatedLoadValue < ulStoppedTimerCompensation ) || ( ulCalculatedLoadValue > ulTimerCountsForOneTick ) )
{
ulCalculatedLoadValue = ( ulTimerCountsForOneTick - 1UL );
}
portNVIC_SYSTICK_LOAD_REG = ulCalculatedLoadValue;
/* As the pending tick will be processed as soon as this
function exits, the tick value maintained by the tick is stepped
forward by one less than the time spent waiting. */
ulCompleteTickPeriods = xExpectedIdleTime - 1UL;
}
else
{
/* Something other than the tick interrupt ended the sleep.
Work out how long the sleep lasted rounded to complete tick
periods (not the ulReload value which accounted for part
ticks). */
ulCompletedSysTickDecrements = ( xExpectedIdleTime * ulTimerCountsForOneTick ) - portNVIC_SYSTICK_CURRENT_VALUE_REG;
/* How many complete tick periods passed while the processor
was waiting? */
ulCompleteTickPeriods = ulCompletedSysTickDecrements / ulTimerCountsForOneTick;
/* The reload value is set to whatever fraction of a single tick
period remains. */
portNVIC_SYSTICK_LOAD_REG = ( ( ulCompleteTickPeriods + 1UL ) * ulTimerCountsForOneTick ) - ulCompletedSysTickDecrements;
}
/* Restart SysTick so it runs from portNVIC_SYSTICK_LOAD_REG
again, then set portNVIC_SYSTICK_LOAD_REG back to its standard
value. */
portNVIC_SYSTICK_CURRENT_VALUE_REG = 0UL;
portNVIC_SYSTICK_CTRL_REG |= portNVIC_SYSTICK_ENABLE_BIT;
vTaskStepTick( ulCompleteTickPeriods );
portNVIC_SYSTICK_LOAD_REG = ulTimerCountsForOneTick - 1UL;
/* Exit with interrupts enabled. */
__asm volatile( "cpsie i" ::: "memory" );
}
}
#endif /* #if configUSE_TICKLESS_IDLE */
/*-----------------------------------------------------------*/
/*
* Setup the systick timer to generate the tick interrupts at the required
* frequency.
*/
__attribute__(( weak )) void vPortSetupTimerInterrupt( void )
{
/* Calculate the constants required to configure the tick interrupt. */
#if( configUSE_TICKLESS_IDLE == 1 )
{
ulTimerCountsForOneTick = ( configSYSTICK_CLOCK_HZ / configTICK_RATE_HZ );
xMaximumPossibleSuppressedTicks = portMAX_24_BIT_NUMBER / ulTimerCountsForOneTick;
ulStoppedTimerCompensation = portMISSED_COUNTS_FACTOR / ( configCPU_CLOCK_HZ / configSYSTICK_CLOCK_HZ );
}
#endif /* configUSE_TICKLESS_IDLE */
/* Stop and clear the SysTick. */
portNVIC_SYSTICK_CTRL_REG = 0UL;
portNVIC_SYSTICK_CURRENT_VALUE_REG = 0UL;
/* Configure SysTick to interrupt at the requested rate. */
portNVIC_SYSTICK_LOAD_REG = ( configSYSTICK_CLOCK_HZ / configTICK_RATE_HZ ) - 1UL;
portNVIC_SYSTICK_CTRL_REG = ( portNVIC_SYSTICK_CLK_BIT | portNVIC_SYSTICK_INT_BIT | portNVIC_SYSTICK_ENABLE_BIT );
}
/*-----------------------------------------------------------*/
/* This is a naked function. */
static void vPortEnableVFP( void )
{
__asm volatile
(
" ldr.w r0, =0xE000ED88 \n" /* The FPU enable bits are in the CPACR. */
" ldr r1, [r0] \n"
" \n"
" orr r1, r1, #( 0xf << 20 ) \n" /* Enable CP10 and CP11 coprocessors, then save back. */
" str r1, [r0] \n"
" bx r14 "
);
}
/*-----------------------------------------------------------*/
#if( configASSERT_DEFINED == 1 )
void vPortValidateInterruptPriority( void )
{
uint32_t ulCurrentInterrupt;
uint8_t ucCurrentPriority;
/* Obtain the number of the currently executing interrupt. */
__asm volatile( "mrs %0, ipsr" : "=r"( ulCurrentInterrupt ) :: "memory" );
/* Is the interrupt number a user defined interrupt? */
if( ulCurrentInterrupt >= portFIRST_USER_INTERRUPT_NUMBER )
{
/* Look up the interrupt's priority. */
ucCurrentPriority = pcInterruptPriorityRegisters[ ulCurrentInterrupt ];
/* The following assertion will fail if a service routine (ISR) for
an interrupt that has been assigned a priority above
configMAX_SYSCALL_INTERRUPT_PRIORITY calls an ISR safe FreeRTOS API
function. ISR safe FreeRTOS API functions must *only* be called
from interrupts that have been assigned a priority at or below
configMAX_SYSCALL_INTERRUPT_PRIORITY.
Numerically low interrupt priority numbers represent logically high
interrupt priorities, therefore the priority of the interrupt must
be set to a value equal to or numerically *higher* than
configMAX_SYSCALL_INTERRUPT_PRIORITY.
Interrupts that use the FreeRTOS API must not be left at their
default priority of zero as that is the highest possible priority,
which is guaranteed to be above configMAX_SYSCALL_INTERRUPT_PRIORITY,
and therefore also guaranteed to be invalid.
FreeRTOS maintains separate thread and ISR API functions to ensure
interrupt entry is as fast and simple as possible.
The following links provide detailed information:
http://www.freertos.org/RTOS-Cortex-M3-M4.html
http://www.freertos.org/FAQHelp.html */
configASSERT( ucCurrentPriority >= ucMaxSysCallPriority );
}
/* Priority grouping: The interrupt controller (NVIC) allows the bits
that define each interrupt's priority to be split between bits that
define the interrupt's pre-emption priority bits and bits that define
the interrupt's sub-priority. For simplicity all bits must be defined
to be pre-emption priority bits. The following assertion will fail if
this is not the case (if some bits represent a sub-priority).
If the application only uses CMSIS libraries for interrupt
configuration then the correct setting can be achieved on all Cortex-M
devices by calling NVIC_SetPriorityGrouping( 0 ); before starting the
scheduler. Note however that some vendor specific peripheral libraries
assume a non-zero priority group setting, in which cases using a value
of zero will result in unpredictable behaviour. */
configASSERT( ( portAIRCR_REG & portPRIORITY_GROUP_MASK ) <= ulMaxPRIGROUPValue );
}
#endif /* configASSERT_DEFINED */

243
Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F/portmacro.h vendored Normal file
View File

@ -0,0 +1,243 @@
/*
* FreeRTOS Kernel V10.3.1
* Copyright (C) 2020 Amazon.com, Inc. or its affiliates. All Rights Reserved.
*
* Permission is hereby granted, free of charge, to any person obtaining a copy of
* this software and associated documentation files (the "Software"), to deal in
* the Software without restriction, including without limitation the rights to
* use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
* the Software, and to permit persons to whom the Software is furnished to do so,
* subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
* FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
* COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
* IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*
* http://www.FreeRTOS.org
* http://aws.amazon.com/freertos
*
* 1 tab == 4 spaces!
*/
#ifndef PORTMACRO_H
#define PORTMACRO_H
#ifdef __cplusplus
extern "C" {
#endif
/*-----------------------------------------------------------
* Port specific definitions.
*
* The settings in this file configure FreeRTOS correctly for the
* given hardware and compiler.
*
* These settings should not be altered.
*-----------------------------------------------------------
*/
/* Type definitions. */
#define portCHAR char
#define portFLOAT float
#define portDOUBLE double
#define portLONG long
#define portSHORT short
#define portSTACK_TYPE uint32_t
#define portBASE_TYPE long
typedef portSTACK_TYPE StackType_t;
typedef long BaseType_t;
typedef unsigned long UBaseType_t;
#if( configUSE_16_BIT_TICKS == 1 )
typedef uint16_t TickType_t;
#define portMAX_DELAY ( TickType_t ) 0xffff
#else
typedef uint32_t TickType_t;
#define portMAX_DELAY ( TickType_t ) 0xffffffffUL
/* 32-bit tick type on a 32-bit architecture, so reads of the tick count do
not need to be guarded with a critical section. */
#define portTICK_TYPE_IS_ATOMIC 1
#endif
/*-----------------------------------------------------------*/
/* Architecture specifics. */
#define portSTACK_GROWTH ( -1 )
#define portTICK_PERIOD_MS ( ( TickType_t ) 1000 / configTICK_RATE_HZ )
#define portBYTE_ALIGNMENT 8
/*-----------------------------------------------------------*/
/* Scheduler utilities. */
#define portYIELD() \
{ \
/* Set a PendSV to request a context switch. */ \
portNVIC_INT_CTRL_REG = portNVIC_PENDSVSET_BIT; \
\
/* Barriers are normally not required but do ensure the code is completely \
within the specified behaviour for the architecture. */ \
__asm volatile( "dsb" ::: "memory" ); \
__asm volatile( "isb" ); \
}
#define portNVIC_INT_CTRL_REG ( * ( ( volatile uint32_t * ) 0xe000ed04 ) )
#define portNVIC_PENDSVSET_BIT ( 1UL << 28UL )
#define portEND_SWITCHING_ISR( xSwitchRequired ) if( xSwitchRequired != pdFALSE ) portYIELD()
#define portYIELD_FROM_ISR( x ) portEND_SWITCHING_ISR( x )
/*-----------------------------------------------------------*/
/* Critical section management. */
extern void vPortEnterCritical( void );
extern void vPortExitCritical( void );
#define portSET_INTERRUPT_MASK_FROM_ISR() ulPortRaiseBASEPRI()
#define portCLEAR_INTERRUPT_MASK_FROM_ISR(x) vPortSetBASEPRI(x)
#define portDISABLE_INTERRUPTS() vPortRaiseBASEPRI()
#define portENABLE_INTERRUPTS() vPortSetBASEPRI(0)
#define portENTER_CRITICAL() vPortEnterCritical()
#define portEXIT_CRITICAL() vPortExitCritical()
/*-----------------------------------------------------------*/
/* Task function macros as described on the FreeRTOS.org WEB site. These are
not necessary for to use this port. They are defined so the common demo files
(which build with all the ports) will build. */
#define portTASK_FUNCTION_PROTO( vFunction, pvParameters ) void vFunction( void *pvParameters )
#define portTASK_FUNCTION( vFunction, pvParameters ) void vFunction( void *pvParameters )
/*-----------------------------------------------------------*/
/* Tickless idle/low power functionality. */
#ifndef portSUPPRESS_TICKS_AND_SLEEP
extern void vPortSuppressTicksAndSleep( TickType_t xExpectedIdleTime );
#define portSUPPRESS_TICKS_AND_SLEEP( xExpectedIdleTime ) vPortSuppressTicksAndSleep( xExpectedIdleTime )
#endif
/*-----------------------------------------------------------*/
/* Architecture specific optimisations. */
#ifndef configUSE_PORT_OPTIMISED_TASK_SELECTION
#define configUSE_PORT_OPTIMISED_TASK_SELECTION 1
#endif
#if configUSE_PORT_OPTIMISED_TASK_SELECTION == 1
/* Generic helper function. */
__attribute__( ( always_inline ) ) static inline uint8_t ucPortCountLeadingZeros( uint32_t ulBitmap )
{
uint8_t ucReturn;
__asm volatile ( "clz %0, %1" : "=r" ( ucReturn ) : "r" ( ulBitmap ) : "memory" );
return ucReturn;
}
/* Check the configuration. */
#if( configMAX_PRIORITIES > 32 )
#error configUSE_PORT_OPTIMISED_TASK_SELECTION can only be set to 1 when configMAX_PRIORITIES is less than or equal to 32. It is very rare that a system requires more than 10 to 15 difference priorities as tasks that share a priority will time slice.
#endif
/* Store/clear the ready priorities in a bit map. */
#define portRECORD_READY_PRIORITY( uxPriority, uxReadyPriorities ) ( uxReadyPriorities ) |= ( 1UL << ( uxPriority ) )
#define portRESET_READY_PRIORITY( uxPriority, uxReadyPriorities ) ( uxReadyPriorities ) &= ~( 1UL << ( uxPriority ) )
/*-----------------------------------------------------------*/
#define portGET_HIGHEST_PRIORITY( uxTopPriority, uxReadyPriorities ) uxTopPriority = ( 31UL - ( uint32_t ) ucPortCountLeadingZeros( ( uxReadyPriorities ) ) )
#endif /* configUSE_PORT_OPTIMISED_TASK_SELECTION */
/*-----------------------------------------------------------*/
#ifdef configASSERT
void vPortValidateInterruptPriority( void );
#define portASSERT_IF_INTERRUPT_PRIORITY_INVALID() vPortValidateInterruptPriority()
#endif
/* portNOP() is not required by this port. */
#define portNOP()
#define portINLINE __inline
#ifndef portFORCE_INLINE
#define portFORCE_INLINE inline __attribute__(( always_inline))
#endif
portFORCE_INLINE static BaseType_t xPortIsInsideInterrupt( void )
{
uint32_t ulCurrentInterrupt;
BaseType_t xReturn;
/* Obtain the number of the currently executing interrupt. */
__asm volatile( "mrs %0, ipsr" : "=r"( ulCurrentInterrupt ) :: "memory" );
if( ulCurrentInterrupt == 0 )
{
xReturn = pdFALSE;
}
else
{
xReturn = pdTRUE;
}
return xReturn;
}
/*-----------------------------------------------------------*/
portFORCE_INLINE static void vPortRaiseBASEPRI( void )
{
uint32_t ulNewBASEPRI;
__asm volatile
(
" mov %0, %1 \n" \
" msr basepri, %0 \n" \
" isb \n" \
" dsb \n" \
:"=r" (ulNewBASEPRI) : "i" ( configMAX_SYSCALL_INTERRUPT_PRIORITY ) : "memory"
);
}
/*-----------------------------------------------------------*/
portFORCE_INLINE static uint32_t ulPortRaiseBASEPRI( void )
{
uint32_t ulOriginalBASEPRI, ulNewBASEPRI;
__asm volatile
(
" mrs %0, basepri \n" \
" mov %1, %2 \n" \
" msr basepri, %1 \n" \
" isb \n" \
" dsb \n" \
:"=r" (ulOriginalBASEPRI), "=r" (ulNewBASEPRI) : "i" ( configMAX_SYSCALL_INTERRUPT_PRIORITY ) : "memory"
);
/* This return will not be reached but is necessary to prevent compiler
warnings. */
return ulOriginalBASEPRI;
}
/*-----------------------------------------------------------*/
portFORCE_INLINE static void vPortSetBASEPRI( uint32_t ulNewMaskValue )
{
__asm volatile
(
" msr basepri, %0 " :: "r" ( ulNewMaskValue ) : "memory"
);
}
/*-----------------------------------------------------------*/
#define portMEMORY_BARRIER() __asm volatile( "" ::: "memory" )
#ifdef __cplusplus
}
#endif
#endif /* PORTMACRO_H */

4
User/bsp/fdcan.c
View File

@ -27,13 +27,13 @@
#define FDCAN2_FILTER_CONFIG_TABLE(X) \
X(0, FDCAN_STANDARD_ID, FDCAN_FILTER_MASK, 0x000 , 0x000 , 0) \
X(1, FDCAN_EXTENDED_ID, FDCAN_FILTER_MASK, 0x00000000, 0x00000000, 0)
#define FDCAN2_GLOBAL_FILTER FDCAN_REJECT, FDCAN_REJECT, FDCAN_FILTER_REMOTE, FDCAN_FILTER_REMOTE/* 全局过滤器参数（用于 HAL_FDCAN_ConfigGlobalFilter） */
#define FDCAN2_GLOBAL_FILTER FDCAN_ACCEPT_IN_RX_FIFO1, FDCAN_ACCEPT_IN_RX_FIFO1, FDCAN_FILTER_REMOTE, FDCAN_FILTER_REMOTE/* 全局过滤器参数（用于 HAL_FDCAN_ConfigGlobalFilter） */
#endif
#ifdef FDCAN3_EN
#define FDCAN3_FILTER_CONFIG_TABLE(X) \
X(0, FDCAN_STANDARD_ID, FDCAN_FILTER_MASK, 0x000 , 0x000 , 0) \
X(1, FDCAN_EXTENDED_ID, FDCAN_FILTER_MASK, 0x00000000, 0x00000000, 0)
#define FDCAN3_GLOBAL_FILTER FDCAN_REJECT, FDCAN_REJECT, FDCAN_FILTER_REMOTE, FDCAN_FILTER_REMOTE/* 全局过滤器参数（用于 HAL_FDCAN_ConfigGlobalFilter） */
#define FDCAN3_GLOBAL_FILTER FDCAN_REJECT, FDCAN_ACCEPT_IN_RX_FIFO1, FDCAN_FILTER_REMOTE, FDCAN_FILTER_REMOTE/* 全局过滤器参数（用于 HAL_FDCAN_ConfigGlobalFilter） */
#endif
/* ====宏展开实现==== */

167
User/component/lqr.c Normal file
View File

@ -0,0 +1,167 @@
/*
* LQR控制器实现：单腿建模
*/
#include "lqr.h"
#include <string.h>
#include <stddef.h>
/* Private macros ----------------------------------------------------------- */
#define LQR_LIMIT(x, min, max) ((x) < (min) ? (min) : ((x) > (max) ? (max) : (x)))
/* Private variables -------------------------------------------------------- */
/* Private function prototypes ---------------------------------------------- */
static int8_t LQR_CalculateErrorState(LQR_t *lqr);
static float LQR_PolynomialCalc(const float *coeff, float leg_length);
/* Public functions --------------------------------------------------------- */
int8_t LQR_Init(LQR_t *lqr, LQR_GainMatrix_t *gain_matrix) {
if (lqr == NULL || gain_matrix == NULL) {
return -1;
}
/* 清零结构体 */
memset(lqr, 0, sizeof(LQR_t));
/* 设置增益矩阵 */
lqr->gain_matrix = gain_matrix;
/* 初始化当前腿长为中等值 */
lqr->current_leg_length = 0.25f;
/* 计算初始增益矩阵 */
LQR_CalculateGainMatrix(lqr, lqr->current_leg_length);
lqr->initialized = true;
return 0;
}
int8_t LQR_UpdateState(LQR_t *lqr, const LQR_State_t *current_state) {
if (lqr == NULL || current_state == NULL) {
return -1;
}
/* 更新当前状态 */
lqr->current_state = *current_state;
/* 计算状态误差 */
return LQR_CalculateErrorState(lqr);
}
int8_t LQR_SetTargetState(LQR_t *lqr, const LQR_State_t *target_state) {
if (lqr == NULL || target_state == NULL) {
return -1;
}
lqr->target_state = *target_state;
/* 重新计算状态误差 */
return LQR_CalculateErrorState(lqr);
}
int8_t LQR_CalculateGainMatrix(LQR_t *lqr, float leg_length) {
if (lqr == NULL || lqr->gain_matrix == NULL) {
return -1;
}
/* 限制腿长范围 */
leg_length = LQR_LIMIT(leg_length, 0.1f, 0.4f);
lqr->current_leg_length = leg_length;
/* 使用多项式拟合计算当前增益矩阵 */
lqr->K_matrix[0][0] = LQR_PolynomialCalc(lqr->gain_matrix->k11_coeff, leg_length); /* K11: theta */
lqr->K_matrix[0][1] = LQR_PolynomialCalc(lqr->gain_matrix->k12_coeff, leg_length); /* K12: d_theta */
lqr->K_matrix[0][2] = LQR_PolynomialCalc(lqr->gain_matrix->k13_coeff, leg_length); /* K13: x */
lqr->K_matrix[0][3] = LQR_PolynomialCalc(lqr->gain_matrix->k14_coeff, leg_length); /* K14: d_x */
lqr->K_matrix[0][4] = LQR_PolynomialCalc(lqr->gain_matrix->k15_coeff, leg_length); /* K15: phi */
lqr->K_matrix[0][5] = LQR_PolynomialCalc(lqr->gain_matrix->k16_coeff, leg_length); /* K16: d_phi */
lqr->K_matrix[1][0] = LQR_PolynomialCalc(lqr->gain_matrix->k21_coeff, leg_length); /* K21: theta */
lqr->K_matrix[1][1] = LQR_PolynomialCalc(lqr->gain_matrix->k22_coeff, leg_length); /* K22: d_theta */
lqr->K_matrix[1][2] = LQR_PolynomialCalc(lqr->gain_matrix->k23_coeff, leg_length); /* K23: x */
lqr->K_matrix[1][3] = LQR_PolynomialCalc(lqr->gain_matrix->k24_coeff, leg_length); /* K24: d_x */
lqr->K_matrix[1][4] = LQR_PolynomialCalc(lqr->gain_matrix->k25_coeff, leg_length); /* K25: phi */
lqr->K_matrix[1][5] = LQR_PolynomialCalc(lqr->gain_matrix->k26_coeff, leg_length); /* K26: d_phi */
return 0;
}
int8_t LQR_Control(LQR_t *lqr) {
if (lqr == NULL || !lqr->initialized) {
return -1;
}
/* LQR控制律: u = -K * x_error
* 其中 u = [T; Tp], x_error = x_current - x_target
*/
/* 计算轮毂力矩T */
lqr->control_output.T = -(
lqr->K_matrix[0][0] * lqr->error_state.theta +
lqr->K_matrix[0][1] * lqr->error_state.d_theta +
lqr->K_matrix[0][2] * lqr->error_state.x +
lqr->K_matrix[0][3] * lqr->error_state.d_x +
lqr->K_matrix[0][4] * lqr->error_state.phi +
lqr->K_matrix[0][5] * lqr->error_state.d_phi
);
/* 计算髋关节力矩Tp */
lqr->control_output.Tp = -(
lqr->K_matrix[1][0] * lqr->error_state.theta +
lqr->K_matrix[1][1] * lqr->error_state.d_theta +
lqr->K_matrix[1][2] * lqr->error_state.x +
lqr->K_matrix[1][3] * lqr->error_state.d_x +
lqr->K_matrix[1][4] * lqr->error_state.phi +
lqr->K_matrix[1][5] * lqr->error_state.d_phi
);
return 0;
}
int8_t LQR_Reset(LQR_t *lqr) {
if (lqr == NULL) {
return -1;
}
/* 清零状态和输出 */
memset(&lqr->current_state, 0, sizeof(LQR_State_t));
memset(&lqr->target_state, 0, sizeof(LQR_State_t));
memset(&lqr->error_state, 0, sizeof(LQR_State_t));
memset(&lqr->control_output, 0, sizeof(LQR_Output_t));
return 0;
}
/* Private functions -------------------------------------------------------- */
static int8_t LQR_CalculateErrorState(LQR_t *lqr) {
if (lqr == NULL) {
return -1;
}
/* 计算状态误差 */
lqr->error_state.theta = lqr->current_state.theta - lqr->target_state.theta;
lqr->error_state.d_theta = lqr->current_state.d_theta - lqr->target_state.d_theta;
lqr->error_state.x = lqr->current_state.x - lqr->target_state.x;
lqr->error_state.d_x = lqr->current_state.d_x - lqr->target_state.d_x;
lqr->error_state.phi = lqr->current_state.phi - lqr->target_state.phi;
lqr->error_state.d_phi = lqr->current_state.d_phi - lqr->target_state.d_phi;
return 0;
}
static float LQR_PolynomialCalc(const float *coeff, float leg_length) {
if (coeff == NULL) {
return 0.0f;
}
/* 计算3阶多项式: coeff[0]*L^3 + coeff[1]*L^2 + coeff[2]*L + coeff[3] */
float L = leg_length;
float L2 = L * L;
float L3 = L2 * L;
return coeff[0] * L3 + coeff[1] * L2 + coeff[2] * L + coeff[3];
}

134
User/component/lqr.h Normal file
View File

@ -0,0 +1,134 @@
/*
* LQR控制器
* 用于轮腿平衡机器人的线性二次调节器控制：单腿建模
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <stdint.h>
#include <stdbool.h>
#include <math.h>
/* Exported constants ------------------------------------------------------- */
#define LQR_STATE_DIM 6 /* 状态向量维度: theta, d_theta, x, d_x, phi, d_phi */
#define LQR_INPUT_DIM 2 /* 输入向量维度: T(轮毂力矩), Tp(髋关节力矩) */
#define LQR_POLY_ORDER 4 /* 多项式拟合阶数 */
/* Exported types ----------------------------------------------------------- */
typedef struct {
float theta; /* 摆杆角度 */
float d_theta; /* 摆杆角速度 */
float x; /* 驱动轮位移 */
float d_x; /* 驱动轮速度 */
float phi; /* 机体俯仰角 */
float d_phi; /* 机体俯仰角速度 */
} LQR_State_t;
typedef struct {
float T; /* 轮毂力矩 (N·m) */
float Tp; /* 髋关节力矩 (N·m) */
} LQR_Output_t;
typedef struct {
/* K矩阵第一行(轮毂力矩T对应的增益) */
float k11_coeff[LQR_POLY_ORDER]; /* K(1,1): theta */
float k12_coeff[LQR_POLY_ORDER]; /* K(1,2): d_theta */
float k13_coeff[LQR_POLY_ORDER]; /* K(1,3): x */
float k14_coeff[LQR_POLY_ORDER]; /* K(1,4): d_x */
float k15_coeff[LQR_POLY_ORDER]; /* K(1,5): phi */
float k16_coeff[LQR_POLY_ORDER]; /* K(1,6): d_phi */
/* K矩阵第二行(髋关节力矩Tp对应的增益) */
float k21_coeff[LQR_POLY_ORDER]; /* K(2,1): theta */
float k22_coeff[LQR_POLY_ORDER]; /* K(2,2): d_theta */
float k23_coeff[LQR_POLY_ORDER]; /* K(2,3): x */
float k24_coeff[LQR_POLY_ORDER]; /* K(2,4): d_x */
float k25_coeff[LQR_POLY_ORDER]; /* K(2,5): phi */
float k26_coeff[LQR_POLY_ORDER]; /* K(2,6): d_phi */
} LQR_GainMatrix_t;
/**
* @brief LQR控制器主结构体
*/
typedef struct {
LQR_GainMatrix_t *gain_matrix; /* 增益矩阵参数指针 */
LQR_State_t current_state; /* 当前状态 */
LQR_State_t target_state; /* 目标状态 */
LQR_State_t error_state; /* 状态误差 */
LQR_Output_t control_output; /* 控制输出 */
/* 运行时变量 */
float current_leg_length; /* 当前腿长 */
float K_matrix[LQR_INPUT_DIM][LQR_STATE_DIM]; /* 当前增益矩阵 */
bool initialized; /* 初始化标志 */
} LQR_t;
/* Exported functions prototypes -------------------------------------------- */
/**
* @brief 初始化LQR控制器
*
* @param lqr LQR控制器结构体指针
* @param gain_matrix 增益矩阵参数指针
* @return int8_t 0-成功, 其他-失败
*/
int8_t LQR_Init(LQR_t *lqr, LQR_GainMatrix_t *gain_matrix);
/**
* @brief 更新当前状态
*
* @param lqr LQR控制器结构体指针
* @param state 当前状态
* @return int8_t 0-成功, 其他-失败
*/
int8_t LQR_UpdateState(LQR_t *lqr, const LQR_State_t *current_state);
/**
* @brief 设置目标状态
*
* @param lqr LQR控制器结构体指针
* @param target_state 目标状态
* @return int8_t 0-成功, 其他-失败
*/
int8_t LQR_SetTargetState(LQR_t *lqr, const LQR_State_t *target_state);
/**
* @brief 根据当前腿长计算增益矩阵
*
* @param lqr LQR控制器结构体指针
* @param leg_length 当前腿长 (m)
* @return int8_t 0-成功, 其他-失败
*/
int8_t LQR_CalculateGainMatrix(LQR_t *lqr, float leg_length);
/**
* @brief 执行LQR控制计算
*
* @param lqr LQR控制器结构体指针
* @return int8_t 0-成功, 其他-失败
*/
int8_t LQR_Control(LQR_t *lqr);
/**
* @brief 重置LQR控制器
*
* @param lqr LQR控制器结构体指针
* @return int8_t 0-成功, 其他-失败
*/
int8_t LQR_Reset(LQR_t *lqr);
#ifdef __cplusplus
}
#endif

383
User/component/vmc.c Normal file
View File

@ -0,0 +1,383 @@
/*
* VMC虚拟模型控制器实现
*
* 本文件实现了轮腿机器人的VMC (Virtual Model Control) 虚拟模型控制算法
* 主要功能包括:
* 1. 五连杆机构的正逆运动学解算
* 2. 虚拟力到关节力矩的映射
* 3. 地面接触检测
* 4. 等效摆动杆模型转换
*
* 参考文献:
* - 韭菜的菜 知乎: 平衡步兵控制系统设计
* - 上交轮腿电控开源方案 (2023)
*/
#include "vmc.h"
#include <string.h>
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
#define VMC_EPSILON (1e-6f) // 数值计算精度
#define VMC_MAX_ITER (10) // 最大迭代次数
/* Private macro ------------------------------------------------------------ */
/**
* @brief 限制数值范围
*/
#define VMC_CLAMP(val, min, max) ((val) < (min) ? (min) : ((val) > (max) ? (max) : (val)))
/**
* @brief 安全开方
*/
#define VMC_SAFE_SQRT(x) (((x) > 0) ? sqrtf(x) : 0.0f)
/* Private variables -------------------------------------------------------- */
/* Private function prototypes ---------------------------------------------- */
static int8_t VMC_ValidateParams(const VMC_Param_t *param);
static void VMC_UpdateKinematics(VMC_t *vmc, float phi1, float phi4);
static int8_t VMC_SolveClosedLoop(VMC_t *vmc);
static float VMC_ComputeNumericDerivative(float current, float previous, float dt);
/* Exported functions ------------------------------------------------------- */
/**
* @brief 初始化VMC控制器
*/
int8_t VMC_Init(VMC_t *vmc, const VMC_Param_t *param, float sample_freq) {
if (vmc == NULL || param == NULL || sample_freq <= 0) {
return -1;
}
// 复制参数
memcpy(&vmc->param, param, sizeof(VMC_Param_t));
// 设置控制周期
vmc->dt = 1.0f / sample_freq;
// 重置状态
VMC_Reset(vmc);
vmc->initialized = true;
return 0;
}
/**
* @brief VMC五连杆正解算
*
* 通过髋关节角度和机体姿态，计算足端位置和等效摆动杆参数
*
* 坐标系定义:
* - x轴: 机体前进方向为正
* - y轴: 竖直向下为正
* - 角度: 顺时针为正
*/
int8_t VMC_ForwardSolve(VMC_t *vmc, float phi1, float phi4, float body_pitch, float body_pitch_rate) {
if (vmc == NULL || !vmc->initialized) {
return -1;
}
// body_pitch = -body_pitch; // 机体俯仰角取反
VMC_Leg_t *leg = &vmc->leg;
// 保存历史值
leg->last_phi0 = leg->phi0;
leg->last_L0 = leg->L0;
leg->last_d_L0 = leg->d_L0;
leg->last_d_theta = leg->d_theta;
// 更新关节角度
leg->phi1 = phi1;
leg->phi4 = phi4;
// 更新运动学状态
VMC_UpdateKinematics(vmc, phi1, phi4);
// 求解闭环运动学
if (VMC_SolveClosedLoop(vmc) != 0) {
return -1;
}
// 计算足端坐标
leg->foot_x = leg->XC - vmc->param.hip_length / 2.0f;
leg->foot_y = leg->YC;
// 计算等效摆动杆参数
leg->L0 = VMC_SAFE_SQRT(leg->foot_x * leg->foot_x + leg->foot_y * leg->foot_y);
leg->phi0 = atan2f(leg->foot_y, leg->foot_x);
// 计算等效摆动杆角度(相对于机体坐标系)
leg->alpha = VMC_PI_2 - leg->phi0;
leg->theta = VMC_PI - (VMC_PI_2 + body_pitch - leg->phi0);
// 角度归一化
VMC_ANGLE_NORMALIZE(leg->theta);
VMC_ANGLE_NORMALIZE(leg->phi0);
// 计算角速度和长度变化率
leg->d_phi0 = VMC_ComputeNumericDerivative(leg->phi0, leg->last_phi0, vmc->dt);
leg->d_alpha = -leg->d_phi0;
leg->d_theta = body_pitch_rate + leg->d_phi0;
leg->d_L0 = VMC_ComputeNumericDerivative(leg->L0, leg->last_L0, vmc->dt);
// 计算角加速度
leg->dd_theta = VMC_ComputeNumericDerivative(leg->d_theta, leg->last_d_theta, vmc->dt);
VMC_GroundContactDetection(vmc); // 更新地面接触状态
return 0;
}
/**
* @brief VMC五连杆逆解算(力矩分配)
*
* 根据期望的虚拟力和力矩，通过雅可比矩阵计算关节力矩
*/
int8_t VMC_InverseSolve(VMC_t *vmc, float F_virtual, float T_virtual) {
if (vmc == NULL || !vmc->initialized) {
return -1;
}
// 保存虚拟力和力矩
vmc->leg.F_virtual = F_virtual;
vmc->leg.T_virtual = -T_virtual;
// 计算雅可比矩阵
if (VMC_ComputeJacobian(vmc) != 0) {
return -1;
}
VMC_Leg_t *leg = &vmc->leg;
// 通过雅可比转置计算关节力矩
// tau = J^T * F_virtual
leg->tau_hip_rear = leg->J11 * vmc->leg.F_virtual + leg->J12 * vmc->leg.T_virtual;
leg->tau_hip_front = leg->J21 * vmc->leg.F_virtual + leg->J22 * vmc->leg.T_virtual;
return 0;
}
/**
* @brief 地面接触检测
*
* 基于虚拟力和腿部状态估计地面法向力
*/
float VMC_GroundContactDetection(VMC_t *vmc) {
if (vmc == NULL || !vmc->initialized) {
return 0.0f;
}
VMC_Leg_t *leg = &vmc->leg;
// 计算地面法向力
// Fn = F0*cos(theta) + Tp*sin(theta)/L0 + mg
leg->Fn = leg->F_virtual * cosf(leg->theta) +
leg->T_virtual * sinf(leg->theta) / leg->L0 +
10.0f; // 添加轮子重力，假设轮子质量约1kg
// 地面接触判断
leg->is_ground_contact = (leg->Fn > 20.0f); // 10N阈值
return leg->Fn;
}
/**
* @brief 获取足端位置
*/
int8_t VMC_GetFootPosition(const VMC_t *vmc, float *x, float *y) {
if (vmc == NULL || !vmc->initialized || x == NULL || y == NULL) {
return -1;
}
*x = vmc->leg.foot_x;
*y = vmc->leg.foot_y;
return 0;
}
/**
* @brief 获取等效摆动杆参数
*/
int8_t VMC_GetVirtualLegState(const VMC_t *vmc, float *length, float *angle, float *d_length, float *d_angle) {
if (vmc == NULL || !vmc->initialized) {
return -1;
}
if (length) *length = vmc->leg.L0;
if (angle) *angle = vmc->leg.theta;
if (d_length) *d_length = vmc->leg.d_L0;
if (d_angle) *d_angle = vmc->leg.d_theta;
return 0;
}
/**
* @brief 获取关节输出力矩
*/
int8_t VMC_GetJointTorques(const VMC_t *vmc, float *tau_front, float *tau_rear) {
if (vmc == NULL || !vmc->initialized || tau_front == NULL || tau_rear == NULL) {
return -1;
}
*tau_front = vmc->leg.tau_hip_front;
*tau_rear = vmc->leg.tau_hip_rear;
return 0;
}
/**
* @brief 重置VMC控制器状态
*/
void VMC_Reset(VMC_t *vmc) {
if (vmc == NULL) {
return;
}
// 清零腿部状态
memset(&vmc->leg, 0, sizeof(VMC_Leg_t));
// 设置初始值
vmc->leg.L0 = 0.15f; // 默认腿长15cm
vmc->leg.theta = 0.0f;
}
/**
* @brief 设置虚拟力和力矩
*/
void VMC_SetVirtualForces(VMC_t *vmc, float F_virtual, float T_virtual) {
if (vmc == NULL || !vmc->initialized) {
return;
}
vmc->leg.F_virtual = F_virtual;
vmc->leg.T_virtual = T_virtual;
}
/**
* @brief 计算雅可比矩阵
*
* 根据当前关节配置计算从虚拟力到关节力矩的雅可比矩阵
*/
int8_t VMC_ComputeJacobian(VMC_t *vmc) {
if (vmc == NULL || !vmc->initialized) {
return -1;
}
VMC_Leg_t *leg = &vmc->leg;
// 检查分母不为零
float sin_diff = sinf(leg->phi3 - leg->phi2);
if (fabsf(sin_diff) < VMC_EPSILON) {
return -1; // 奇异配置
}
// 计算雅可比矩阵元素
// J11: 后髋关节到支撑力的雅可比
leg->J11 = (vmc->param.leg_1 * sinf(leg->phi0 - leg->phi3) *
sinf(leg->phi1 - leg->phi2)) / sin_diff;
// J12: 后髋关节到摆动力矩的雅可比
leg->J12 = (vmc->param.leg_1 * cosf(leg->phi0 - leg->phi3) *
sinf(leg->phi1 - leg->phi2)) / (leg->L0 * sin_diff);
// J21: 前髋关节到支撑力的雅可比
leg->J21 = (vmc->param.leg_4 * sinf(leg->phi0 - leg->phi2) *
sinf(leg->phi3 - leg->phi4)) / sin_diff;
// J22: 前髋关节到摆动力矩的雅可比
leg->J22 = (vmc->param.leg_4 * cosf(leg->phi0 - leg->phi2) *
sinf(leg->phi3 - leg->phi4)) / (leg->L0 * sin_diff);
return 0;
}
/* Private functions -------------------------------------------------------- */
/**
* @brief 验证VMC参数有效性
*/
static int8_t VMC_ValidateParams(const VMC_Param_t *param) {
if (param->hip_length <= 0 || param->leg_1 <= 0 || param->leg_2 <= 0 ||
param->leg_3 <= 0 || param->leg_4 <= 0 || param->wheel_radius <= 0) {
return -1;
}
// 检查腿部几何约束
if (param->leg_2 + param->leg_3 <= param->leg_1 + param->leg_4) {
return -1; // 不满足闭环几何约束
}
return 0;
}
/**
* @brief 更新基本运动学参数
*/
static void VMC_UpdateKinematics(VMC_t *vmc, float phi1, float phi4) {
VMC_Leg_t *leg = &vmc->leg;
// 计算关键点坐标
// 点B (后髋关节末端)
leg->XB = vmc->param.leg_1 * cosf(phi1);
leg->YB = vmc->param.leg_1 * sinf(phi1);
// 点D (前髋关节末端)
leg->XD = vmc->param.hip_length + vmc->param.leg_4 * cosf(phi4);
leg->YD = vmc->param.leg_4 * sinf(phi4);
// 计算BD连杆长度
float dx = leg->XD - leg->XB;
float dy = leg->YD - leg->YB;
leg->lBD = VMC_SAFE_SQRT(dx * dx + dy * dy);
}
/**
* @brief 求解闭环运动学方程
*
* 根据两个髋关节角度，求解中间关节角度
*/
static int8_t VMC_SolveClosedLoop(VMC_t *vmc) {
VMC_Leg_t *leg = &vmc->leg;
// 使用余弦定理求解phi2
leg->A0 = 2 * vmc->param.leg_2 * (leg->XD - leg->XB);
leg->B0 = 2 * vmc->param.leg_2 * (leg->YD - leg->YB);
leg->C0 = vmc->param.leg_2 * vmc->param.leg_2 +
leg->lBD * leg->lBD -
vmc->param.leg_3 * vmc->param.leg_3;
// 检查判别式
float discriminant = leg->A0 * leg->A0 + leg->B0 * leg->B0 - leg->C0 * leg->C0;
if (discriminant < 0) {
return -1; // 无解
}
float sqrt_discriminant = VMC_SAFE_SQRT(discriminant);
// 计算phi2 (选择合适的解)
leg->phi2 = 2 * atan2f(leg->B0 + sqrt_discriminant, leg->A0 + leg->C0);
// 计算phi3
leg->phi3 = atan2f(leg->YB - leg->YD + vmc->param.leg_2 * sinf(leg->phi2),
leg->XB - leg->XD + vmc->param.leg_2 * cosf(leg->phi2));
// 计算足端坐标点C
leg->XC = leg->XB + vmc->param.leg_2 * cosf(leg->phi2);
leg->YC = leg->YB + vmc->param.leg_2 * sinf(leg->phi2);
return 0;
}
/**
* @brief 计算数值微分
*/
static float VMC_ComputeNumericDerivative(float current, float previous, float dt) {
if (dt <= 0) {
return 0.0f;
}
return (current - previous) / dt;
}

194
User/component/vmc.h Normal file
View File

@ -0,0 +1,194 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <stdint.h>
#include <stdbool.h>
#include <math.h>
/* Exported types ----------------------------------------------------------- */
/**
* @brief VMC虚拟模型控制参数结构体
*/
typedef struct {
float hip_length; // 髋关节间距
float leg_1; // 大腿前端长度 (L1)
float leg_2; // 大腿后端长度 (L2)
float leg_3; // 小腿长度 (L3)
float leg_4; // 小腿前端长度 (L4)
float wheel_radius; // 轮子半径
float wheel_mass; // 轮子质量
} VMC_Param_t;
/**
* @brief VMC腿部运动学状态结构体
*/
typedef struct {
// 关节角度
float phi1; // 后髋关节角度 (rad)
float phi2; // 大腿后端角度 (rad)
float phi3; // 小腿角度 (rad)
float phi4; // 前髋关节角度 (rad)
// 足端坐标
float foot_x; // 足端x坐标
float foot_y; // 足端y坐标
// 等效摆动杆参数
float L0; // 等效摆动杆长度
float d_L0; // 等效摆动杆长度变化率
float theta; // 等效摆动杆角度
float d_theta; // 等效摆动杆角速度
float dd_theta; // 等效摆动杆角加速度
// 虚拟力和力矩
float F_virtual; // 虚拟支撑力
float T_virtual; // 虚拟摆动力矩
// 雅可比矩阵元素
float J11, J12, J21, J22;
// 输出力矩
float tau_hip_front; // 前髋关节输出力矩
float tau_hip_rear; // 后髋关节输出力矩
// 内部计算变量
float XB, YB, XC, YC, XD, YD; // 各关键点坐标
float lBD; // BD连杆长度
float A0, B0, C0; // 运动学计算中间变量
float phi0; // 足端极角
float alpha; // 等效摆动杆与竖直方向夹角
float d_phi0; // 足端极角变化率
float d_alpha; // alpha角变化率
// 历史值（用于数值微分）
float last_phi0;
float last_L0;
float last_d_L0;
float last_d_theta;
// 地面接触检测
float Fn; // 地面法向力
bool is_ground_contact; // 地面接触标志
} VMC_Leg_t;
/**
* @brief VMC控制器结构体
*/
typedef struct {
VMC_Param_t param; // VMC参数
VMC_Leg_t leg; // 腿部状态
float dt; // 控制周期
bool initialized; // 初始化标志
} VMC_t;
/* Exported constants ------------------------------------------------------- */
#define VMC_PI_2 (1.5707963267948966f)
#define VMC_PI (3.1415926535897932f)
#define VMC_2PI (6.2831853071795865f)
/* Exported macros ---------------------------------------------------------- */
/**
* @brief 角度范围限制到[-PI, PI]
*/
#define VMC_ANGLE_NORMALIZE(angle) do { \
while((angle) > VMC_PI) (angle) -= VMC_2PI; \
while((angle) < -VMC_PI) (angle) += VMC_2PI; \
} while(0)
/* Exported functions prototypes -------------------------------------------- */
/**
* @brief 初始化VMC控制器
* @param vmc VMC控制器实例
* @param param VMC参数
* @param sample_freq 采样频率 (Hz)
* @return 0:成功, -1:失败
*/
int8_t VMC_Init(VMC_t *vmc, const VMC_Param_t *param, float sample_freq);
/**
* @brief VMC五连杆正解算
* @param vmc VMC控制器实例
* @param phi1 后髋关节角度 (rad)
* @param phi4 前髋关节角度 (rad)
* @param body_pitch 机体pitch角 (rad)
* @param body_pitch_rate 机体pitch角速度 (rad/s)
* @return 0:成功, -1:失败
*/
int8_t VMC_ForwardSolve(VMC_t *vmc, float phi1, float phi4, float body_pitch, float body_pitch_rate);
/**
* @brief VMC五连杆逆解算(力矩分配)
* @param vmc VMC控制器实例
* @param F_virtual 期望虚拟支撑力 (N)
* @param T_virtual 期望虚拟摆动力矩 (N*m)
* @return 0:成功, -1:失败
*/
int8_t VMC_InverseSolve(VMC_t *vmc, float F_virtual, float T_virtual);
/**
* @brief 地面接触检测
* @param vmc VMC控制器实例
* @return 地面法向力 (N)
*/
float VMC_GroundContactDetection(VMC_t *vmc);
/**
* @brief 获取足端位置(直角坐标)
* @param vmc VMC控制器实例
* @param x 足端x坐标输出
* @param y 足端y坐标输出
* @return 0:成功, -1:失败
*/
int8_t VMC_GetFootPosition(const VMC_t *vmc, float *x, float *y);
/**
* @brief 获取等效摆动杆参数
* @param vmc VMC控制器实例
* @param length 等效摆动杆长度输出
* @param angle 等效摆动杆角度输出
* @param d_length 等效摆动杆长度变化率输出
* @param d_angle 等效摆动杆角速度输出
* @return 0:成功, -1:失败
*/
int8_t VMC_GetVirtualLegState(const VMC_t *vmc, float *length, float *angle, float *d_length, float *d_angle);
/**
* @brief 获取关节输出力矩
* @param vmc VMC控制器实例
* @param tau_front 前髋关节力矩输出
* @param tau_rear 后髋关节力矩输出
* @return 0:成功, -1:失败
*/
int8_t VMC_GetJointTorques(const VMC_t *vmc, float *tau_front, float *tau_rear);
/**
* @brief 重置VMC控制器状态
* @param vmc VMC控制器实例
*/
void VMC_Reset(VMC_t *vmc);
/**
* @brief 设置虚拟力和力矩
* @param vmc VMC控制器实例
* @param F_virtual 虚拟支撑力 (N)
* @param T_virtual 虚拟摆动力矩 (N*m)
*/
void VMC_SetVirtualForces(VMC_t *vmc, float F_virtual, float T_virtual);
/**
* @brief 计算雅可比矩阵
* @param vmc VMC控制器实例
* @return 0:成功, -1:失败
*/
int8_t VMC_ComputeJacobian(VMC_t *vmc);
#ifdef __cplusplus
}
#endif

165
User/device/gimbal_imu.c Normal file
View File

@ -0,0 +1,165 @@
/*
* Gimbal IMU Device - 接收来自云台的CAN IMU数据
*/
/* Includes ----------------------------------------------------------------- */
#include "gimbal_imu.h"
#include <string.h>
#include "bsp/can.h"
#include "bsp/time.h"
/* Private function prototypes ---------------------------------------------- */
/**
* @brief 无符号整数转换为浮点数
*
* @param x_int 输入的整数值
* @param x_min 最小值
* @param x_max 最大值
* @param bits 位数
* @return float 转换后的浮点数
*/
static float uint_to_float(uint32_t x_int, float x_min, float x_max, int bits) {
float span = x_max - x_min;
float offset = x_min;
return ((float)x_int) * span / ((float)((1 << bits) - 1)) + offset;
}
/**
* @brief 解析CAN消息中的IMU数据
*/
static void GIMBAL_IMU_ParseMessage(GIMBAL_IMU_t *gimbal_imu, uint32_t id, const uint8_t *data) {
if (gimbal_imu == NULL || data == NULL) {
return;
}
/* 判断是哪个ID的数据 */
if (id == gimbal_imu->param.accl_id) {
/* 解析加速度计数据 - 21位精度解包 */
uint32_t acc_x = ((uint32_t)data[0] << 13) | ((uint32_t)data[1] << 5) | ((uint32_t)data[2] >> 3);
uint32_t acc_y = (((uint32_t)data[2] & 0x07) << 18) | ((uint32_t)data[3] << 10) | ((uint32_t)data[4] << 2) | ((uint32_t)data[5] >> 6);
uint32_t acc_z = (((uint32_t)data[5] & 0x3F) << 15) | ((uint32_t)data[6] << 7) | ((uint32_t)data[7] >> 1);
gimbal_imu->data.accl.x = uint_to_float(acc_x, GIMBAL_IMU_ACCEL_MIN, GIMBAL_IMU_ACCEL_MAX, 21);
gimbal_imu->data.accl.y = uint_to_float(acc_y, GIMBAL_IMU_ACCEL_MIN, GIMBAL_IMU_ACCEL_MAX, 21);
gimbal_imu->data.accl.z = uint_to_float(acc_z, GIMBAL_IMU_ACCEL_MIN, GIMBAL_IMU_ACCEL_MAX, 21);
} else if (id == gimbal_imu->param.gyro_id) {
/* 解析陀螺仪数据 - 21位精度解包 */
uint32_t gyro_x = ((uint32_t)data[0] << 13) | ((uint32_t)data[1] << 5) | ((uint32_t)data[2] >> 3);
uint32_t gyro_y = (((uint32_t)data[2] & 0x07) << 18) | ((uint32_t)data[3] << 10) | ((uint32_t)data[4] << 2) | ((uint32_t)data[5] >> 6);
uint32_t gyro_z = (((uint32_t)data[5] & 0x3F) << 15) | ((uint32_t)data[6] << 7) | ((uint32_t)data[7] >> 1);
gimbal_imu->data.gyro.x = uint_to_float(gyro_x, GIMBAL_IMU_GYRO_MIN, GIMBAL_IMU_GYRO_MAX, 21);
gimbal_imu->data.gyro.y = uint_to_float(gyro_y, GIMBAL_IMU_GYRO_MIN, GIMBAL_IMU_GYRO_MAX, 21);
gimbal_imu->data.gyro.z = uint_to_float(gyro_z, GIMBAL_IMU_GYRO_MIN, GIMBAL_IMU_GYRO_MAX, 21);
} else if (id == gimbal_imu->param.eulr_id) {
/* 解析欧拉角数据 - 21位精度解包 */
uint32_t pit = ((uint32_t)data[0] << 13) | ((uint32_t)data[1] << 5) | ((uint32_t)data[2] >> 3);
uint32_t yaw = (((uint32_t)data[2] & 0x07) << 18) | ((uint32_t)data[3] << 10) | ((uint32_t)data[4] << 2) | ((uint32_t)data[5] >> 6);
uint32_t rol = (((uint32_t)data[5] & 0x3F) << 15) | ((uint32_t)data[6] << 7) | ((uint32_t)data[7] >> 1);
gimbal_imu->data.eulr.pit = uint_to_float(pit, GIMBAL_IMU_PITCH_MIN, GIMBAL_IMU_PITCH_MAX, 21);
gimbal_imu->data.eulr.yaw = uint_to_float(yaw, GIMBAL_IMU_YAW_MIN, GIMBAL_IMU_YAW_MAX, 21);
gimbal_imu->data.eulr.rol = uint_to_float(rol, GIMBAL_IMU_ROLL_MIN, GIMBAL_IMU_ROLL_MAX, 21);
} else if (id == gimbal_imu->param.quat_id) {
/* 解析四元数数据 - 使用14位精度解包 */
uint16_t q0 = ((uint16_t)data[1] << 6) | ((uint16_t)data[2] >> 2);
uint16_t q1 = (((uint16_t)data[2] & 0x03) << 12) | ((uint16_t)data[3] << 4) | ((uint16_t)data[4] >> 4);
uint16_t q2 = (((uint16_t)data[4] & 0x0F) << 10) | ((uint16_t)data[5] << 2) | ((uint16_t)data[6] >> 6);
uint16_t q3 = (((uint16_t)data[6] & 0x3F) << 8) | (uint16_t)data[7];
gimbal_imu->data.quat.q0 = uint_to_float(q0, GIMBAL_IMU_QUAT_MIN, GIMBAL_IMU_QUAT_MAX, 14);
gimbal_imu->data.quat.q1 = uint_to_float(q1, GIMBAL_IMU_QUAT_MIN, GIMBAL_IMU_QUAT_MAX, 14);
gimbal_imu->data.quat.q2 = uint_to_float(q2, GIMBAL_IMU_QUAT_MIN, GIMBAL_IMU_QUAT_MAX, 14);
gimbal_imu->data.quat.q3 = uint_to_float(q3, GIMBAL_IMU_QUAT_MIN, GIMBAL_IMU_QUAT_MAX, 14);
}
}
/* Exported functions ------------------------------------------------------- */
/**
* @brief 初始化Gimbal IMU设备
* @param gimbal_imu Gimbal IMU结构体指针
* @param param 参数配置
* @return 初始化结果
* - DEVICE_OK: 成功
* - DEVICE_ERR_NULL: 参数为空
*/
int8_t GIMBAL_IMU_Init(GIMBAL_IMU_t *gimbal_imu, const GIMBAL_IMU_Param_t *param) {
if (gimbal_imu == NULL || param == NULL) {
return DEVICE_ERR_NULL;
}
/* 清空数据 */
memset(gimbal_imu, 0, sizeof(GIMBAL_IMU_t));
/* 复制参数 */
gimbal_imu->param = *param;
/* 设置设备类型 */
gimbal_imu->header.online = false;
/* 注册CAN ID到消息队列 */
BSP_CAN_RegisterId(param->can, param->accl_id, BSP_CAN_DEFAULT_QUEUE_SIZE);
BSP_CAN_RegisterId(param->can, param->gyro_id, BSP_CAN_DEFAULT_QUEUE_SIZE);
BSP_CAN_RegisterId(param->can, param->eulr_id, BSP_CAN_DEFAULT_QUEUE_SIZE);
BSP_CAN_RegisterId(param->can, param->quat_id, BSP_CAN_DEFAULT_QUEUE_SIZE);
return DEVICE_OK;
}
/**
* @brief 更新Gimbal IMU设备状态 - 从CAN队列读取并解析数据
* @param gimbal_imu Gimbal IMU结构体指针
* @return 更新结果
* - DEVICE_OK: 成功
* - DEVICE_ERR_NULL: 参数为空
*/
int8_t GIMBAL_IMU_Update(GIMBAL_IMU_t *gimbal_imu) {
if (gimbal_imu == NULL) {
return DEVICE_ERR_NULL;
}
BSP_CAN_Message_t msg;
bool received = false;
/* 读取所有可用的CAN消息 */
while (BSP_CAN_GetMessage(gimbal_imu->param.can, gimbal_imu->param.accl_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
GIMBAL_IMU_ParseMessage(gimbal_imu, gimbal_imu->param.accl_id, msg.data);
received = true;
}
while (BSP_CAN_GetMessage(gimbal_imu->param.can, gimbal_imu->param.gyro_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
GIMBAL_IMU_ParseMessage(gimbal_imu, gimbal_imu->param.gyro_id, msg.data);
received = true;
}
while (BSP_CAN_GetMessage(gimbal_imu->param.can, gimbal_imu->param.eulr_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
GIMBAL_IMU_ParseMessage(gimbal_imu, gimbal_imu->param.eulr_id, msg.data);
received = true;
}
while (BSP_CAN_GetMessage(gimbal_imu->param.can, gimbal_imu->param.quat_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
GIMBAL_IMU_ParseMessage(gimbal_imu, gimbal_imu->param.quat_id, msg.data);
received = true;
}
/* 更新在线状态和时间戳 */
if (received) {
gimbal_imu->header.online = true;
gimbal_imu->header.last_online_time = BSP_TIME_Get();
} else {
/* 超时检测 - 100ms */
uint64_t now_time = BSP_TIME_Get();
if (now_time - gimbal_imu->header.last_online_time > 100000) { /* 100ms超时，单位微秒 */
gimbal_imu->header.online = false;
}
}
return DEVICE_OK;
}

88
User/device/gimbal_imu.h Normal file
View File

@ -0,0 +1,88 @@
/*
* Gimbal IMU Device - 接收来自云台的CAN IMU数据
*/
#pragma once
#include <math.h>
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <stdbool.h>
#include <stdint.h>
#include "bsp/can.h"
#include "component/ahrs.h"
#include "device/device.h"
/* Exported constants ------------------------------------------------------- */
/* 数据范围定义（与发送端保持一致） */
#define GIMBAL_IMU_ACCEL_MAX (58.8f)
#define GIMBAL_IMU_ACCEL_MIN (-58.8f)
#define GIMBAL_IMU_GYRO_MAX (34.88f)
#define GIMBAL_IMU_GYRO_MIN (-34.88f)
#define GIMBAL_IMU_PITCH_MAX (90.0f)
#define GIMBAL_IMU_PITCH_MIN (-90.0f)
#define GIMBAL_IMU_ROLL_MAX (180.0f)
#define GIMBAL_IMU_ROLL_MIN (-180.0f)
#define GIMBAL_IMU_YAW_MAX (180.0f)
#define GIMBAL_IMU_YAW_MIN (-180.0f)
#define GIMBAL_IMU_QUAT_MIN (-1.0f)
#define GIMBAL_IMU_QUAT_MAX (1.0f)
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
/* Gimbal IMU参数配置 */
typedef struct {
BSP_CAN_t can; /* 使用的CAN总线 */
uint16_t accl_id;
uint16_t gyro_id;
uint16_t eulr_id;
uint16_t quat_id;
} GIMBAL_IMU_Param_t;
typedef struct {
AHRS_Accl_t accl; /* 加速度计数据 */
AHRS_Gyro_t gyro; /* 陀螺仪数据 */
AHRS_Eulr_t eulr; /* 欧拉角数据 */
AHRS_Quaternion_t quat; /* 四元数数据 */
float temp; /* 温度数据 (摄氏度) */
} GIMBAL_IMU_Data_t;
/* Gimbal IMU完整数据结构 */
typedef struct {
DEVICE_Header_t header;
GIMBAL_IMU_Param_t param; /* 参数配置 */
GIMBAL_IMU_Data_t data; /* IMU数据 */
} GIMBAL_IMU_t;
/* Exported functions prototypes -------------------------------------------- */
/**
* @brief 初始化Gimbal IMU设备
* @param gimbal_imu Gimbal IMU结构体指针
* @param param 参数配置
* @return 初始化结果
* - DEVICE_OK: 成功
* - DEVICE_ERR_NULL: 参数为空
*/
int8_t GIMBAL_IMU_Init(GIMBAL_IMU_t *gimbal_imu, const GIMBAL_IMU_Param_t *param);
/**
* @brief 更新Gimbal IMU设备状态
* @param gimbal_imu Gimbal IMU结构体指针
* @return 更新结果
* - DEVICE_OK: 成功
* - DEVICE_ERR_NULL: 参数为空
*/
int8_t GIMBAL_IMU_Update(GIMBAL_IMU_t *gimbal_imu);
#ifdef __cplusplus
}
#endif

731
User/module/balance_chassis.c Normal file
View File

@ -0,0 +1,731 @@
#include "module/balance_chassis.h"
#include "bsp/can.h"
#include "bsp/time.h"
#include "component/filter.h"
#include "component/user_math.h"
#include "device/motor_lk.h"
#include "device/motor_lz.h"
#include <math.h>
#include <stddef.h>
#include <string.h>
/**
* @brief 使能所有电机
* @param c 底盘结构体指针
* @return
*/
static int8_t Chassis_MotorEnable(Chassis_t *c) {
if (c == NULL)
return CHASSIS_ERR_NULL;
for (int i = 0; i < 4; i++) {
MOTOR_LZ_Param_t *joint_param = &c->param->joint_param[i];
MOTOR_LZ_Enable(joint_param);
}
for (int i = 0; i < 2; i++) {
MOTOR_LK_Param_t *wheel_param = &c->param->wheel_param[i];
MOTOR_LK_MotorOn(wheel_param);
}
return CHASSIS_OK;
}
static int8_t Chassis_MotorRelax(Chassis_t *c) {
if (c == NULL)
return CHASSIS_ERR_NULL;
for (int i = 0; i < 4; i++) {
MOTOR_LZ_Param_t *joint_param = &c->param->joint_param[i];
MOTOR_LZ_Relax(joint_param);
}
for (int i = 0; i < 2; i++) {
MOTOR_LK_Param_t *wheel_param = &c->param->wheel_param[i];
MOTOR_LK_Relax(wheel_param);
}
return CHASSIS_OK;
}
static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) {
if (c == NULL)
return CHASSIS_ERR_NULL; /* 主结构体不能为空 */
if (mode == c->mode)
return CHASSIS_OK; /* 模式未改变直接返回 */
// 特殊处理：从JUMP切换到WHELL_LEG_BALANCE时不重置
// 但从WHELL_LEG_BALANCE切换到JUMP时，需要重置以触发新的跳跃
if (c->mode == CHASSIS_MODE_JUMP && mode == CHASSIS_MODE_WHELL_LEG_BALANCE) {
c->mode = mode;
return CHASSIS_OK;
}
if (c->mode == CHASSIS_MODE_WHELL_LEG_BALANCE && mode == CHASSIS_MODE_JUMP) {
c->mode = mode;
c->state = 0; // 重置状态，确保每次切换模式时都重新初始化
c->jump_time=0; // 重置跳跃时间，切换到JUMP模式时会触发新跳跃
return CHASSIS_OK;
}
Chassis_MotorEnable(c);
PID_Reset(&c->pid.leg_length[0]);
PID_Reset(&c->pid.leg_length[1]);
PID_Reset(&c->pid.yaw);
PID_Reset(&c->pid.roll);
PID_Reset(&c->pid.tp[0]);
PID_Reset(&c->pid.tp[1]);
// 双零点自动选择逻辑（使用user_math的CircleError函数）
float current_yaw = c->feedback.yaw.rotor_abs_angle;
float zero_point_1 = c->param->mech_zero_yaw;
float zero_point_2 = zero_point_1 + M_PI; // 第二个零点，相差180度
// 使用CircleError计算到两个零点的角度差（范围为M_2PI）
float error_to_zero1 = CircleError(zero_point_1, current_yaw, M_2PI);
float error_to_zero2 = CircleError(zero_point_2, current_yaw, M_2PI);
// 选择误差绝对值更小的零点作为目标，并记录是否使用了第二个零点
if (fabsf(error_to_zero1) <= fabsf(error_to_zero2)) {
c->yaw_control.target_yaw = zero_point_1;
c->yaw_control.is_reversed = false; // 使用第一个零点，不反转
} else {
c->yaw_control.target_yaw = zero_point_2;
c->yaw_control.is_reversed = true; // 使用第二个零点，需要反转前后方向
}
// 清空位移
c->chassis_state.position_x = 0.0f;
c->chassis_state.velocity_x = 0.0f;
c->chassis_state.last_velocity_x = 0.0f;
c->chassis_state.target_x = 0.0f;
c->chassis_state.target_velocity_x = 0.0f;
c->chassis_state.last_target_velocity_x = 0.0f;
LQR_Reset(&c->lqr[0]);
LQR_Reset(&c->lqr[1]);
LowPassFilter2p_Reset(&c->filter.joint_out[0], 0.0f);
LowPassFilter2p_Reset(&c->filter.joint_out[1], 0.0f);
LowPassFilter2p_Reset(&c->filter.joint_out[2], 0.0f);
LowPassFilter2p_Reset(&c->filter.joint_out[3], 0.0f);
LowPassFilter2p_Reset(&c->filter.wheel_out[0], 0.0f);
LowPassFilter2p_Reset(&c->filter.wheel_out[1], 0.0f);
c->mode = mode;
c->state = 0; // 重置状态，确保每次切换模式时都重新初始化
c->jump_time=0; // 重置跳跃时间，切换到JUMP模式时会触发新跳跃
c->wz_multi=0.0f;
return CHASSIS_OK;
}
/* 更新机体状态估计 */
static void Chassis_UpdateChassisState(Chassis_t *c) {
if (c == NULL)
return;
// 从轮子编码器估计机体速度 (参考C++代码)
float left_wheel_speed_dps = c->feedback.wheel[0].rotor_speed; // dps (度每秒)
float right_wheel_speed_dps =
c->feedback.wheel[1].rotor_speed; // dps (度每秒)
// 将dps转换为rad/s
float left_wheel_speed = left_wheel_speed_dps * M_PI / 180.0f; // rad/s
float right_wheel_speed = right_wheel_speed_dps * M_PI / 180.0f; // rad/s
float wheel_radius = 0.068f;
float left_wheel_linear_vel = left_wheel_speed * wheel_radius;
float right_wheel_linear_vel = right_wheel_speed * wheel_radius;
// 机体x方向速度 (轮子中心速度)
c->chassis_state.last_velocity_x = c->chassis_state.velocity_x;
c->chassis_state.velocity_x =
(left_wheel_linear_vel + right_wheel_linear_vel) / 2.0f;
// 在跳跃模式的起跳和收腿阶段暂停位移积分，避免轮子空转导致的位移误差
if (!(c->mode == CHASSIS_MODE_JUMP && (c->state == 2 || c->state == 3))) {
// 积分得到位置
c->chassis_state.position_x += c->chassis_state.velocity_x * c->dt;
}
}
int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) {
if (c == NULL || param == NULL || target_freq <= 0.0f) {
return -1; // 参数错误
}
c->param = param;
c->mode = CHASSIS_MODE_RELAX;
/*初始化can*/
BSP_CAN_Init();
/*初始化和注册所有电机*/
MOTOR_LZ_Init();
// 注册关节电机
for (int i = 0; i < 4; i++) {
if (MOTOR_LZ_Register(&c->param->joint_param[i]) != DEVICE_OK) {
return DEVICE_ERR;
}
}
// 注册轮子电机
for (int i = 0; i < 2; i++) {
if (MOTOR_LK_Register(&c->param->wheel_param[i]) != DEVICE_OK) {
return DEVICE_ERR;
}
}
MOTOR_DM_Register(&c->param->yaw_motor);
/*初始化VMC控制器*/
VMC_Init(&c->vmc_[0], &param->vmc_param[0], target_freq);
VMC_Init(&c->vmc_[1], &param->vmc_param[1], target_freq);
/*初始化pid*/
PID_Init(&c->pid.leg_length[0], KPID_MODE_CALC_D, target_freq,
&param->leg_length);
PID_Init(&c->pid.leg_length[1], KPID_MODE_CALC_D, target_freq,
&param->leg_length);
PID_Init(&c->pid.yaw, KPID_MODE_CALC_D, target_freq, &param->yaw);
PID_Init(&c->pid.roll, KPID_MODE_CALC_D, target_freq, &param->roll);
PID_Init(&c->pid.tp[0], KPID_MODE_CALC_D, target_freq, &param->tp);
PID_Init(&c->pid.tp[1], KPID_MODE_CALC_D, target_freq, &param->tp);
/*初始化LQR控制器*/
LQR_Init(&c->lqr[0], &param->lqr_gains);
LQR_Init(&c->lqr[1], &param->lqr_gains);
LowPassFilter2p_Init(&c->filter.joint_out[0], target_freq,
param->low_pass_cutoff_freq.out);
LowPassFilter2p_Init(&c->filter.joint_out[1], target_freq,
param->low_pass_cutoff_freq.out);
LowPassFilter2p_Init(&c->filter.joint_out[2], target_freq,
param->low_pass_cutoff_freq.out);
LowPassFilter2p_Init(&c->filter.joint_out[3], target_freq,
param->low_pass_cutoff_freq.out);
LowPassFilter2p_Init(&c->filter.wheel_out[0], target_freq,
param->low_pass_cutoff_freq.out);
LowPassFilter2p_Init(&c->filter.wheel_out[1], target_freq,
param->low_pass_cutoff_freq.out);
Chassis_MotorEnable(c);
/*初始化机体状态*/
c->chassis_state.position_x = 0.0f;
c->chassis_state.velocity_x = 0.0f;
c->chassis_state.last_velocity_x = 0.0f;
c->chassis_state.target_x = 0.0f;
c->chassis_state.target_velocity_x = 0.0f;
c->chassis_state.last_target_velocity_x = 0.0f;
/*初始化yaw控制状态*/
c->yaw_control.target_yaw = 0.0f;
c->yaw_control.current_yaw = 0.0f;
c->yaw_control.yaw_force = 0.0f;
c->yaw_control.is_reversed = false;
return CHASSIS_OK;
}
int8_t Chassis_UpdateFeedback(Chassis_t *c) {
if (c == NULL) {
return -1; // 参数错误
}
// 更新所有电机数据
MOTOR_LZ_UpdateAll();
MOTOR_LK_UpdateAll();
// 更新反馈数据
for (int i = 0; i < 4; i++) {
MOTOR_LZ_t* joint_motor = MOTOR_LZ_GetMotor(&c->param->joint_param[i]);
if (joint_motor != NULL) {
c->feedback.joint[i] = joint_motor->motor.feedback;
}
}
for (int i = 0; i < 2; i++) {
MOTOR_LK_t* wheel_motor = MOTOR_LK_GetMotor(&c->param->wheel_param[i]);
if (wheel_motor != NULL) {
c->feedback.wheel[i] = wheel_motor->motor.feedback;
}
}
for (int i = 0; i < 4; i++) {
// 机械零点调整
if (c->feedback.joint[i].rotor_abs_angle > M_PI) {
c->feedback.joint[i].rotor_abs_angle -= M_2PI;
}
c->feedback.joint[i].rotor_abs_angle =
-c->feedback.joint[i].rotor_abs_angle; // 机械零点调整
}
// for (int i = 0; i < 4; i++) {
// c->feedback.joint[i] = virtual_chassis.data.joint[i];
// if (c->feedback.joint[i].rotor_abs_angle > M_PI) {
// c->feedback.joint[i].rotor_abs_angle -= M_2PI;
// }
// c->feedback.joint[i].rotor_abs_angle =
// -c->feedback.joint[i].rotor_abs_angle; // 机械零点调整
// }
// for (int i = 0; i < 2; i++) {
// c->feedback.wheel[i] = virtual_chassis.data.wheel[i];
// }
// c->feedback.imu.accl = virtual_chassis.data.imu.accl;
// c->feedback.imu.gyro = virtual_chassis.data.imu.gyro;
// c->feedback.imu.euler = virtual_chassis.data.imu.euler;
// 更新机体状态估计
Chassis_UpdateChassisState(c);
return 0;
}
int8_t Chassis_UpdateIMU(Chassis_t *c, const Chassis_IMU_t imu) {
if (c == NULL) {
return -1; // 参数错误
}
c->feedback.imu = imu;
return 0;
}
int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
if (c == NULL || c_cmd == NULL) {
return CHASSIS_ERR_NULL; // 参数错误
}
c->dt = (BSP_TIME_Get_us() - c->lask_wakeup) /
1000000.0f; /* 计算两次调用的时间间隔，单位秒 */
c->lask_wakeup = BSP_TIME_Get_us();
/*设置底盘模式*/
if (Chassis_SetMode(c, c_cmd->mode) != CHASSIS_OK) {
return CHASSIS_ERR_MODE; // 设置模式失败
}
VMC_ForwardSolve(&c->vmc_[0], c->feedback.joint[0].rotor_abs_angle,
c->feedback.joint[1].rotor_abs_angle,
c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
VMC_ForwardSolve(&c->vmc_[1], c->feedback.joint[3].rotor_abs_angle,
c->feedback.joint[2].rotor_abs_angle,
c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
/*根据底盘模式执行不同的控制逻辑*/
switch (c->mode) {
case CHASSIS_MODE_RELAX:
// 放松模式，电机不输出
Chassis_MotorRelax(c);
Chassis_LQRControl(c, c_cmd);
break;
case CHASSIS_MODE_RECOVER: {
float fn, tp;
fn = -20.0f;
tp = 0.0f;
Chassis_LQRControl(c, c_cmd);
VMC_InverseSolve(&c->vmc_[0], fn, tp);
VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0], &c->output.joint[1]);
VMC_InverseSolve(&c->vmc_[1], fn, tp);
VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3], &c->output.joint[2]);
c->output.wheel[0] = c_cmd->move_vec.vx * 0.2f;
c->output.wheel[1] = c_cmd->move_vec.vx * 0.2f;
Chassis_Output(c); // 统一输出
} break;
case CHASSIS_MODE_JUMP:
// 跳跃模式状态机
// 状态转换: 0(准备)->1(0.3s)->2(0.2s)->3(0.3s)->0(完成)
// jump_time == 0: 未开始跳跃
// jump_time == 1: 已完成跳跃（不再触发）
// jump_time > 1: 跳跃进行中
// 检测是否需要开始新的跳跃（state为0且jump_time为0）
if (c->state == 0 && c->jump_time == 0) {
// 开始跳跃，进入state 1
c->state = 1;
c->jump_time = BSP_TIME_Get_us();
}
// 只有在跳跃进行中时才处理状态转换（jump_time > 1）
if (c->jump_time > 1) {
// 计算已经过的时间（微秒）
uint64_t elapsed_us = BSP_TIME_Get_us() - c->jump_time;
// 状态转换逻辑
if (c->state == 1 && elapsed_us >= 300000) {
// state 1 保持0.3s后转到state 2
c->state = 2;
c->jump_time = BSP_TIME_Get_us(); // 重置计时
} else if (c->state == 2 && elapsed_us >= 230000) {
// state 2 保持0.25s后转到state 3，确保腿部充分伸展
c->state = 3;
c->jump_time = BSP_TIME_Get_us(); // 重置计时
} else if (c->state == 3 && elapsed_us >= 500000) {
// state 3 保持0.4s后转到state 0，确保收腿到最高
c->state = 0;
c->jump_time = 1; // 设置为1，表示已完成跳跃，不再触发
}
}
// 执行LQR控制（包含PID腿长控制）
Chassis_LQRControl(c, c_cmd);
Chassis_Output(c); // 统一输出
break;
case CHASSIS_MODE_WHELL_LEG_BALANCE:
case CHASSIS_MODE_ROTOR:
// 执行LQR控制（包含PID腿长控制）
Chassis_LQRControl(c, c_cmd);
Chassis_Output(c); // 统一输出
break;
break;
default:
return CHASSIS_ERR_MODE;
}
return CHASSIS_OK;
}
void Chassis_Output(Chassis_t *c) {
if (c == NULL)
return;
c->output.joint[0] =
LowPassFilter2p_Apply(&c->filter.joint_out[0], c->output.joint[0]);
c->output.joint[1] =
LowPassFilter2p_Apply(&c->filter.joint_out[1], c->output.joint[1]);
c->output.joint[2] =
LowPassFilter2p_Apply(&c->filter.joint_out[2], c->output.joint[2]);
c->output.joint[3] =
LowPassFilter2p_Apply(&c->filter.joint_out[3], c->output.joint[3]);
MOTOR_LZ_MotionParam_t motion_param = {
.torque=0.0f,
.target_angle=0.0f,
.target_velocity=0.0f,
.kp=0.0f,
.kd=0.0f,
};
for (int i = 0; i < 4; i++) {
motion_param.torque = c->output.joint[i];
MOTOR_LZ_MotionControl(&c->param->joint_param[i],&motion_param);
}
c->output.wheel[0] =
LowPassFilter2p_Apply(&c->filter.wheel_out[0], c->output.wheel[0]);
c->output.wheel[1] =
LowPassFilter2p_Apply(&c->filter.wheel_out[1], c->output.wheel[1]);
MOTOR_LK_SetOutput(&c->param->wheel_param[0], c->output.wheel[0]);
MOTOR_LK_SetOutput(&c->param->wheel_param[1], c->output.wheel[1]);
// Virtual_Chassis_SendWheelCommand(&virtual_chassis, c->output.wheel[0],
// c->output.wheel[1]);
// Virtual_Chassis_SendWheelCommand(&virtual_chassis, 0.0f,
// 0.0f); // 暂时不让轮子动
}
int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
if (c == NULL || c_cmd == NULL) {
return CHASSIS_ERR_NULL;
}
/* 运动参数（参考C++版本的状态估计） */
static float xhat = 0.0f, x_dot_hat = 0.0f;
float target_dot_x = 0.0f;
float max_acceleration = 2.0f; // 最大加速度限制: 3 m/s²
// 简化的速度估计（后续可以改进为C++版本的复杂滤波）
x_dot_hat = c->chassis_state.velocity_x;
xhat = c->chassis_state.position_x;
// 目标速度设定（原始期望速度）
float raw_vx = c_cmd->move_vec.vx * 2;
// 根据零点选择情况决定是否反转前后方向
float desired_velocity = c->yaw_control.is_reversed ? -raw_vx : raw_vx;
// 加速度限制处理
float velocity_change =
desired_velocity - c->chassis_state.last_target_velocity_x;
float max_velocity_change = max_acceleration * c->dt; // 最大允许的速度变化
// 限制速度变化率（加速度限制）
if (velocity_change > max_velocity_change) {
velocity_change = max_velocity_change;
} else if (velocity_change < -max_velocity_change) {
velocity_change = -max_velocity_change;
}
// 应用加速度限制后的目标速度
target_dot_x = c->chassis_state.last_target_velocity_x + velocity_change;
c->chassis_state.target_velocity_x = target_dot_x;
c->chassis_state.last_target_velocity_x = target_dot_x;
// 更新目标位置
c->chassis_state.target_x += target_dot_x * c->dt;
/* 分别计算左右腿的LQR控制 */
/* 构建当前状态 */
LQR_State_t current_state = {0};
current_state.theta = c->vmc_[0].leg.theta;
current_state.d_theta = c->vmc_[0].leg.d_theta;
current_state.x = xhat;
current_state.d_x = x_dot_hat;
current_state.phi = c->feedback.imu.euler.pit;
current_state.d_phi = -c->feedback.imu.gyro.x;
LQR_UpdateState(&c->lqr[0], &current_state);
current_state.theta = c->vmc_[1].leg.theta;
current_state.d_theta = c->vmc_[1].leg.d_theta;
LQR_UpdateState(&c->lqr[1], &current_state);
/* 根据当前腿长更新增益矩阵 */
LQR_CalculateGainMatrix(&c->lqr[0], c->vmc_[0].leg.L0);
LQR_CalculateGainMatrix(&c->lqr[1], c->vmc_[1].leg.L0);
/* 构建目标状态 */
LQR_State_t target_state = {0};
// 在跳跃收腿阶段强制摆角目标为0，确保落地时腿部竖直
if (c->mode == CHASSIS_MODE_JUMP && c->state == 3) {
target_state.theta = 0.0f; // 强制摆杆角度为0，确保腿部竖直
} else {
target_state.theta = 0.00; // 目标摆杆角度
}
target_state.d_theta = 0.0f; // 目标摆杆角速度
target_state.phi = -0.2f; // 目标俯仰角
target_state.d_phi = 0.0f; // 目标俯仰角速度
target_state.x = 0.0f; // 目标位置
target_state.d_x = 0.0f; // 目标速度
if (c->mode != CHASSIS_MODE_ROTOR){
c->yaw_control.current_yaw = c->feedback.yaw.rotor_abs_angle;
// 双零点自动选择逻辑（考虑手动控制偏移，使用CircleError函数）
float manual_offset = c_cmd->move_vec.vy * M_PI_2;
float base_target_1 = c->param->mech_zero_yaw + manual_offset;
float base_target_2 = c->param->mech_zero_yaw + M_PI + manual_offset;
// 使用CircleError计算到两个目标的角度误差
float error_to_target1 = CircleError(base_target_1, c->yaw_control.current_yaw, M_2PI);
float error_to_target2 = CircleError(base_target_2, c->yaw_control.current_yaw, M_2PI);
// 选择误差绝对值更小的目标，并更新反转状态
if (fabsf(error_to_target1) <= fabsf(error_to_target2)) {
c->yaw_control.target_yaw = base_target_1;
c->yaw_control.is_reversed = false; // 使用第一个零点，不反转
} else {
c->yaw_control.target_yaw = base_target_2;
c->yaw_control.is_reversed = true; // 使用第二个零点，需要反转前后方向
}
// c->yaw_control.yaw_force =
// PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw,
// c->feedback.yaw.rotor_abs_angle, 0.0f, c->dt);
c->yaw_control.yaw_force = 0.0f; // 关闭偏航控制
}else{
// 小陀螺模式：使用速度环控制
c->yaw_control.current_yaw = c->feedback.imu.euler.yaw;
// 渐增旋转速度，最大角速度约6.9 rad/s（约400度/秒）
c->wz_multi += 0.005f;
if (c->wz_multi > 1.2f)
c->wz_multi = 1.2f;
// 目标角速度（rad/s），可根据需要调整最大速度
float target_yaw_velocity = c->wz_multi * 1.0f; // 最大约7.2 rad/s
// 当前角速度反馈来自IMU陀螺仪
float current_yaw_velocity = c->feedback.imu.gyro.z;
// 使用PID控制角速度，输出力矩
c->yaw_control.yaw_force =
PID_Calc(&c->pid.yaw, target_yaw_velocity,
current_yaw_velocity, 0.0f, c->dt);
}
if (c->vmc_[0].leg.is_ground_contact) {
/* 更新LQR状态 */
LQR_SetTargetState(&c->lqr[0], &target_state);
LQR_Control(&c->lqr[0]);
} else {
// 在跳跃收腿阶段强制摆角目标为0，其他情况为0.16f
float non_contact_theta = (c->mode == CHASSIS_MODE_JUMP && c->state == 3) ? 0.0f : 0.16f;
target_state.theta = non_contact_theta; // 非接触时的腿摆角目标
LQR_SetTargetState(&c->lqr[0], &target_state);
c->lqr[0].control_output.T = 0.0f;
c->lqr[0].control_output.Tp =
c->lqr[0].K_matrix[1][0] * (-c->vmc_[0].leg.theta + non_contact_theta) +
c->lqr[0].K_matrix[1][1] * (-c->vmc_[0].leg.d_theta + 0.0f);
c->yaw_control.yaw_force = 0.0f; // 单腿离地时关闭偏航控制
}
if (c->vmc_[1].leg.is_ground_contact){
LQR_SetTargetState(&c->lqr[1], &target_state);
LQR_Control(&c->lqr[1]);
}else {
// 在跳跃收腿阶段强制摆角目标为0，其他情况为0.16f
float non_contact_theta = (c->mode == CHASSIS_MODE_JUMP && c->state == 3) ? 0.0f : 0.16f;
target_state.theta = non_contact_theta; // 非接触时的腿摆角目标
LQR_SetTargetState(&c->lqr[1], &target_state);
c->lqr[1].control_output.T = 0.0f;
c->lqr[1].control_output.Tp =
c->lqr[1].K_matrix[1][0] * (-c->vmc_[1].leg.theta + non_contact_theta) +
c->lqr[1].K_matrix[1][1] * (-c->vmc_[1].leg.d_theta + 0.0f);
c->yaw_control.yaw_force = 0.0f; // 单腿离地时关闭偏航控制
}
/* 轮毂力矩输出（参考C++版本的减速比） */
// 在跳跃的起跳和收腿阶段强制关闭轮子输出，防止离地时轮子猛转
if (c->mode == CHASSIS_MODE_JUMP && (c->state == 2 || c->state == 3)) {
c->output.wheel[0] = 0.0f;
c->output.wheel[1] = 0.0f;
} else {
c->output.wheel[0] =
c->lqr[0].control_output.T / 4.5f + c->yaw_control.yaw_force;
c->output.wheel[1] =
c->lqr[1].control_output.T / 4.5f - c->yaw_control.yaw_force;
}
/* 腿长控制和VMC逆解算（使用PID控制） */
float virtual_force[2];
float target_L0[2];
float leg_d_length[2]; // 腿长变化率
/* 横滚角PID补偿计算 */
// 使用PID控制器计算横滚角补偿长度，目标横滚角为0
float roll_compensation_length =
PID_Calc(&c->pid.roll, 0.0f, c->feedback.imu.euler.rol,
c->feedback.imu.gyro.x, c->dt);
// 限制补偿长度范围，防止过度补偿
// 如果任一腿离地，限制补偿长度
if (!c->vmc_[0].leg.is_ground_contact || !c->vmc_[1].leg.is_ground_contact) {
if (roll_compensation_length > 0.02f)
roll_compensation_length = 0.02f;
if (roll_compensation_length < -0.02f)
roll_compensation_length = -0.02f;
} else {
if (roll_compensation_length > 0.05f)
roll_compensation_length = 0.05f;
if (roll_compensation_length < -0.05f)
roll_compensation_length = -0.05f;
}
// 目标腿长设定（包含横滚角补偿）
switch (c->state) {
case 0: // 正常状态，根据高度指令调节腿长
target_L0[0] = 0.12f + c_cmd->height * 0.22f + roll_compensation_length; // 左腿：基础腿长 + 高度调节 + 横滚补偿
target_L0[1] = 0.12f + c_cmd->height * 0.22f - roll_compensation_length; // 右腿：基础腿长 + 高度调节 - 横滚补偿
break;
case 1: // 准备阶段，腿长收缩
target_L0[0] = 0.14f + roll_compensation_length; // 左腿：收缩到较短腿长 + 横滚补偿
target_L0[1] = 0.14f - roll_compensation_length; // 右腿：收缩到较短腿长 - 横滚补偿
break;
case 2: // 起跳阶段，腿长快速伸展
target_L0[0] = 0.65f; // 左腿：伸展到最大腿长（跳跃时不进行横滚补偿）
target_L0[1] = 0.65f; // 右腿：伸展到最大腿长（跳跃时不进行横滚补偿）
break;
case 3: // 收腿阶段，腿长收缩到最高
target_L0[0] = 0.06f; // 左腿：收缩到最短腿长（确保收腿到最高）
target_L0[1] = 0.06f; // 右腿：收缩到最短腿长（确保收腿到最高）
break;
default:
target_L0[0] = 0.16f + c_cmd->height * 0.22f + roll_compensation_length;
target_L0[1] = 0.16f + c_cmd->height * 0.22f - roll_compensation_length;
break;
}
// 腿长限幅：根据跳跃状态调整限制范围
if (c->mode == CHASSIS_MODE_JUMP && c->state == 3) {
// 在跳跃收腿阶段，允许更短的腿长，确保收腿到最高
if (target_L0[0] < 0.06f) target_L0[0] = 0.06f;
if (target_L0[1] < 0.06f) target_L0[1] = 0.06f;
} else {
// 正常情况下的腿长限制：最短0.10，最长0.42m
if (target_L0[0] < 0.10f) target_L0[0] = 0.10f;
if (target_L0[1] < 0.10f) target_L0[1] = 0.10f;
}
if (target_L0[0] > 0.42f) target_L0[0] = 0.42f;
if (target_L0[1] > 0.42f) target_L0[1] = 0.42f;
// 获取腿长变化率
VMC_GetVirtualLegState(&c->vmc_[0], NULL, NULL, &leg_d_length[0], NULL);
VMC_GetVirtualLegState(&c->vmc_[1], NULL, NULL, &leg_d_length[1], NULL);
/* 左右腿摆角相互补偿（参考C++版本的Delta_Tp机制） */
float Delta_Tp[2];
// 使用tp_pid进行左右腿摆角同步控制
// 左腿补偿力矩：使左腿theta向右腿theta靠拢
Delta_Tp[0] = c->vmc_[0].leg.L0 * 10.0f *
PID_Calc(&c->pid.tp[0], c->vmc_[1].leg.theta,
c->vmc_[0].leg.theta, c->vmc_[0].leg.d_theta, c->dt);
// 右腿补偿力矩：使右腿theta向左腿theta靠拢（符号相反）
Delta_Tp[1] = c->vmc_[1].leg.L0 * 10.0f *
PID_Calc(&c->pid.tp[1], c->vmc_[0].leg.theta,
c->vmc_[1].leg.theta, c->vmc_[1].leg.d_theta, c->dt);
// 左腿控制
{
// 使用PID进行腿长控制
float pid_output = PID_Calc(&c->pid.leg_length[0], target_L0[0],
c->vmc_[0].leg.L0, leg_d_length[0], c->dt);
// 腿长控制力 = LQR摆杆力矩的径向分量 + PID腿长控制输出 + 基础支撑力
virtual_force[0] =
(c->lqr[0].control_output.Tp) * sinf(c->vmc_[0].leg.theta) +
pid_output + 60.0f;
// VMC逆解算（包含摆角补偿）
if (VMC_InverseSolve(&c->vmc_[0], virtual_force[0],
c->lqr[0].control_output.Tp + Delta_Tp[0]) == 0) {
VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0],
&c->output.joint[1]);
} else {
// VMC失败，设为0力矩
c->output.joint[0] = 0.0f;
c->output.joint[1] = 0.0f;
}
}
// 右腿控制
{
// 使用PID进行腿长控制
float pid_output = PID_Calc(&c->pid.leg_length[1], target_L0[1],
c->vmc_[1].leg.L0, leg_d_length[1], c->dt);
// 腿长控制力 = LQR摆杆力矩的径向分量 + PID腿长控制输出 + 基础支撑力
virtual_force[1] =
(c->lqr[1].control_output.Tp) * sinf(c->vmc_[1].leg.theta) +
pid_output + 65.0f;
// VMC逆解算（包含摆角补偿）
if (VMC_InverseSolve(&c->vmc_[1], virtual_force[1],
c->lqr[1].control_output.Tp + Delta_Tp[1]) == 0) {
VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3],
&c->output.joint[2]);
} else {
// VMC失败，设为0力矩
c->output.joint[2] = 0.0f;
c->output.joint[3] = 0.0f;
}
}
/* 安全限制 */
for (int i = 0; i < 2; i++) {
if (fabsf(c->output.wheel[i]) > 1.0f) {
c->output.wheel[i] = (c->output.wheel[i] > 0) ? 1.0f : -1.0f;
}
}
for (int i = 0; i < 4; i++) {
if (fabsf(c->output.joint[i]) > 15.0f) {
c->output.joint[i] = (c->output.joint[i] > 0) ? 15.0f : -15.0f;
}
}
return CHASSIS_OK;
}

239
User/module/balance_chassis.h Normal file
View File

@ -0,0 +1,239 @@
/*
* 平衡底盘模组
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
// Front
// | 1 4 |
// (1)Left| |Right(2)
// | 2 3 |
/* Includes ----------------------------------------------------------------- */
#include <stdint.h>
#include <stdbool.h>
#include "component/vmc.h"
#include "component/lqr.h"
#include "component/ahrs.h"
#include "component/filter.h"
#include "component/pid.h"
#include "device/motor.h"
#include "device/motor_lk.h"
#include "device/motor_lz.h"
#include "device/motor_dm.h"
/* Exported constants ------------------------------------------------------- */
#define CHASSIS_OK (0) /* 运行正常 */
#define CHASSIS_ERR (-1) /* 运行时发现了其他错误 */
#define CHASSIS_ERR_NULL (-2) /* 运行时发现NULL指针 */
#define CHASSIS_ERR_MODE (-3) /* 运行时配置了错误的CMD_ChassisMode_t */
#define CHASSIS_ERR_TYPE (-4) /* 运行时配置了错误的Chassis_Type_t */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef enum {
CHASSIS_MODE_RELAX, /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */
CHASSIS_MODE_RECOVER, /* 复位模式 */
CHASSIS_MODE_WHELL_LEG_BALANCE, /* 轮子+腿平衡模式，底盘自我平衡 */
CHASSIS_MODE_JUMP, /* 跳跃模式，底盘跳跃 */
CHASSIS_MODE_ROTOR, /* 小陀螺模式，底盘高速旋转 */
} Chassis_Mode_t;
typedef enum {
CHASSIS_JUMP_STATE_READY, /* 准备跳跃 */
CHASSIS_JUMP_STATE_JUMPING, /* 跳跃中 */
CHASSIS_JUMP_STATE_LANDING, /* 着陆中 */
CHASSIS_JUMP_STATE_END, /* 跳跃结束 */
} Chassis_JumpState_t;
typedef struct {
Chassis_Mode_t mode; /* 底盘模式 */
MoveVector_t move_vec; /* 底盘运动向量 */
float height; /* 目标高度 */
} Chassis_CMD_t;
typedef struct {
AHRS_Accl_t accl; /* IMU加速度计 */
AHRS_Gyro_t gyro; /* IMU陀螺仪 */
AHRS_Eulr_t euler; /* IMU欧拉角 */
}Chassis_IMU_t;
typedef struct {
MOTOR_Feedback_t joint[4]; /* 四个关节电机反馈 */
MOTOR_Feedback_t wheel[2]; /* 两个轮子电机反馈 */
MOTOR_Feedback_t yaw; /* 云台Yaw轴电机反馈 */
Chassis_IMU_t imu; /* IMU数据 */
}Chassis_Feedback_t;
typedef struct {
float joint[4]; /* 四个关节电机输出 */
float wheel[2]; /* 两个轮子电机输出 */
}Chassis_Output_t;
/* 底盘参数的结构体，包含所有初始化用的参数，通常是const，存好几组 */
typedef struct {
MOTOR_LZ_Param_t joint_param[4]; // 4个电机参数
MOTOR_LK_Param_t wheel_param[2]; // 2个轮子电机参数
MOTOR_DM_Param_t yaw_motor; /* 云台Yaw轴电机参数 */
VMC_Param_t vmc_param[2]; /* VMC参数 */
LQR_GainMatrix_t lqr_gains; /* LQR增益矩阵参数 */
KPID_Params_t yaw; /* yaw轴PID控制参数，用于控制底盘朝向 */
KPID_Params_t roll; /* roll轴PID控制参数，用于横滚角补偿 */
KPID_Params_t tp; /* 摆力矩PID控制参数，用于控制摆力矩 */
KPID_Params_t leg_length; /* 腿长PID控制参数，用于控制虚拟腿长度 */
KPID_Params_t leg_theta; /* 摆角PID控制参数，用于控制虚拟腿摆角 */
float mech_zero_yaw; /* 机械零点 */
float theta;
float x;
float phi;
/* 低通滤波器截止频率 */
struct {
float in; /* 输入 */
float out; /* 输出 */
} low_pass_cutoff_freq;
} Chassis_Params_t;
/*
* 运行的主结构体，所有这个文件里的函数都在操作这个结构体
* 包含了初始化参数，中间变量，输出变量
*/
typedef struct {
uint64_t lask_wakeup;
float dt;
Chassis_Params_t *param; /* 底盘的参数，用Chassis_Init设定 */
AHRS_Eulr_t *mech_zero;
/* 模块通用 */
Chassis_Mode_t mode; /* 底盘模式 */
/* 反馈信息 */
Chassis_Feedback_t feedback;
/* 控制信息*/
Chassis_Output_t output;
VMC_t vmc_[2]; /* 两条腿的VMC */
LQR_t lqr[2]; /* 两条腿的LQR控制器 */
int8_t state;
uint64_t jump_time;
float angle;
float height;
/* 机体状态估计 */
struct {
float position_x; /* 机体x位置 */
float velocity_x; /* 机体x速度 */
float last_velocity_x; /* 上一次速度用于数值微分 */
float target_x; /* 目标x位置 */
float target_velocity_x; /* 目标速度 */
float last_target_velocity_x; /* 上一次目标速度 */
} chassis_state;
/* yaw控制相关 */
struct {
float target_yaw; /* 目标yaw角度 */
float current_yaw; /* 当前yaw角度 */
float yaw_force; /* yaw轴控制力矩 */
bool is_reversed; /* 是否使用反转的零点（180度零点），影响前后方向 */
} yaw_control;
float wz_multi; /* 小陀螺模式旋转方向 */
/* PID计算的目标值 */
struct {
AHRS_Eulr_t chassis;
} setpoint;
/* 反馈控制用的PID */
struct {
KPID_t yaw; /* 跟随云台用的PID */
KPID_t roll; /* 横滚角控制PID */
KPID_t tp[2];
KPID_t leg_length[2]; /* 两条腿的腿长PID */
KPID_t leg_theta[2]; /* 两条腿的摆角PID */
} pid;
/* 滤波器 */
struct {
LowPassFilter2p_t joint_out[4]; /* 关节输出滤波器 */
LowPassFilter2p_t wheel_out[2]; /* 轮子输出滤波器 */
} filter;
} Chassis_t;
/* Exported functions prototypes -------------------------------------------- */
/**
* \brief 初始化底盘
*
* \param c 包含底盘数据的结构体
* \param param 包含底盘参数的结构体指针
* \param target_freq 任务预期的运行频率
*
* \return 函数运行结果
*/
int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq);
/**
* \brief 更新底盘的反馈信息
*
* \param c 包含底盘数据的结构体
* \return 函数运行结果
*/
int8_t Chassis_UpdateFeedback(Chassis_t *c);
/**
* @brief 更新IMU数据
* @param c 包含底盘数据的结构体
* @param imu IMU数据
* @return
*/
int8_t Chassis_UpdateIMU(Chassis_t *c, const Chassis_IMU_t imu);
/**
* \brief 运行底盘控制逻辑
*
* \param c 包含底盘数据的结构体
* \param c_cmd 底盘控制指令
*
* \return 函数运行结果
*/
int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd);
/**
* \brief LQR控制逻辑
*
* \param c 包含底盘数据的结构体
* \param c_cmd 底盘控制指令
*
* \return 函数运行结果
*/
int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd);
/**
* \brief 底盘输出值
*
* \param s 包含底盘数据的结构体
* \param out CAN设备底盘输出结构体
*/
void Chassis_Output(Chassis_t *c);
#ifdef __cplusplus
}
#endif

266
User/module/config.c
View File

@ -4,6 +4,7 @@
/* Includes ----------------------------------------------------------------- */
#include "module/config.h"
#include <stdbool.h>
/* Private typedef ---------------------------------------------------------- */
@ -19,34 +20,116 @@
*/
Config_RobotParam_t robot_config = {
/* USER CODE BEGIN robot_config */
.gimbal_param = {
.pid = {
.yaw_omega = {
.k = 1.0f,
.p = 1.0f,
.i = 0.3f,
.d = 0.0f,
.i_limit = 1.0f,
.out_limit = 1.0f,
.d_cutoff_freq = -1.0f,
.range = -1.0f,
},
.yaw_angle = {
.k = 8.0f,
.p = 3.0f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 0.0f,
.out_limit = 10.0f,
.d_cutoff_freq = -1.0f,
.range = M_2PI,
},
.pit_omega = {
.k = 0.25f,
.p = 1.0f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 1.0f,
.out_limit = 1.0f,
.d_cutoff_freq = -1.0f,
.range = -1.0f,
},
.pit_angle = {
.k = 5.0f,
.p = 5.0f,
.i = 2.5f,
.d = 0.0f,
.i_limit = 0.0f,
.out_limit = 10.0f,
.d_cutoff_freq = -1.0f,
.range = M_2PI,
},
},
.mech_zero = {
.yaw = 0.0f,
.pit = 2.12f,
},
.travel = {
.yaw = -1.0f,
.pit = 0.9f,
},
.low_pass_cutoff_freq = {
.out = -1.0f,
.gyro = 1000.0f,
},
.pit_motor ={
.can = BSP_CAN_1,
.id = 0x206,
.gear = false,
.module = MOTOR_GM6020,
.reverse = true,
},
.yaw_motor = {
.can = BSP_CAN_1,
.can_id = 0x1,
.master_id = 0x11,
.module = MOTOR_DM_J4310,
.reverse = false,
},
.imu = {
.can = BSP_CAN_2,
.accl_id = 0x100, // 加速度计 (十进制256)
.gyro_id = 0x101, // 陀螺仪 (十进制257)
.eulr_id = 0x102, // 欧拉角 (十进制258)
.quat_id = 0x103 // 四元数 (十进制259)
},
},
.shoot_param = {
.trig_step_angle=M_2PI/8,
.shot_delay_time=0.05f,
.shot_burst_num=1,
.shot_delay_time=1.0f,
.shot_burst_num=20,
.fric_motor_param[0] = {
.can = BSP_CAN_2,
.can = BSP_CAN_1,
.id = 0x201,
.module = MOTOR_M3508,
.reverse = true,
.gear=false,
},
.fric_motor_param[1] = {
.can = BSP_CAN_2,
.id = 0x202,
.module = MOTOR_M3508,
.reverse = false,
.gear=false,
},
.fric_motor_param[1] = {
.can = BSP_CAN_1,
.id = 0x202,
.module = MOTOR_M3508,
.reverse = true,
.gear=false,
},
.trig_motor_param = {
.can = BSP_CAN_1,
.id = 0x201,
.id = 0x203,
.module = MOTOR_M2006,
.reverse = true,
.gear=true,
},
.fric_follow = {
.k=1.0f,
.p=1.8f,
.p=1.5f,
.i=0.0f,
.d=0.0f,
.i_limit=0.0f,
@ -93,10 +176,165 @@ Config_RobotParam_t robot_config = {
.in = 30.0f,
.out = 30.0f,
},
},
.chassis_param = {
.yaw={
.k=0.0f,
.p=1.0f,
.i=0.0f,
.d=0.3f,
.i_limit=0.0f,
.out_limit=1.0f,
.d_cutoff_freq=30.0f,
.range=M_2PI, /* 2*PI，用于处理角度循环误差 */
},
.roll={
.k=0.2f,
.p=0.5f, /* 横滚角比例系数 */
.i=0.01f, /* 横滚角积分系数 */
.d=0.01f, /* 横滚角微分系数 */
.i_limit=0.2f, /* 积分限幅 */
.out_limit=1.0f, /* 输出限幅，腿长差值限制 */
.d_cutoff_freq=30.0f, /* 微分截止频率 */
.range=-1.0f, /* 不使用循环误差处理 */
},
.leg_length={
.k = 40.0f,
.p = 20.0f,
.i = 0.01f,
.d = 2.0f,
.i_limit = 0.0f,
.out_limit = 200.0f,
.d_cutoff_freq = -1.0f,
.range = -1.0f,
},
.leg_theta={
.k=5.0f,
.p=2.0f, /* 摆角比例系数 */
.i=0.0f, /* 摆角积分系数 */
.d=0.0f, /* 摆角微分系数 */
.i_limit=0.0f, /* 积分限幅 */
.out_limit=0.05f, /* 输出限幅，腿长差值限制 */
.d_cutoff_freq=30.0f, /* 微分截止频率 */
.range=-1.0f, /* 不使用循环误差处理 */
},
.tp={
.k=4.0f,
.p=3.0f, /* 俯仰角比例系数 */
.i=0.0f, /* 俯仰角积分系数 */
.d=0.0f, /* 俯仰角微分系数 */
.i_limit=0.0f, /* 积分限幅 */
.out_limit=10.0f, /* 输出限幅，腿长差值限制 */
.d_cutoff_freq=30.0f, /* 微分截止频率 */
.range=-1.0f, /* 不使用循环误差处理 */
},
.low_pass_cutoff_freq = {
.in = 30.0f,
.out = 30.0f,
},
.yaw_motor = { // 云台Yaw轴电机
.can = BSP_CAN_1,
.can_id = 0x1,
.master_id = 0x11,
.module = MOTOR_DM_J4310,
.reverse = false,
},
.joint_param = {
{ // 左髋关节
.can = BSP_CAN_3,
.motor_id = 124,
.host_id = 0xFF,
.module = MOTOR_LZ_RSO3,
.reverse = false,
.mode = MOTOR_LZ_MODE_MOTION,
},
{ // 右髋关节
.can = BSP_CAN_3,
.motor_id = 125,
.host_id = 0xFF,
.module = MOTOR_LZ_RSO3,
.reverse = false,
.mode = MOTOR_LZ_MODE_MOTION,
},
{ // 左膝关节
.can = BSP_CAN_3,
.motor_id = 126,
.host_id = 0xFF,
.module = MOTOR_LZ_RSO3,
.reverse = true,
.mode = MOTOR_LZ_MODE_MOTION,
},
{ // 右膝关节
.can = BSP_CAN_3,
.motor_id = 127,
.host_id = 0xFF,
.module = MOTOR_LZ_RSO3,
.reverse = true,
.mode = MOTOR_LZ_MODE_MOTION,
},
},
.wheel_param = {
{ // 左轮电机
.can = BSP_CAN_3,
.id = 0x142,
.module = MOTOR_LK_MF9025,
.reverse = false,
},
{ // 右轮电机
.can = BSP_CAN_3,
.id = 0x141,
.module = MOTOR_LK_MF9025,
.reverse = true,
},
},
.mech_zero_yaw = 4.34085676f, /* 250.5度，机械零点 */
.vmc_param = {
{ // 左腿
.leg_1 = 0.215f, // 后髋连杆长度 (L1) (m)
.leg_2 = 0.258f, // 后膝连杆长度 (L2) (m)
.leg_3 = 0.258f, // 前膝连杆长度 (L3) (m)
.leg_4 = 0.215f, // 前髋连杆长度 (L4) (m)
.hip_length = 0.0f // 髋宽 (L5) (m)
},
{ // 右腿
.leg_1 = 0.215f, // 后髋连杆长度 (L1) (m)
.leg_2 = 0.258f, // 后膝连杆长度 (L2) (m)
.leg_3 = 0.258f, // 前膝连杆长度 (L3) (m)
.leg_4 = 0.215f, // 前髋连杆长度 (L4) (m)
.hip_length = 0.0f // 髋宽 (L5) (m)
}
},
.lqr_gains = {
.k11_coeff = { -2.111003343424443e+02f, 2.534552823281283e+02f, -1.288428090302336e+02f, -4.837794661314790e-01f }, // theta
.k12_coeff = { 9.842131763802227e+00f, -7.572331320496771e+00f, -6.747008033464407e+00f, 2.794374462854655e-02f }, // d_theta
.k13_coeff = { -7.026854551050478e+01f, 7.582881862804882e+01f, -2.920474536554779e+01f, 1.123343465922494e+00f }, // x
.k14_coeff = { -7.955300118741869e+01f, 8.683186601398823e+01f, -3.503519879783562e+01f, 1.204200953017506e+00f }, // d_x
.k15_coeff = { 2.214748921482655e+01f, -5.719424594843836e+00f, -8.215498046486028e+00f, 3.357225411090161e+00f }, // phi
.k16_coeff = { -2.588732118811617e-01f, 3.189187555191166e+00f, -3.235989186139448e+00f, 9.848256812196189e-01f }, // d_phi
.k21_coeff = { 3.783375900877637e+02f, -3.033633007638497e+02f, 4.903946574528503e+01f, 1.550109363173939e+01f }, // theta
.k22_coeff = { 2.878430656512739e+01f, -2.993892895824752e+01f, 1.019545567095573e+01f, 1.119137437173934e+00f }, // d_theta
.k23_coeff = { 2.518975783541210e+01f, 2.088553619504979e+00f, -1.929185285401291e+01f, 8.558202445583612e+00f }, // x
.k24_coeff = { 2.519335578848752e+01f, 5.878465085842500e+00f, -2.359290011046876e+01f, 1.050067818606354e+01f }, // d_x
.k25_coeff = { 1.146427691025439e+02f, -1.303101485018965e+02f, 5.234994297700838e+01f, 1.889931627651257e+00f }, // phi
.k26_coeff = { 3.214791991578748e+01f, -3.467989029865478e+01f, 1.318125503363052e+01f, -3.471228203459202e-01f }, // d_phi
},
.theta = 0.0f,
.x = 0.0f,
.phi = -0.1f,
}
/* USER CODE END robot_config */
};

5
User/module/config.h
View File

@ -11,6 +11,8 @@ extern "C" {
#include <stdint.h>
#include <stdbool.h>
#include "module/shoot.h"
#include "module/balance_chassis.h"
#include "module/gimbal.h"
/**
* @brief 机器人参数配置结构体
* @note 在此添加您的配置参数
@ -18,7 +20,8 @@ extern "C" {
typedef struct {
/* USER CODE BEGIN Config_RobotParam */
Shoot_Params_t shoot_param;
Chassis_Params_t chassis_param;
Gimbal_Params_t gimbal_param;
/* USER CODE END Config_RobotParam */
} Config_RobotParam_t;

260
User/module/gimbal.c Normal file
View File

@ -0,0 +1,260 @@
/*
* 云台模组
*/
/* Includes ----------------------------------------------------------------- */
#include "gimbal.h"
#include "bsp/can.h"
#include "bsp/time.h"
#include <math.h>
#include "component/filter.h"
#include "component/pid.h"
#include "device/gimbal_imu.h"
#include "device/motor_dm.h"
#include "device/motor_rm.h"
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* Private function -------------------------------------------------------- */
/**
* \brief 设置云台模式
*
* \param c 包含云台数据的结构体
* \param mode 要设置的模式
*
* \return 函数运行结果
*/
static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) {
if (g == NULL)
return -1;
if (mode == g->mode)
return GIMBAL_OK;
PID_Reset(&g->pid.yaw_angle);
PID_Reset(&g->pid.yaw_omega);
PID_Reset(&g->pid.pit_angle);
PID_Reset(&g->pid.pit_omega);
LowPassFilter2p_Reset(&g->filter_out.yaw, 0.0f);
LowPassFilter2p_Reset(&g->filter_out.pit, 0.0f);
MOTOR_DM_Enable(&(g->param->yaw_motor));
AHRS_ResetEulr(&(g->setpoint.eulr)); /* 切换模式后重置设定值 */
// if (g->mode == GIMBAL_MODE_RELAX) {
// if (mode == GIMBAL_MODE_ABSOLUTE) {
// g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw;
// } else if (mode == GIMBAL_MODE_RELATIVE) {
// g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw;
// }
// }
g->setpoint.eulr.pit = g->feedback.imu.eulr.rol;
g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw;
g->mode = mode;
return 0;
}
/* Exported functions ------------------------------------------------------- */
/**
* \brief 初始化云台
*
* \param g 包含云台数据的结构体
* \param param 包含云台参数的结构体指针
* \param target_freq 任务预期的运行频率
*
* \return 函数运行结果
*/
int8_t Gimbal_Init(Gimbal_t *g, const Gimbal_Params_t *param,
float target_freq) {
if (g == NULL)
return -1;
g->param = (Gimbal_Params_t *)param;
g->mode = GIMBAL_MODE_RELAX; /* 设置默认模式 */
/* 先初始化CAN */
BSP_CAN_Init();
/* 初始化云台电机控制PID和LPF */
PID_Init(&(g->pid.yaw_angle), KPID_MODE_NO_D, target_freq,
&(g->param->pid.yaw_angle));
PID_Init(&(g->pid.yaw_omega), KPID_MODE_CALC_D, target_freq,
&(g->param->pid.yaw_omega));
PID_Init(&(g->pid.pit_angle), KPID_MODE_NO_D, target_freq,
&(g->param->pid.pit_angle));
PID_Init(&(g->pid.pit_omega), KPID_MODE_CALC_D, target_freq,
&(g->param->pid.pit_omega));
LowPassFilter2p_Init(&g->filter_out.yaw, target_freq,
g->param->low_pass_cutoff_freq.out);
LowPassFilter2p_Init(&g->filter_out.pit, target_freq,
g->param->low_pass_cutoff_freq.out);
g->limit.yaw.max = g->param->mech_zero.yaw + g->param->travel.yaw;
g->limit.yaw.min = g->param->mech_zero.yaw;
g->limit.pit.max = g->param->mech_zero.pit + g->param->travel.pit;
g->limit.pit.min = g->param->mech_zero.pit;
MOTOR_RM_Register(&(g->param->pit_motor));
MOTOR_DM_Register(&(g->param->yaw_motor));
MOTOR_DM_Enable(&(g->param->yaw_motor));
/* 最后初始化IMU，确保CAN和电机都已经设置好 */
GIMBAL_IMU_Init(&g->imu, &(g->param->imu));
return 0;
}
/**
* \brief 通过CAN设备更新云台反馈信息
*
* \param gimbal 云台
* \param can CAN设备
*
* \return 函数运行结果
*/
int8_t Gimbal_UpdateFeedback(Gimbal_t *gimbal) {
if (gimbal == NULL)
return -1;
/* 更新RM电机反馈数据（pitch轴） */
MOTOR_RM_Update(&(gimbal->param->pit_motor));
MOTOR_RM_t *rm_motor = MOTOR_RM_GetMotor(&(gimbal->param->pit_motor));
if (rm_motor != NULL) {
gimbal->feedback.motor.pit = rm_motor->feedback;
}
/* 更新DM电机反馈数据（yaw轴） */
MOTOR_DM_Update(&(gimbal->param->yaw_motor));
MOTOR_DM_t *dm_motor = MOTOR_DM_GetMotor(&(gimbal->param->yaw_motor));
if (dm_motor != NULL) {
gimbal->feedback.motor.yaw = dm_motor->motor.feedback;
}
return 0;
}
int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal){
if (gimbal == NULL) {
return -1;
}
GIMBAL_IMU_Update(&gimbal->imu);
/* 将IMU数据复制到feedback中供控制使用 */
gimbal->feedback.imu.eulr = gimbal->imu.data.eulr;
gimbal->feedback.imu.gyro = gimbal->imu.data.gyro;
gimbal->feedback.imu.accl = gimbal->imu.data.accl;
return 0;
}
/**
* \brief 运行云台控制逻辑
*
* \param g 包含云台数据的结构体
* \param g_cmd 云台控制指令
*
* \return 函数运行结果
*/
int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
if (g == NULL || g_cmd == NULL) {
return -1;
}
g->dt = (BSP_TIME_Get_us() - g->lask_wakeup) / 1000000.0f;
g->lask_wakeup = BSP_TIME_Get_us();
Gimbal_SetMode(g, g_cmd->mode);
/* 处理yaw控制命令，软件限位 - 使用电机绝对角度 */
float delta_yaw = g_cmd->delta_yaw * g->dt * 1.5f;
if (g->param->travel.yaw > 0) {
/* 计算当前电机角度与IMU角度的偏差 */
float motor_imu_offset = g->feedback.motor.yaw.rotor_abs_angle - g->feedback.imu.eulr.yaw;
/* 处理跨越±π的情况 */
if (motor_imu_offset > M_PI) motor_imu_offset -= M_2PI;
if (motor_imu_offset < -M_PI) motor_imu_offset += M_2PI;
/* 计算到限位边界的距离 */
const float delta_max = CircleError(g->limit.yaw.max,
(g->setpoint.eulr.yaw + motor_imu_offset + delta_yaw), M_2PI);
const float delta_min = CircleError(g->limit.yaw.min,
(g->setpoint.eulr.yaw + motor_imu_offset + delta_yaw), M_2PI);
/* 限制控制命令 */
if (delta_yaw > delta_max) delta_yaw = delta_max;
if (delta_yaw < delta_min) delta_yaw = delta_min;
}
CircleAdd(&(g->setpoint.eulr.yaw), delta_yaw, M_2PI);
/* 处理pitch控制命令，软件限位 - 使用电机绝对角度 */
float delta_pit = g_cmd->delta_pit * g->dt;
if (g->param->travel.pit > 0) {
/* 计算当前电机角度与IMU角度的偏差 */
float motor_imu_offset = g->feedback.motor.pit.rotor_abs_angle - g->feedback.imu.eulr.rol;
/* 处理跨越±π的情况 */
if (motor_imu_offset > M_PI) motor_imu_offset -= M_2PI;
if (motor_imu_offset < -M_PI) motor_imu_offset += M_2PI;
/* 计算到限位边界的距离 */
const float delta_max = CircleError(g->limit.pit.max,
(g->setpoint.eulr.pit + motor_imu_offset + delta_pit), M_2PI);
const float delta_min = CircleError(g->limit.pit.min,
(g->setpoint.eulr.pit + motor_imu_offset + delta_pit), M_2PI);
/* 限制控制命令 */
if (delta_pit > delta_max) delta_pit = delta_max;
if (delta_pit < delta_min) delta_pit = delta_min;
}
CircleAdd(&(g->setpoint.eulr.pit), delta_pit, M_2PI);
/* 控制相关逻辑 */
float yaw_omega_set_point, pit_omega_set_point;
switch (g->mode) {
case GIMBAL_MODE_RELAX:
g->out.yaw = 0.0f;
g->out.pit = 0.0f;
break;
case GIMBAL_MODE_ABSOLUTE:
case GIMBAL_MODE_RELATIVE:
yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), g->setpoint.eulr.yaw,
g->feedback.imu.eulr.yaw, 0.0f, g->dt);
g->out.yaw = PID_Calc(&(g->pid.pit_omega), yaw_omega_set_point,
g->feedback.imu.gyro.z, 0.f, g->dt);
pit_omega_set_point = PID_Calc(&(g->pid.pit_angle), g->setpoint.eulr.pit,
g->feedback.imu.eulr.rol, 0.0f, g->dt);
g->out.pit = PID_Calc(&(g->pid.pit_omega), pit_omega_set_point,
g->feedback.imu.gyro.y, 0.f, g->dt);
break;
}
/* 输出滤波 */
g->out.yaw = LowPassFilter2p_Apply(&g->filter_out.yaw, g->out.yaw);
g->out.pit = LowPassFilter2p_Apply(&g->filter_out.pit, g->out.pit);
return 0;
}
/**
* \brief 云台输出
*
* \param s 包含云台数据的结构体
* \param out CAN设备云台输出结构体
*/
void Gimbal_Output(Gimbal_t *g){
MOTOR_RM_SetOutput(&g->param->pit_motor, g->out.pit);
MOTOR_MIT_Output_t output = {0};
output.torque = g->out.yaw * 5.0f; // 乘以减速比
output.kd = 0.3f;
MOTOR_RM_Ctrl(&g->param->pit_motor);
MOTOR_DM_MITCtrl(&g->param->yaw_motor, &output);
}

183
User/module/gimbal.h Normal file
View File

@ -0,0 +1,183 @@
/*
* 云台模组
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include "component/ahrs.h"
#include "component/filter.h"
#include "component/pid.h"
#include "device/motor.h"
#include "device/motor_dm.h"
#include "device/motor_rm.h"
#include "device/gimbal_imu.h"
/* Exported constants ------------------------------------------------------- */
#define GIMBAL_OK (0) /* 运行正常 */
#define GIMBAL_ERR (-1) /* 运行时发现了其他错误 */
#define GIMBAL_ERR_NULL (-2) /* 运行时发现NULL指针 */
#define GIMBAL_ERR_MODE (-3) /* 运行时配置了错误的CMD_GimbalMode_t */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef enum {
GIMBAL_MODE_RELAX, /* 放松模式，电机不输出。一般情况云台初始化之后的模式 */
GIMBAL_MODE_ABSOLUTE, /* 绝对坐标系控制，控制在空间内的绝对姿态 */
GIMBAL_MODE_RELATIVE, /* 相对坐标系控制，控制相对于底盘的姿态 */
} Gimbal_Mode_t;
typedef struct {
Gimbal_Mode_t mode;
float delta_yaw;
float delta_pit;
} Gimbal_CMD_t;
/* 软件限位 */
typedef struct {
float max;
float min;
} Gimbal_Limit_t;
/* 云台参数的结构体，包含所有初始化用的参数，通常是const，存好几组。*/
typedef struct {
MOTOR_RM_Param_t pit_motor; /* pitch轴电机参数 */
MOTOR_DM_Param_t yaw_motor; /* yaw轴电机参数 */
GIMBAL_IMU_Param_t imu; /* 云台IMU参数 */
struct {
KPID_Params_t yaw_omega; /* yaw轴角速度环PID参数 */
KPID_Params_t yaw_angle; /* yaw轴角位置环PID参数 */
KPID_Params_t pit_omega; /* pitch轴角速度环PID参数 */
KPID_Params_t pit_angle; /* pitch轴角位置环PID参数 */
} pid;
/* 低通滤波器截止频率 */
struct {
float out; /* 电机输出 */
float gyro; /* 陀螺仪数据 */
} low_pass_cutoff_freq;
struct {
float yaw; /* yaw轴机械限位 */
float pit; /* pitch轴机械限位 */
} mech_zero;
struct {
float yaw; /* yaw轴机械限位行程 -1表示无限位 */
float pit; /* pitch轴机械限位行程 -1表示无限位*/
} travel;
} Gimbal_Params_t;
/* 云台反馈数据的结构体，包含反馈控制用的反馈数据 */
typedef struct {
struct {
AHRS_Eulr_t eulr; /* 云台姿态欧拉角 */
AHRS_Gyro_t gyro; /* 云台陀螺仪数据 */
AHRS_Accl_t accl; /* 云台加速度计数据 */
}imu;
struct {
MOTOR_Feedback_t yaw; /* yaw轴电机反馈 */
MOTOR_Feedback_t pit; /* pitch轴电机反馈 */
} motor;
} Gimbal_Feedback_t;
/* 云台输出数据的结构体*/
typedef struct {
float yaw; /* yaw轴电机输出 */
float pit; /* pitch轴电机输出 */
} Gimbal_Output_t;
/*
* 运行的主结构体，所有这个文件里的函数都在操作这个结构体。
* 包含了初始化参数，中间变量，输出变量。
*/
typedef struct {
uint64_t lask_wakeup;
float dt;
Gimbal_Params_t *param; /* 云台的参数，用Gimbal_Init设定 */
/* 模块通用 */
Gimbal_Mode_t mode; /* 云台模式 */
GIMBAL_IMU_t imu; /* 云台IMU数据 */
/* PID计算的目标值 */
struct {
AHRS_Eulr_t eulr; /* 表示云台姿态的欧拉角 */
} setpoint;
struct {
KPID_t yaw_angle; /* yaw轴角位置环PID */
KPID_t yaw_omega; /* yaw轴角速度环PID */
KPID_t pit_angle; /* pitch轴角位置环PID */
KPID_t pit_omega; /* pitch轴角速度环PID */
} pid;
struct {
Gimbal_Limit_t yaw;
Gimbal_Limit_t pit;
} limit;
struct {
LowPassFilter2p_t yaw;
LowPassFilter2p_t pit;
} filter_out;
Gimbal_Output_t out; /* 云台输出 */
Gimbal_Feedback_t feedback; /* 反馈 */
} Gimbal_t;
/* Exported functions prototypes -------------------------------------------- */
/**
* \brief 初始化云台
*
* \param g 包含云台数据的结构体
* \param param 包含云台参数的结构体指针
* \param target_freq 任务预期的运行频率
*
* \return 函数运行结果
*/
int8_t Gimbal_Init(Gimbal_t *g, const Gimbal_Params_t *param,
float target_freq);
/**
* \brief 通过CAN设备更新云台反馈信息
*
* \param gimbal 云台
* \param can CAN设备
*
* \return 函数运行结果
*/
int8_t Gimbal_UpdateFeedback(Gimbal_t *gimbal);
int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal);
/**
* \brief 运行云台控制逻辑
*
* \param g 包含云台数据的结构体
* \param fb 云台反馈信息
* \param g_cmd 云台控制指令
* \param dt_sec 两次调用的时间间隔
*
* \return 函数运行结果
*/
int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd);
/**
* \brief 云台输出
*
* \param s 包含云台数据的结构体
* \param out CAN设备云台输出结构体
*/
void Gimbal_Output(Gimbal_t *g);
#ifdef __cplusplus
}
#endif

7
User/module/shoot.c
View File

@ -5,6 +5,7 @@
#include "component/user_math.h"
#include <math.h>
#include "bsp/time.h"
#include "device/motor_rm.h"
static bool last_firecmd;
@ -120,6 +121,7 @@ int8_t Shoot_Init(Shoot_t *s, Shoot_Params_t *param, float target_freq)
for(int i=0;i<SHOOT_FRIC_NUM;i++){
MOTOR_RM_Register(&param->fric_motor_param[i]);
PID_Init(&s->pid.fric_follow[i], KPID_MODE_CALC_D, target_freq,&param->fric_follow);
PID_Init(&s->pid.fric_err[i], KPID_MODE_CALC_D, target_freq,&param->fric_err);
LowPassFilter2p_Init(&s->filter.fric.in[i], target_freq, s->param->filter.fric.in);
LowPassFilter2p_Init(&s->filter.fric.out[i], target_freq, s->param->filter.fric.out);
}
@ -132,7 +134,7 @@ int8_t Shoot_Init(Shoot_t *s, Shoot_Params_t *param, float target_freq)
memset(&s->shoot_Anglecalu,0,sizeof(s->shoot_Anglecalu));
memset(&s->output,0,sizeof(s->output));
s->target_variable.target_rpm=5000.0f;
s->target_variable.target_rpm=6000.0f; /* 归一化目标转速 */
return 0;
}
@ -208,7 +210,7 @@ int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *cmd)
case SHOOT_STATE_IDLE:/*熄火等待*/
for(int i=0;i<SHOOT_FRIC_NUM;i++)
{
PID_ResetIntegral(&s->pid.fric_follow[i]);
s->output.out_follow[i]=PID_Calc(&s->pid.fric_follow[i],0.0f,s->feedback.fric_rpm[i],0,s->dt);
s->output.out_fric[i]=s->output.out_follow[i];
s->output.lpfout_fric[i] = LowPassFilter2p_Apply(&s->filter.fric.out[i], s->output.out_fric[i]);
@ -290,7 +292,6 @@ int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *cmd)
}
}
MOTOR_RM_Ctrl(&s->param->fric_motor_param[0]);
MOTOR_RM_Ctrl(&s->param->trig_motor_param);
last_firecmd = cmd->firecmd;
return 0;
}

19
User/task/atti_esit.c
View File

@ -12,6 +12,7 @@
#include "component/ahrs.h"
#include "component/pid.h"
#include "device/bmi088.h"
#include "module/balance_chassis.h"
#include "task/user_task.h"
/* USER INCLUDE END */
@ -25,7 +26,7 @@ BMI088_t bmi088;
AHRS_t gimbal_ahrs;
AHRS_Magn_t magn;
AHRS_Eulr_t eulr_to_send;
Chassis_IMU_t imu_for_chassis;
KPID_t imu_temp_ctrl_pid;
/*默认校准参数*/
@ -81,6 +82,22 @@ void Task_atti_esit(void *argument) {
/* 根据解析出来的四元数计算欧拉角 */
AHRS_GetEulr(&eulr_to_send, &gimbal_ahrs);
/* 加速度计数据绕z轴旋转270度: (x,y,z) -> (y,-x,z) */
imu_for_chassis.accl.x = bmi088.accl.y;
imu_for_chassis.accl.y = -bmi088.accl.x;
imu_for_chassis.accl.z = bmi088.accl.z;
/* 陀螺仪数据绕z轴旋转270度: (x,y,z) -> (y,-x,z) */
imu_for_chassis.gyro.x = bmi088.gyro.y;
imu_for_chassis.gyro.y = -bmi088.gyro.x;
imu_for_chassis.gyro.z = bmi088.gyro.z;
/* 欧拉角绕z轴旋转270度 */
imu_for_chassis.euler.rol = eulr_to_send.pit;
imu_for_chassis.euler.pit = -eulr_to_send.rol;
imu_for_chassis.euler.yaw = eulr_to_send.yaw - 1.57079632679f; // -90度
osMessageQueueReset(
task_runtime.msgq.chassis.imu); // 重置消息队列，防止阻塞
osMessageQueuePut(task_runtime.msgq.chassis.imu, &imu_for_chassis, 0,
0); // 非阻塞发送底盘IMU数据
BSP_PWM_SetComp(BSP_PWM_IMU_HEAT, PID_Calc(&imu_temp_ctrl_pid, 40.5f,
bmi088.temp, 0.0f, 0.0f));
osKernelUnlock();

1
User/task/blink.c
View File

@ -42,6 +42,7 @@ void Task_blink(void *argument) {
/* USER CODE BEGIN */
count++;
uint16_t phase = count % 1000;
if (count == 1001) count = 1;
if (phase == 0) {
/* 每秒开始播放C4音符 */
BUZZER_Set(&buzzer, 261.63f, 0.5f); // C4音符频率约261.63Hz

42
User/task/ctrl_chassis.c
View File

@ -7,9 +7,10 @@
#include "task/user_task.h"
/* USER INCLUDE BEGIN */
#include "bsp/can.h"
#include "device/motor.h"
#include "device/motor_lk.h"
#include "device/motor_lz.h"
#include "module/config.h"
#include "module/balance_chassis.h"
/* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */
@ -17,16 +18,17 @@
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */
MOTOR_LZ_Param_t motor_lz_param = {
.can = BSP_CAN_3,
.motor_id = 125,
.host_id = 0xFF,
.module = MOTOR_LZ_RSO3,
.reverse = false,
.mode = MOTOR_LZ_MODE_MOTION,
Chassis_t chassis;
Chassis_CMD_t chassis_cmd = {
.mode = CHASSIS_MODE_RELAX, // 改为RECOVER模式，让电机先启动
.move_vec = {
.vx = 0.0f,
.vy = 0.0f,
.wz = 0.0f,
},
.height = 0.0f,
};
float out = 0.0f;
Chassis_IMU_t chassis_imu;
/* USER STRUCT END */
/* Private function --------------------------------------------------------- */
@ -42,19 +44,29 @@ void Task_ctrl_chassis(void *argument) {
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
/* USER CODE INIT BEGIN */
BSP_CAN_Init();
MOTOR_LZ_Init();
MOTOR_LZ_Register(&motor_lz_param);
Chassis_Init(&chassis, &Config_GetRobotParam()->chassis_param, CTRL_CHASSIS_FREQ);
/* USER CODE INIT END */
while (1) {
tick += delay_tick; /* 计算下一个唤醒时刻 */
/* USER CODE BEGIN */
MOTOR_LZ_Enable(&motor_lz_param);
if (osMessageQueueGet(task_runtime.msgq.chassis.cmd, &chassis_cmd, NULL, 0) != osOK) {
// 没有新命令，保持上次命令
}
if (osMessageQueueGet(task_runtime.msgq.chassis.imu, &chassis_imu, NULL, 0) == osOK) {
chassis.feedback.imu = chassis_imu;
}
osMessageQueueGet(task_runtime.msgq.chassis.yaw, &chassis.feedback.yaw, NULL, 0);
Chassis_UpdateFeedback(&chassis);
Chassis_Control(&chassis, &chassis_cmd);
/* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
}
}
}

37
User/task/ctrl_gimbal.c
View File

@ -6,9 +6,10 @@
/* Includes ----------------------------------------------------------------- */
#include "task/user_task.h"
/* USER INCLUDE BEGIN */
#include "component/ahrs.h"
#include "module/gimbal.h"
#include "module/config.h"
#include "bsp/can.h"
#include "device/motor.h"
#include "device/motor_dm.h"
/* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */
@ -16,13 +17,9 @@
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */
MOTOR_DM_Param_t motor_dm_yaw = {
.can = BSP_CAN_1,
.can_id = 0x1,
.master_id = 0x11,
.module = MOTOR_DM_J4310,
.reverse = false,
};
Gimbal_t gimbal;
Gimbal_CMD_t gimbal_cmd;
// BSP_FDCAN_StdDataFrame_t can_frame;
/* USER STRUCT END */
/* Private function --------------------------------------------------------- */
@ -38,14 +35,30 @@ void Task_ctrl_gimbal(void *argument) {
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
/* USER CODE INIT BEGIN */
BSP_CAN_Init();
MOTOR_DM_Register(&motor_dm_yaw);
Gimbal_Init(&gimbal, &Config_GetRobotParam()->gimbal_param, CTRL_GIMBAL_FREQ);
/* USER CODE INIT END */
while (1) {
tick += delay_tick; /* 计算下一个唤醒时刻 */
/* USER CODE BEGIN */
MOTOR_DM_Enable(&motor_dm_yaw);
// can_frame.id = 0x200;
// can_frame.dlc = 8;
// BSP_CAN_TransmitStdDataFrame(BSP_CAN_2, &can_frame);
Gimbal_UpdateIMU(&gimbal);
osMessageQueueGet(task_runtime.msgq.gimbal.cmd, &gimbal_cmd, NULL, 0);
Gimbal_UpdateFeedback(&gimbal);
// osMessageQueueReset(task_runtime.msgq.chassis_yaw);
// osMessageQueuePut(task_runtime.msgq.chassis.yaw, &gimbal.feedback.motor.yaw, 0, 0);
Gimbal_Control(&gimbal, &gimbal_cmd);
Gimbal_Output(&gimbal);
/* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
}

1
User/task/ctrl_shoot.c
View File

@ -44,6 +44,7 @@ void Task_ctrl_shoot(void *argument) {
Shoot_UpdateFeedback(&shoot);
// Shoot_Test(&shoot);
Shoot_Control(&shoot,&shoot_cmd);
/* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
}

8
User/task/init.c
View File

@ -8,6 +8,8 @@
/* USER INCLUDE BEGIN */
#include "module/shoot.h"
#include "module/balance_chassis.h"
#include "module/gimbal.h"
/* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */
@ -32,7 +34,7 @@ void Task_Init(void *argument) {
/* 创建任务线程 */
task_runtime.thread.rc = osThreadNew(Task_rc, NULL, &attr_rc);
task_runtime.thread.atti_esit = osThreadNew(Task_atti_esit, NULL, &attr_atti_esit);
task_runtime.thread.ctrl_chassis = osThreadNew(Task_ctrl_chassis, NULL, &attr_ctrl_chassis);
// task_runtime.thread.ctrl_chassis = osThreadNew(Task_ctrl_chassis, NULL, &attr_ctrl_chassis);
task_runtime.thread.ctrl_gimbal = osThreadNew(Task_ctrl_gimbal, NULL, &attr_ctrl_gimbal);
task_runtime.thread.monitor = osThreadNew(Task_monitor, NULL, &attr_monitor);
task_runtime.thread.blink = osThreadNew(Task_blink, NULL, &attr_blink);
@ -42,6 +44,10 @@ void Task_Init(void *argument) {
/* USER MESSAGE BEGIN */
task_runtime.msgq.user_msg= osMessageQueueNew(2u, 10, NULL);
task_runtime.msgq.shoot.shoot_cmd = osMessageQueueNew(2u, sizeof(Shoot_CMD_t), NULL);
task_runtime.msgq.chassis.imu = osMessageQueueNew(2u, sizeof(Chassis_IMU_t), NULL);
task_runtime.msgq.chassis.cmd = osMessageQueueNew(2u, sizeof(Chassis_CMD_t), NULL);
task_runtime.msgq.chassis.yaw = osMessageQueueNew(2u, sizeof(MOTOR_Feedback_t), NULL);
task_runtime.msgq.gimbal.cmd = osMessageQueueNew(2u, sizeof(Gimbal_CMD_t), NULL);
/* USER MESSAGE END */
osKernelUnlock(); // 解锁内核

83
User/task/rc.c
View File

@ -8,6 +8,8 @@
/* USER INCLUDE BEGIN */
#include "device/dr16.h"
#include "module/shoot.h"
#include "module/balance_chassis.h"
#include "module/gimbal.h"
/* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */
@ -17,7 +19,8 @@
/* USER STRUCT BEGIN */
DR16_t dr16;
Shoot_CMD_t for_shoot;
Chassis_CMD_t cmd_for_chassis;
Gimbal_CMD_t cmd_for_gimbal;
/* USER STRUCT END */
/* Private function --------------------------------------------------------- */
@ -73,33 +76,63 @@ void Task_rc(void *argument) {
// task_runtime.msgq.chassis.cmd); // 重置消息队列，防止阻塞
// osMessageQueuePut(task_runtime.msgq.chassis.cmd, &cmd_for_chassis, 0,
// 0); // 非阻塞发送底盘控制命令
/************************* 云台命令 **************************************/
switch (dr16.data.sw_l) {
case DR16_SW_UP:
cmd_for_gimbal.mode = GIMBAL_MODE_RELAX;
cmd_for_gimbal.delta_yaw = 0.0f;
cmd_for_gimbal.delta_pit = 0.0f;
break;
case DR16_SW_MID:
cmd_for_gimbal.mode = GIMBAL_MODE_ABSOLUTE;
cmd_for_gimbal.delta_yaw = -dr16.data.ch_r_x * 5.0f;
cmd_for_gimbal.delta_pit = dr16.data.ch_r_y * 5.0f;
break;
case DR16_SW_DOWN:
cmd_for_gimbal.mode = GIMBAL_MODE_ABSOLUTE;
cmd_for_gimbal.delta_yaw = -dr16.data.ch_r_x * 5.0f;
cmd_for_gimbal.delta_pit = dr16.data.ch_r_y * 5.0f;
break;
default:
cmd_for_gimbal.mode = GIMBAL_MODE_RELAX;
cmd_for_gimbal.delta_yaw = 0.0f;
cmd_for_gimbal.delta_pit = 0.0f;
break;
}
// switch (dr16.data.sw_l) {
// case DR16_SW_UP:
// cmd_for_gimbal.mode = GIMBAL_MODE_RELAX;
// cmd_for_gimbal.delta_yaw = 0.0f;
// cmd_for_gimbal.delta_pit = 0.0f;
// break;
// case DR16_SW_MID:
// cmd_for_gimbal.mode = GIMBAL_MODE_ABSOLUTE;
// cmd_for_gimbal.delta_yaw = -dr16.data.ch_r_x * 5.0f;
// cmd_for_gimbal.delta_pit = dr16.data.ch_r_y * 5.0f;
// break;
// case DR16_SW_DOWN:
// cmd_for_gimbal.mode = GIMBAL_MODE_ABSOLUTE;
// cmd_for_gimbal.delta_yaw = -dr16.data.ch_r_x * 5.0f;
// cmd_for_gimbal.delta_pit = dr16.data.ch_r_y * 5.0f;
// break;
// default:
// cmd_for_gimbal.mode = GIMBAL_MODE_RELAX;
// cmd_for_gimbal.delta_yaw = 0.0f;
// cmd_for_gimbal.delta_pit = 0.0f;
// break;
// }
osMessageQueueReset(task_runtime.msgq.gimbal.cmd);
osMessageQueuePut(task_runtime.msgq.gimbal.cmd, &cmd_for_gimbal, 0, 0);
// osMessageQueueReset(task_runtime.msgq.gimbal.cmd);
// osMessageQueuePut(task_runtime.msgq.gimbal.cmd, &cmd_for_gimbal, 0, 0);
/************************* 底盘命令 **************************************/
switch (dr16.data.sw_l) {
case DR16_SW_UP:
cmd_for_chassis.mode = CHASSIS_MODE_RELAX;
break;
case DR16_SW_MID:
cmd_for_chassis.mode = CHASSIS_MODE_RECOVER;
// cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
break;
case DR16_SW_DOWN:
// cmd_for_chassis.mode = CHASSIS_MODE_RECOVER;
// cmd_for_chassis.mode = CHASSIS_MODE_ROTOR;
cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
// cmd_for_chassis.mode = CHASSIS_MODE_JUMP;
break;
default:
cmd_for_chassis.mode = CHASSIS_MODE_RELAX;
break;
}
cmd_for_chassis.move_vec.vx = dr16.data.ch_l_y;
cmd_for_chassis.move_vec.vy = dr16.data.ch_l_x;
cmd_for_chassis.move_vec.wz = dr16.data.ch_r_x;
cmd_for_chassis.height = dr16.data.ch_res;
osMessageQueueReset(
task_runtime.msgq.chassis.cmd); // 重置消息队列，防止阻塞
osMessageQueuePut(task_runtime.msgq.chassis.cmd, &cmd_for_chassis, 0,
0); // 非阻塞发送底盘控制命令
/************************* 发射命令 **************************************/
for_shoot.online = dr16.header.online;
switch (dr16.data.sw_r) {
case DR16_SW_UP:

6
User/task/user_task.c
View File

@ -22,12 +22,12 @@ const osThreadAttr_t attr_atti_esit = {
const osThreadAttr_t attr_ctrl_chassis = {
.name = "ctrl_chassis",
.priority = osPriorityNormal,
.stack_size = 256 * 4,
.stack_size = 256 * 8,
};
const osThreadAttr_t attr_ctrl_gimbal = {
.name = "ctrl_gimbal",
.priority = osPriorityNormal,
.stack_size = 256 * 4,
.stack_size = 256 * 8,
};
const osThreadAttr_t attr_monitor = {
.name = "monitor",
@ -42,5 +42,5 @@ const osThreadAttr_t attr_blink = {
const osThreadAttr_t attr_ctrl_shoot = {
.name = "ctrl_shoot",
.priority = osPriorityNormal,
.stack_size = 256 * 4,
.stack_size = 256 * 8,
};

BIN
utils/.DS_Store vendored Normal file
View File

Binary file not shown.

BIN
utils/Simulation-master/.DS_Store vendored Normal file
View File

Binary file not shown.

13
utils/Simulation-master/.gitignore vendored Normal file
View File

@ -0,0 +1,13 @@
# Duplicate files
*(*)*
# MATLAB files
balance/parallel_legs/slprj
balance/series_legs/slprj
balance/series_parallel_legs/slprj
*/slprj/
*/codegen/
*.prj
*.slxc
*.autosave
*.asv

21
utils/Simulation-master/LICENSE Normal file
View File

@ -0,0 +1,21 @@
MIT License
Copyright (c) 2024 是小企鹅呀
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.

BIN
utils/Simulation-master/balance/.DS_Store vendored Normal file
View File

Binary file not shown.

BIN
utils/Simulation-master/balance/parallel_legs/blance_leg.slx Normal file
View File

Binary file not shown.

BIN
utils/Simulation-master/balance/parallel_legs/blance_leg.slx.original Normal file
View File

Binary file not shown.

BIN
utils/Simulation-master/balance/parallel_legs/blance_leg.slx.r2022b Normal file
View File

Binary file not shown.

BIN
utils/Simulation-master/balance/parallel_legs/blance_leg.slx.r2023a Normal file
View File

Binary file not shown.

20
utils/Simulation-master/balance/parallel_legs/d_phi0.m Normal file
View File

@ -0,0 +1,20 @@
syms phi_0 phi_1 phi_2 phi_4 l1 l2 l3 l4 l5 L0 d_phi1 d_phi4
XD=l5+l4*cos(phi_4);
YD=l4*sin(phi_4);
XB=l1*cos(phi_1);
YB=l1*sin(phi_1);
A0=2*l2*(XD-XB);
B0=2*l2*(YD-YB);
lBD=sqrt((XD-XB)^2+(YD-YB)^2);
C0=l2^2+lBD^2-l3^2;
phi_2=2*atan((B0+sqrt(A0^2+B0^2-C0^2))/(A0+C0));
XC=XB+l2*cos(phi_2);
YC=YB+l2*sin(phi_2);
L0=sqrt((XC-l5/2)^2+YC^2);
phi_0=atan(YC/(XC-l5/2));
J=jacobian([L0;phi_0],[phi_1;phi_4]);
phi0_dot = J(2,1)*d_phi1 +J(2,2)*d_phi4

19
utils/Simulation-master/balance/parallel_legs/display_polynomial.m Normal file
View File

@ -0,0 +1,19 @@
% 定义一个函数来显示多项式方程
function display_polynomial(coefficients, name)
equation = sprintf('%s = ', name);
n = length(coefficients);
for i = 1:n
if coefficients(i) ~= 0
if i == n
term = sprintf('%.4f', coefficients(i));
else
term = sprintf('%.4f*t%d', coefficients(i), n-i);
end
if i > 1 && coefficients(i) > 0
term = [' + ', term];
end
equation = [equation, term];
end
end
fprintf('%s;\n', equation);
end

82
utils/Simulation-master/balance/parallel_legs/get_k.m Normal file
View File

@ -0,0 +1,82 @@
%计算不同腿长下适合的K矩阵，再进行多项式拟合，得到2*6矩阵每个参数对应的多项式参数
tic
j=1;
leg=0.1:0.01:0.4;
for i=leg
k=get_k_length(i);
k11(j) = k(1,1);
k12(j) = k(1,2);
k13(j) = k(1,3);
k14(j) = k(1,4);
k15(j) = k(1,5);
k16(j) = k(1,6);
k21(j) = k(2,1);
k22(j) = k(2,2);
k23(j) = k(2,3);
k24(j) = k(2,4);
k25(j) = k(2,5);
k26(j) = k(2,6);
j=j+1;
fprintf('leg_length=%d\n', i);
end
a11=polyfit(leg,k11,3);
a12=polyfit(leg,k12,3);
a13=polyfit(leg,k13,3);
a14=polyfit(leg,k14,3);
a15=polyfit(leg,k15,3);
a16=polyfit(leg,k16,3);
a21=polyfit(leg,k21,3);
a22=polyfit(leg,k22,3);
a23=polyfit(leg,k23,3);
a24=polyfit(leg,k24,3);
a25=polyfit(leg,k25,3);
a26=polyfit(leg,k26,3);
toc
% x0=leg; %步长为0.1
% y11=polyval(a11,x0); %返回值y0是对应于x0的函数值
% y12=polyval(a12,x0); %返回值y0是对应于x0的函数值
% y13=polyval(a13,x0); %返回值y0是对应于x0的函数值
% y14=polyval(a14,x0); %返回值y0是对应于x0的函数值
% y15=polyval(a15,x0); %返回值y0是对应于x0的函数值
% y16=polyval(a16,x0); %返回值y0是对应于x0的函数值
%
% y21=polyval(a21,x0); %返回值y0是对应于x0的函数值
% y22=polyval(a22,x0); %返回值y0是对应于x0的函数值
% y23=polyval(a23,x0); %返回值y0是对应于x0的函数值
% y24=polyval(a24,x0); %返回值y0是对应于x0的函数值
% y25=polyval(a25,x0); %返回值y0是对应于x0的函数值
% y26=polyval(a26,x0); %返回值y0是对应于x0的函数值
% subplot(3,4,1);plot(leg,k11,'o',x0,y11,'r');xlabel('x');ylabel('y');title('k11');
% subplot(3,4,2);plot(leg,k12,'o',x0,y12,'r');xlabel('x');ylabel('y');title('k12');
% subplot(3,4,5);plot(leg,k13,'o',x0,y13,'r');xlabel('x');ylabel('y');title('k13');
% subplot(3,4,6);plot(leg,k14,'o',x0,y14,'r');xlabel('x');ylabel('y');title('k14');
% subplot(3,4,9);plot(leg,k15,'o',x0,y15,'r');xlabel('x');ylabel('y');title('k15');
% subplot(3,4,10);plot(leg,k16,'o',x0,y16,'r');xlabel('x');ylabel('y');title('k16');
%
% subplot(3,4,3);plot(leg,k21,'o',x0,y21,'r');xlabel('x');ylabel('y');title('k21');
% subplot(3,4,4);plot(leg,k22,'o',x0,y22,'r');xlabel('x');ylabel('y');title('k22');
% subplot(3,4,7);plot(leg,k23,'o',x0,y23,'r');xlabel('x');ylabel('y');title('k23');
% subplot(3,4,8);plot(leg,k24,'o',x0,y24,'r');xlabel('x');ylabel('y');title('k24');
% subplot(3,4,11);plot(leg,k25,'o',x0,y25,'r');xlabel('x');ylabel('y');title('k25');
% subplot(3,4,12);plot(leg,k26,'o',x0,y26,'r');xlabel('x');ylabel('y');title('k26');
% grid on; %添加网格线
% set(gca,'GridLineStyle',':','GridColor','k','GridAlpha',1); %将网格线变成虚线
% fprintf('fp32 a11[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a11(1),a11(2),a11(3),a11(4));
% fprintf('fp32 a12[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a12(1),a12(2),a12(3),a12(4));
% fprintf('fp32 a13[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a13(1),a13(2),a13(3),a13(4));
% fprintf('fp32 a14[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a14(1),a14(2),a14(3),a14(4));
% fprintf('fp32 a15[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a15(1),a15(2),a15(3),a15(4));
% fprintf('fp32 a16[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a16(1),a16(2),a16(3),a16(4));
%
% fprintf('fp32 a21[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a21(1),a21(2),a21(3),a21(4));
% fprintf('fp32 a22[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a22(1),a22(2),a22(3),a22(4));
% fprintf('fp32 a23[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a23(1),a23(2),a23(3),a23(4));
% fprintf('fp32 a24[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a24(1),a24(2),a24(3),a24(4));
% fprintf('fp32 a25[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a25(1),a25(2),a25(3),a25(4));
% fprintf('fp32 a26[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a26(1),a26(2),a26(3),a26(4));
toc

56
utils/Simulation-master/balance/parallel_legs/get_k_length.m Normal file
View File

@ -0,0 +1,56 @@
function K = get_k_length(leg_length)
%theta : 摆杆与竖直方向夹角 R ：驱动轮半径
%x : 驱动轮位移 L : 摆杆重心到驱动轮轴距离
%phi : 机体与水平夹角 LM : 摆杆重心到其转轴距离
%T ：驱动轮输出力矩 l : 机体重心到其转轴距离
%Tp : 髋关节输出力矩 mw : 驱动轮转子质量
%N ：驱动轮对摆杆力的水平分量 mp : 摆杆质量
%P ：驱动轮对摆杆力的竖直分量 M : 机体质量
%Nm ：摆杆对机体力水平方向分量 Iw : 驱动轮转子转动惯量
%Pm ：摆杆对机体力竖直方向分量 Ip : 摆杆绕质心转动惯量
%Nf : 地面对驱动轮摩擦力 Im : 机体绕质心转动惯量
syms x(t) T R Iw mw M L LM theta(t) l phi(t) mp g Tp Ip IM
syms f1 f2 f3 d_theta d_x d_phi theta0 x0 phi0
R1=0.072; %驱动轮半径
L1=leg_length/2; %摆杆重心到驱动轮轴距离
LM1=leg_length/2; %摆杆重心到其转轴距离
l1=-0.01; %机体质心距离转轴距离
mw1=0.80; %驱动轮质量
mp1=1.11; %杆质量
M1=12.0; %机体质量
Iw1=mw1*R1^2; %驱动轮转动惯量
Ip1=mp1*((L1+LM1)^2+0.05^2)/12.0; %摆杆转动惯量
IM1=M1*(0.3^2+0.12^2)/12.0; %机体绕质心转动惯量
NM = M*diff(x + (L + LM )*sin(theta)-l*sin(phi),t,2);
N = NM + mp*diff(x + L*sin(theta),t,2);
PM = M*g + M*diff((L+LM)*cos(theta)+l*cos(phi),t,2);
P = PM +mp*g+mp*diff(L*cos(theta),t,2);
eqn1 = diff(x,t,2) == (T -N*R)/(Iw/R + mw*R);
eqn2 = Ip*diff(theta,t,2) == (P*L + PM*LM)*sin(theta)-(N*L+NM*LM)*cos(theta)-T+Tp;
eqn3 = IM*diff(phi,t,2) == Tp +NM*l*cos(phi)+PM*l*sin(phi);
eqn10 = subs(subs(subs(subs(subs(subs(subs(subs(subs(eqn1,diff(theta,t,2),f1),diff(x,t,2),f2),diff(phi,t,2),f3),diff(theta,t),d_theta),diff(x,t),d_x),diff(phi,t),d_phi),theta,theta0),x,x0),phi,phi0);
eqn20 = subs(subs(subs(subs(subs(subs(subs(subs(subs(eqn2,diff(theta,t,2),f1),diff(x,t,2),f2),diff(phi,t,2),f3),diff(theta,t),d_theta),diff(x,t),d_x),diff(phi,t),d_phi),theta,theta0),x,x0),phi,phi0);
eqn30 = subs(subs(subs(subs(subs(subs(subs(subs(subs(eqn3,diff(theta,t,2),f1),diff(x,t,2),f2),diff(phi,t,2),f3),diff(theta,t),d_theta),diff(x,t),d_x),diff(phi,t),d_phi),theta,theta0),x,x0),phi,phi0);
[f1,f2,f3] = solve(eqn10,eqn20,eqn30,f1,f2,f3);
A=subs(jacobian([d_theta,f1,d_x,f2,d_phi,f3],[theta0,d_theta,x0,d_x,phi0,d_phi]),[theta0,d_theta,d_x,phi0,d_phi,T,Tp],[0,0,0,0,0,0,0]);
A=subs(A,[R,L,LM,l,mw,mp,M,Iw,Ip,IM,g],[R1,L1,LM1,l1,mw1,mp1,M1,Iw1,Ip1,IM1,9.8]);
A=double(A);
B=subs(jacobian([d_theta,f1,d_x,f2,d_phi,f3],[T,Tp]),[theta0,d_theta,d_x,phi0,d_phi,T,Tp],[0,0,0,0,0,0,0]);
B=subs(B,[R,L,LM,l,mw,mp,M,Iw,Ip,IM,g],[R1,L1,LM1,l1,mw1,mp1,M1,Iw1,Ip1,IM1,9.8]);
B=double(B);
Q=diag(1000 1 200 200 5000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
R=[150 0;0 25]; %T Tp
K=lqr(A,B,Q,R);
end

BIN
utils/Simulation-master/balance/parallel_legs/physical_calc.mlx Normal file
View File

Binary file not shown.

BIN
utils/Simulation-master/balance/series_legs/blance_leg.slx Normal file
View File

Binary file not shown.

BIN
utils/Simulation-master/balance/series_legs/calc.mlx Normal file
View File

Binary file not shown.

BIN
utils/Simulation-master/balance/series_legs/doc/2_link.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 22 KiB

154
utils/Simulation-master/balance/series_legs/doc/2连杆分析.md Normal file
View File

@ -0,0 +1,154 @@
# 2连杆分析
参考：[五连杆运动学解算与VMC](https://zhuanlan.zhihu.com/p/613007726)
![pic](./2_link.png)
## 1 正运动学解算
$\phi_1$和$\phi_2$可由电机编码器测量得到。
$C$点坐标：
$$
\left \{
\begin{array}{l}
x_C = l_1\cos\phi_1 + l_2\cos\phi_2\\
y_C = l_1\sin\phi_1 + l_2\sin\phi_2
\end{array}
\right .
$$
得：
$$
\left \{
\begin{array}{l}
L0 = \sqrt{x_C^2 + y_C^2} \\
\phi_0 = \arctan{\frac{y_C}{x_C}}
\end{array}
\right .
$$
## 2 逆运动学解算
由余弦定理易得：
$$
\phi_2+\phi_3 = \arccos\frac{l_1^2+l_2^2-L_0^2}{2l_1l_2}
$$
又：
$$
\phi_3 = \pi - \phi_1
$$
得：
$$
\phi_2 = \arccos\frac{l_1^2+l_2^2-L_0^2}{2l_1l_2}+\phi_1-\pi
$$
## 3 雅可比矩阵
<!-- $$
\left [
\begin{matrix}
\dot L0 \\
\dot \phi_0
\end{matrix}
\right ]
=
J
\left [
\begin{matrix}
\dot\phi_1 \\
\dot\phi_2
\end{matrix}
\right ]
$$ -->
基于文中描述可得：
$$
\left \{
\begin{array}{l}
\dot x_C = -l_1\dot\phi_1\sin\phi_1 - l_2\dot\phi_2\sin\phi_2\\
\dot y_C = l_1\dot\phi_1\cos\phi_1 + l_2\dot\phi_2\cos\phi_2
\end{array}
\right .
$$
即：
$$
\left [
\begin{matrix}
\dot x_C \\
\dot y_C
\end{matrix}
\right ]
=
\left [
\begin{matrix}
-l_1\sin\phi_1 & -l_2\sin\phi_2 \\
l_1\cos\phi_1 & l_2\cos\phi_2
\end{matrix}
\right ]
\left [
\begin{matrix}
\dot\phi_1 \\
\dot\phi_2
\end{matrix}
\right ]
$$
记作：
$$
\left [
\begin{matrix}
\dot x_C \\
\dot y_C
\end{matrix}
\right ]
=
J_0
\left [
\begin{matrix}
\dot\phi_1 \\
\dot\phi_2
\end{matrix}
\right ]
$$
下面操作与文中相同，可得：
$$
J^T = J_0^TRM =
\left[
\begin{matrix}
l_1 \,\sin \left(\phi_0 -\phi_1 \right) & \frac{l_1 \,\cos \left(\phi_0 -\phi_1 \right)}{L_0 }\\
l_2 \,\sin \left(\phi_0 -\phi_2 \right) & \frac{l_2 \,\cos \left(\phi_0 -\phi_2 \right)}{L_0 }
\end{matrix}
\right]
$$
即：
$$
J =
\left[
\begin{matrix}
l_1 \,\sin \left(\phi_0 -\phi_1 \right) & l_2 \,\sin \left(\phi_0 -\phi_2 \right)\\
\frac{l_1 \,\cos \left(\phi_0 -\phi_1 \right)}{L_0 } & \frac{l_2 \,\cos \left(\phi_0 -\phi_2 \right)}{L_0 }
\end{matrix}
\right]
$$

96
utils/Simulation-master/balance/series_legs/get_k.m Normal file
View File

@ -0,0 +1,96 @@
%计算不同腿长下适合的K矩阵，再进行多项式拟合，得到2*6矩阵每个参数对应的多项式参数
tic
j=1;
leg=0.1:0.01:0.4;
for i=leg
k=get_k_length(i);
k11(j) = k(1,1);
k12(j) = k(1,2);
k13(j) = k(1,3);
k14(j) = k(1,4);
k15(j) = k(1,5);
k16(j) = k(1,6);
k21(j) = k(2,1);
k22(j) = k(2,2);
k23(j) = k(2,3);
k24(j) = k(2,4);
k25(j) = k(2,5);
k26(j) = k(2,6);
j=j+1;
fprintf('leg_length=%d\n', i);
end
a11=polyfit(leg,k11,3);
a12=polyfit(leg,k12,3);
a13=polyfit(leg,k13,3);
a14=polyfit(leg,k14,3);
a15=polyfit(leg,k15,3);
a16=polyfit(leg,k16,3);
a21=polyfit(leg,k21,3);
a22=polyfit(leg,k22,3);
a23=polyfit(leg,k23,3);
a24=polyfit(leg,k24,3);
a25=polyfit(leg,k25,3);
a26=polyfit(leg,k26,3);
toc
% 打印格式化的C代码结构，可以直接复制到config.c中
fprintf('\n=========== 可直接复制的C代码 ===========\n');
fprintf('.lqr_gains = {\n');
fprintf(' .k11_coeff = { %.15ef, %.15ef, %.15ef, %.15ef }, // theta\n', a11(1), a11(2), a11(3), a11(4));
fprintf(' .k12_coeff = { %.15ef, %.15ef, %.15ef, %.15ef }, // d_theta\n', a12(1), a12(2), a12(3), a12(4));
fprintf(' .k13_coeff = { %.15ef, %.15ef, %.15ef, %.15ef }, // x\n', a13(1), a13(2), a13(3), a13(4));
fprintf(' .k14_coeff = { %.15ef, %.15ef, %.15ef, %.15ef }, // d_x\n', a14(1), a14(2), a14(3), a14(4));
fprintf(' .k15_coeff = { %.15ef, %.15ef, %.15ef, %.15ef }, // phi\n', a15(1), a15(2), a15(3), a15(4));
fprintf(' .k16_coeff = { %.15ef, %.15ef, %.15ef, %.15ef }, // d_phi\n', a16(1), a16(2), a16(3), a16(4));
fprintf(' .k21_coeff = { %.15ef, %.15ef, %.15ef, %.15ef }, // theta\n', a21(1), a21(2), a21(3), a21(4));
fprintf(' .k22_coeff = { %.15ef, %.15ef, %.15ef, %.15ef }, // d_theta\n', a22(1), a22(2), a22(3), a22(4));
fprintf(' .k23_coeff = { %.15ef, %.15ef, %.15ef, %.15ef }, // x\n', a23(1), a23(2), a23(3), a23(4));
fprintf(' .k24_coeff = { %.15ef, %.15ef, %.15ef, %.15ef }, // d_x\n', a24(1), a24(2), a24(3), a24(4));
fprintf(' .k25_coeff = { %.15ef, %.15ef, %.15ef, %.15ef }, // phi\n', a25(1), a25(2), a25(3), a25(4));
fprintf(' .k26_coeff = { %.15ef, %.15ef, %.15ef, %.15ef }, // d_phi\n', a26(1), a26(2), a26(3), a26(4));
fprintf('},\n');
fprintf('========================================\n\n');
% 可选：显示拟合曲线图
% x0=leg; %步长为0.1
% y11=polyval(a11,x0); %返回值y0是对应于x0的函数值
% y12=polyval(a12,x0); %返回值y0是对应于x0的函数值
% y13=polyval(a13,x0); %返回值y0是对应于x0的函数值
% y14=polyval(a14,x0); %返回值y0是对应于x0的函数值
% y15=polyval(a15,x0); %返回值y0是对应于x0的函数值
% y16=polyval(a16,x0); %返回值y0是对应于x0的函数值
%
% y21=polyval(a21,x0); %返回值y0是对应于x0的函数值
% y22=polyval(a22,x0); %返回值y0是对应于x0的函数值
% y23=polyval(a23,x0); %返回值y0是对应于x0的函数值
% y24=polyval(a24,x0); %返回值y0是对应于x0的函数值
% y25=polyval(a25,x0); %返回值y0是对应于x0的函数值
% y26=polyval(a26,x0); %返回值y0是对应于x0的函数值
%
% figure;
% subplot(3,4,1);plot(leg,k11,'o',x0,y11,'r');xlabel('leg length');ylabel('k11');title('k11拟合');
% subplot(3,4,2);plot(leg,k12,'o',x0,y12,'r');xlabel('leg length');ylabel('k12');title('k12拟合');
% subplot(3,4,3);plot(leg,k13,'o',x0,y13,'r');xlabel('leg length');ylabel('k13');title('k13拟合');
% subplot(3,4,4);plot(leg,k14,'o',x0,y14,'r');xlabel('leg length');ylabel('k14');title('k14拟合');
% subplot(3,4,5);plot(leg,k15,'o',x0,y15,'r');xlabel('leg length');ylabel('k15');title('k15拟合');
% subplot(3,4,6);plot(leg,k16,'o',x0,y16,'r');xlabel('leg length');ylabel('k16');title('k16拟合');
%
% subplot(3,4,7);plot(leg,k21,'o',x0,y21,'r');xlabel('leg length');ylabel('k21');title('k21拟合');
% subplot(3,4,8);plot(leg,k22,'o',x0,y22,'r');xlabel('leg length');ylabel('k22');title('k22拟合');
% subplot(3,4,9);plot(leg,k23,'o',x0,y23,'r');xlabel('leg length');ylabel('k23');title('k23拟合');
% subplot(3,4,10);plot(leg,k24,'o',x0,y24,'r');xlabel('leg length');ylabel('k24');title('k24拟合');
% subplot(3,4,11);plot(leg,k25,'o',x0,y25,'r');xlabel('leg length');ylabel('k25');title('k25拟合');
% subplot(3,4,12);plot(leg,k26,'o',x0,y26,'r');xlabel('leg length');ylabel('k26');title('k26拟合');
%
% for i = 1:12
% subplot(3,4,i);
% grid on; %添加网格线
% set(gca,'GridLineStyle',':','GridColor','k','GridAlpha',0.5); %将网格线变成虚线
% legend('原始数据','拟合曲线','Location','best');
% end
toc

56
utils/Simulation-master/balance/series_legs/get_k_length.m Normal file
View File

@ -0,0 +1,56 @@
function K = get_k_length(leg_length)
%theta : 摆杆与竖直方向夹角 R ：驱动轮半径
%x : 驱动轮位移 L : 摆杆重心到驱动轮轴距离
%phi : 机体与水平夹角 LM : 摆杆重心到其转轴距离
%T ：驱动轮输出力矩 l : 机体重心到其转轴距离
%Tp : 髋关节输出力矩 mw : 驱动轮转子质量
%N ：驱动轮对摆杆力的水平分量 mp : 摆杆质量
%P ：驱动轮对摆杆力的竖直分量 M : 机体质量
%Nm ：摆杆对机体力水平方向分量 Iw : 驱动轮转子转动惯量
%Pm ：摆杆对机体力竖直方向分量 Ip : 摆杆绕质心转动惯量
%Nf : 地面对驱动轮摩擦力 Im : 机体绕质心转动惯量
syms x(t) T R Iw mw M L LM theta(t) l phi(t) mp g Tp Ip IM
syms f1 f2 f3 d_theta d_x d_phi theta0 x0 phi0
R1=0.068; %驱动轮半径
L1=leg_length/2; %摆杆重心到驱动轮轴距离
LM1=leg_length/2; %摆杆重心到其转轴距离
l1=-0.03; %机体质心距离转轴距离
mw1=0.60; %驱动轮质量
mp1=1.8; %杆质量
M1=12.0; %机体质量
Iw1=mw1*R1^2; %驱动轮转动惯量
Ip1=mp1*((L1+LM1)^2+0.05^2)/12.0; %摆杆转动惯量
IM1=M1*(0.3^2+0.12^2)/12.0; %机体绕质心转动惯量
NM = M*diff(x + (L + LM )*sin(theta)-l*sin(phi),t,2);
N = NM + mp*diff(x + L*sin(theta),t,2);
PM = M*g + M*diff((L+LM)*cos(theta)+l*cos(phi),t,2);
P = PM +mp*g+mp*diff(L*cos(theta),t,2);
eqn1 = diff(x,t,2) == (T -N*R)/(Iw/R + mw*R);
eqn2 = Ip*diff(theta,t,2) == (P*L + PM*LM)*sin(theta)-(N*L+NM*LM)*cos(theta)-T+Tp;
eqn3 = IM*diff(phi,t,2) == Tp +NM*l*cos(phi)+PM*l*sin(phi);
eqn10 = subs(subs(subs(subs(subs(subs(subs(subs(subs(eqn1,diff(theta,t,2),f1),diff(x,t,2),f2),diff(phi,t,2),f3),diff(theta,t),d_theta),diff(x,t),d_x),diff(phi,t),d_phi),theta,theta0),x,x0),phi,phi0);
eqn20 = subs(subs(subs(subs(subs(subs(subs(subs(subs(eqn2,diff(theta,t,2),f1),diff(x,t,2),f2),diff(phi,t,2),f3),diff(theta,t),d_theta),diff(x,t),d_x),diff(phi,t),d_phi),theta,theta0),x,x0),phi,phi0);
eqn30 = subs(subs(subs(subs(subs(subs(subs(subs(subs(eqn3,diff(theta,t,2),f1),diff(x,t,2),f2),diff(phi,t,2),f3),diff(theta,t),d_theta),diff(x,t),d_x),diff(phi,t),d_phi),theta,theta0),x,x0),phi,phi0);
[f1,f2,f3] = solve(eqn10,eqn20,eqn30,f1,f2,f3);
A=subs(jacobian([d_theta,f1,d_x,f2,d_phi,f3],[theta0,d_theta,x0,d_x,phi0,d_phi]),[theta0,d_theta,d_x,phi0,d_phi,T,Tp],[0,0,0,0,0,0,0]);
A=subs(A,[R,L,LM,l,mw,mp,M,Iw,Ip,IM,g],[R1,L1,LM1,l1,mw1,mp1,M1,Iw1,Ip1,IM1,9.8]);
A=double(A);
B=subs(jacobian([d_theta,f1,d_x,f2,d_phi,f3],[T,Tp]),[theta0,d_theta,d_x,phi0,d_phi,T,Tp],[0,0,0,0,0,0,0]);
B=subs(B,[R,L,LM,l,mw,mp,M,Iw,Ip,IM,g],[R1,L1,LM1,l1,mw1,mp1,M1,Iw1,Ip1,IM1,9.8]);
B=double(B);
Q=diag([1500 100 2500 1500 8000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
R=[250 0;0 50]; %T Tp
K=lqr(A,B,Q,R);
end

BIN
utils/Simulation-master/balance/series_legs/leg_sim.slx Normal file
View File

Binary file not shown.

3
utils/Simulation-master/balance/series_parallel_legs/README.md Normal file
View File

@ -0,0 +1,3 @@
# 串并联腿仿真总结
这次仿真对小企鹅来说最重要的一点就是确定了替换为串联腿时不需要替换模型，直接移植现有并联腿模型即可。

BIN
utils/Simulation-master/balance/series_parallel_legs/blance_leg.slx Normal file
View File

Binary file not shown.

BIN
utils/Simulation-master/balance/series_parallel_legs/blance_leg.slx.original Normal file
View File

Binary file not shown.

BIN
utils/Simulation-master/balance/series_parallel_legs/blance_leg.slx.r2022b Normal file
View File

Binary file not shown.

BIN
utils/Simulation-master/balance/series_parallel_legs/blance_leg.slx.r2023a Normal file
View File

Binary file not shown.

20
utils/Simulation-master/balance/series_parallel_legs/d_phi0.m Normal file
View File

@ -0,0 +1,20 @@
syms phi_0 phi_1 phi_2 phi_4 l1 l2 l3 l4 l5 L0 d_phi1 d_phi4
XD=l5+l4*cos(phi_4);
YD=l4*sin(phi_4);
XB=l1*cos(phi_1);
YB=l1*sin(phi_1);
A0=2*l2*(XD-XB);
B0=2*l2*(YD-YB);
lBD=sqrt((XD-XB)^2+(YD-YB)^2);
C0=l2^2+lBD^2-l3^2;
phi_2=2*atan((B0+sqrt(A0^2+B0^2-C0^2))/(A0+C0));
XC=XB+l2*cos(phi_2);
YC=YB+l2*sin(phi_2);
L0=sqrt((XC-l5/2)^2+YC^2);
phi_0=atan(YC/(XC-l5/2));
J=jacobian([L0;phi_0],[phi_1;phi_4]);
phi0_dot = J(2,1)*d_phi1 +J(2,2)*d_phi4

19
utils/Simulation-master/balance/series_parallel_legs/display_polynomial.m Normal file
View File

@ -0,0 +1,19 @@
% 定义一个函数来显示多项式方程
function display_polynomial(coefficients, name)
equation = sprintf('%s = ', name);
n = length(coefficients);
for i = 1:n
if coefficients(i) ~= 0
if i == n
term = sprintf('%.4f', coefficients(i));
else
term = sprintf('%.4f*t%d', coefficients(i), n-i);
end
if i > 1 && coefficients(i) > 0
term = [' + ', term];
end
equation = [equation, term];
end
end
fprintf('%s;\n', equation);
end

82
utils/Simulation-master/balance/series_parallel_legs/get_k.m Normal file
View File

@ -0,0 +1,82 @@
%计算不同腿长下适合的K矩阵，再进行多项式拟合，得到2*6矩阵每个参数对应的多项式参数
tic
j=1;
leg=0.1:0.01:0.4;
for i=leg
k=get_k_length(i);
k11(j) = k(1,1);
k12(j) = k(1,2);
k13(j) = k(1,3);
k14(j) = k(1,4);
k15(j) = k(1,5);
k16(j) = k(1,6);
k21(j) = k(2,1);
k22(j) = k(2,2);
k23(j) = k(2,3);
k24(j) = k(2,4);
k25(j) = k(2,5);
k26(j) = k(2,6);
j=j+1;
fprintf('leg_length=%d\n', i);
end
a11=polyfit(leg,k11,3);
a12=polyfit(leg,k12,3);
a13=polyfit(leg,k13,3);
a14=polyfit(leg,k14,3);
a15=polyfit(leg,k15,3);
a16=polyfit(leg,k16,3);
a21=polyfit(leg,k21,3);
a22=polyfit(leg,k22,3);
a23=polyfit(leg,k23,3);
a24=polyfit(leg,k24,3);
a25=polyfit(leg,k25,3);
a26=polyfit(leg,k26,3);
toc
% x0=leg; %步长为0.1
% y11=polyval(a11,x0); %返回值y0是对应于x0的函数值
% y12=polyval(a12,x0); %返回值y0是对应于x0的函数值
% y13=polyval(a13,x0); %返回值y0是对应于x0的函数值
% y14=polyval(a14,x0); %返回值y0是对应于x0的函数值
% y15=polyval(a15,x0); %返回值y0是对应于x0的函数值
% y16=polyval(a16,x0); %返回值y0是对应于x0的函数值
%
% y21=polyval(a21,x0); %返回值y0是对应于x0的函数值
% y22=polyval(a22,x0); %返回值y0是对应于x0的函数值
% y23=polyval(a23,x0); %返回值y0是对应于x0的函数值
% y24=polyval(a24,x0); %返回值y0是对应于x0的函数值
% y25=polyval(a25,x0); %返回值y0是对应于x0的函数值
% y26=polyval(a26,x0); %返回值y0是对应于x0的函数值
% subplot(3,4,1);plot(leg,k11,'o',x0,y11,'r');xlabel('x');ylabel('y');title('k11');
% subplot(3,4,2);plot(leg,k12,'o',x0,y12,'r');xlabel('x');ylabel('y');title('k12');
% subplot(3,4,5);plot(leg,k13,'o',x0,y13,'r');xlabel('x');ylabel('y');title('k13');
% subplot(3,4,6);plot(leg,k14,'o',x0,y14,'r');xlabel('x');ylabel('y');title('k14');
% subplot(3,4,9);plot(leg,k15,'o',x0,y15,'r');xlabel('x');ylabel('y');title('k15');
% subplot(3,4,10);plot(leg,k16,'o',x0,y16,'r');xlabel('x');ylabel('y');title('k16');
%
% subplot(3,4,3);plot(leg,k21,'o',x0,y21,'r');xlabel('x');ylabel('y');title('k21');
% subplot(3,4,4);plot(leg,k22,'o',x0,y22,'r');xlabel('x');ylabel('y');title('k22');
% subplot(3,4,7);plot(leg,k23,'o',x0,y23,'r');xlabel('x');ylabel('y');title('k23');
% subplot(3,4,8);plot(leg,k24,'o',x0,y24,'r');xlabel('x');ylabel('y');title('k24');
% subplot(3,4,11);plot(leg,k25,'o',x0,y25,'r');xlabel('x');ylabel('y');title('k25');
% subplot(3,4,12);plot(leg,k26,'o',x0,y26,'r');xlabel('x');ylabel('y');title('k26');
% grid on; %添加网格线
% set(gca,'GridLineStyle',':','GridColor','k','GridAlpha',1); %将网格线变成虚线
% fprintf('fp32 a11[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a11(1),a11(2),a11(3),a11(4));
% fprintf('fp32 a12[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a12(1),a12(2),a12(3),a12(4));
% fprintf('fp32 a13[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a13(1),a13(2),a13(3),a13(4));
% fprintf('fp32 a14[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a14(1),a14(2),a14(3),a14(4));
% fprintf('fp32 a15[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a15(1),a15(2),a15(3),a15(4));
% fprintf('fp32 a16[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a16(1),a16(2),a16(3),a16(4));
%
% fprintf('fp32 a21[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a21(1),a21(2),a21(3),a21(4));
% fprintf('fp32 a22[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a22(1),a22(2),a22(3),a22(4));
% fprintf('fp32 a23[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a23(1),a23(2),a23(3),a23(4));
% fprintf('fp32 a24[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a24(1),a24(2),a24(3),a24(4));
% fprintf('fp32 a25[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a25(1),a25(2),a25(3),a25(4));
% fprintf('fp32 a26[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a26(1),a26(2),a26(3),a26(4));
toc

56
utils/Simulation-master/balance/series_parallel_legs/get_k_length.m Normal file
View File

@ -0,0 +1,56 @@
function K = get_k_length(leg_length)
%theta : 摆杆与竖直方向夹角 R ：驱动轮半径
%x : 驱动轮位移 L : 摆杆重心到驱动轮轴距离
%phi : 机体与水平夹角 LM : 摆杆重心到其转轴距离
%T ：驱动轮输出力矩 l : 机体重心到其转轴距离
%Tp : 髋关节输出力矩 mw : 驱动轮转子质量
%N ：驱动轮对摆杆力的水平分量 mp : 摆杆质量
%P ：驱动轮对摆杆力的竖直分量 M : 机体质量
%Nm ：摆杆对机体力水平方向分量 Iw : 驱动轮转子转动惯量
%Pm ：摆杆对机体力竖直方向分量 Ip : 摆杆绕质心转动惯量
%Nf : 地面对驱动轮摩擦力 Im : 机体绕质心转动惯量
syms x(t) T R Iw mw M L LM theta(t) l phi(t) mp g Tp Ip IM
syms f1 f2 f3 d_theta d_x d_phi theta0 x0 phi0
R1=0.086; %驱动轮半径
L1=leg_length/2; %摆杆重心到驱动轮轴距离
LM1=leg_length/2; %摆杆重心到其转轴距离
l1=0.03; %机体质心距离转轴距离
mw1=1.18; %驱动轮质量
mp1=1.11; %杆质量
M1=10.3; %机体质量
Iw1=mw1*R1^2; %驱动轮转动惯量
Ip1=mp1*((L1+LM1)^2+0.05^2)/12.0; %摆杆转动惯量
IM1=M1*(0.3^2+0.12^2)/12.0; %机体绕质心转动惯量
NM = M*diff(x + (L + LM )*sin(theta)-l*sin(phi),t,2);
N = NM + mp*diff(x + L*sin(theta),t,2);
PM = M*g + M*diff((L+LM)*cos(theta)+l*cos(phi),t,2);
P = PM +mp*g+mp*diff(L*cos(theta),t,2);
eqn1 = diff(x,t,2) == (T -N*R)/(Iw/R + mw*R);
eqn2 = Ip*diff(theta,t,2) == (P*L + PM*LM)*sin(theta)-(N*L+NM*LM)*cos(theta)-T+Tp;
eqn3 = IM*diff(phi,t,2) == Tp +NM*l*cos(phi)+PM*l*sin(phi);
eqn10 = subs(subs(subs(subs(subs(subs(subs(subs(subs(eqn1,diff(theta,t,2),f1),diff(x,t,2),f2),diff(phi,t,2),f3),diff(theta,t),d_theta),diff(x,t),d_x),diff(phi,t),d_phi),theta,theta0),x,x0),phi,phi0);
eqn20 = subs(subs(subs(subs(subs(subs(subs(subs(subs(eqn2,diff(theta,t,2),f1),diff(x,t,2),f2),diff(phi,t,2),f3),diff(theta,t),d_theta),diff(x,t),d_x),diff(phi,t),d_phi),theta,theta0),x,x0),phi,phi0);
eqn30 = subs(subs(subs(subs(subs(subs(subs(subs(subs(eqn3,diff(theta,t,2),f1),diff(x,t,2),f2),diff(phi,t,2),f3),diff(theta,t),d_theta),diff(x,t),d_x),diff(phi,t),d_phi),theta,theta0),x,x0),phi,phi0);
[f1,f2,f3] = solve(eqn10,eqn20,eqn30,f1,f2,f3);
A=subs(jacobian([d_theta,f1,d_x,f2,d_phi,f3],[theta0,d_theta,x0,d_x,phi0,d_phi]),[theta0,d_theta,d_x,phi0,d_phi,T,Tp],[0,0,0,0,0,0,0]);
A=subs(A,[R,L,LM,l,mw,mp,M,Iw,Ip,IM,g],[R1,L1,LM1,l1,mw1,mp1,M1,Iw1,Ip1,IM1,9.8]);
A=double(A);
B=subs(jacobian([d_theta,f1,d_x,f2,d_phi,f3],[T,Tp]),[theta0,d_theta,d_x,phi0,d_phi,T,Tp],[0,0,0,0,0,0,0]);
B=subs(B,[R,L,LM,l,mw,mp,M,Iw,Ip,IM,g],[R1,L1,LM1,l1,mw1,mp1,M1,Iw1,Ip1,IM1,9.8]);
B=double(B);
Q=diag([100 1 500 100 5000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
R=[240 0;0 25]; %T Tp
K=lqr(A,B,Q,R);
end

BIN
utils/Simulation-master/balance/series_parallel_legs/physical_calc.mlx Normal file
View File

Binary file not shown.

BIN
utils/Simulation-master/balance/series_parallel_legs/save_phy.slx Normal file
View File

Binary file not shown.

BIN
utils/Simulation-master/dog_simple/dog.slx Normal file
View File

Binary file not shown.

BIN
utils/Simulation-master/dog_simple/dog.slx.r2023a Normal file
View File

Binary file not shown.

78
utils/k_calc/chassis_calc_heu.m Normal file
View File

@ -0,0 +1,78 @@
%LQR求解
clear;
syms theta dot_theta ddot_theta;
syms x dot_x ddot_x;
syms x_body dot_x_body ddot_x_body;
syms phi dot_phi ddot_phi;
syms T T_p;
syms R L L_M l m_w m_p M I_w I_p I_M g;
body_fusion = 1; %机体速度
g = 9.8; %重力加速度
R = 0.076; %轮半径
m_w = 0.47; %轮质量
m_p = 1.874; %摆杆质量
M = 6.975; %机体质量
I_w = 0.001974; %轮转动惯量
I_M = 0.143; %机体转动惯量
l = 0.0353; %机体质心离转轴距离
Q_cost=diag([160 160 200 120 2000 50]);
R_cost=diag([1, 0.25]);
if body_fusion
ddot_x = ddot_x_body - (L+L_M)*cos(theta)*ddot_theta + (L+L_M)*sin(theta)*dot_theta^2;
end
%对机体受力分析
N_M = M * (ddot_x + (L + L_M) * (-dot_theta^2*sin(theta) + ddot_theta*cos(theta)) - l*(-dot_phi^2*sin(phi) + ddot_phi*cos(phi)));
P_M = M*g + M*((L+L_M)*(-dot_theta^2*cos(theta) - ddot_theta*sin(theta)) + l*(-dot_phi^2*cos(phi) - ddot_phi*sin(phi)));
N = N_M + m_p*(ddot_x + L*(-dot_theta^2*sin(theta) + ddot_theta*cos(theta)));
P = P_M + m_p*g + m_p*L*(-dot_theta^2*cos(theta) - ddot_theta*sin(theta));
%方程求解
equ1 = ddot_x - (T - N*R)/(I_w/R + m_w*R);
equ2 = (P*L + P_M*L_M)*sin(theta) - (N*L+N_M*L_M)*cos(theta) - T + T_p - I_p*ddot_theta;
equ3 = T_p + N_M * l * cos(phi) + P_M * l * sin(phi) - I_M * ddot_phi;
if body_fusion
[ddot_theta, ddot_x_body, ddot_phi] = solve([equ1, equ2, equ3], [ddot_theta, ddot_x_body, ddot_phi]);
Ja = jacobian([dot_theta ddot_theta dot_x_body ddot_x_body dot_phi ddot_phi], [theta, dot_theta, x_body, dot_x_body, phi, dot_phi]);
Jb = jacobian([dot_theta ddot_theta dot_x_body ddot_x_body dot_phi ddot_phi], [T, T_p]);
A = simplify(vpa(subs(Ja, [theta, dot_theta, x_body, dot_x_body, phi, dot_phi], [0, 0, 0, 0, 0, 0])));
B = simplify(vpa(subs(Jb, [theta, dot_theta, x_body, dot_x_body, phi, dot_phi], [0, 0, 0, 0, 0, 0])));
else
[ddot_theta, ddot_x, ddot_phi] = solve([equ1, equ2, equ3], [ddot_theta, ddot_x, ddot_phi]);
Ja = jacobian([dot_theta ddot_theta dot_x ddot_x dot_phi ddot_phi], [theta, dot_theta, x, dot_x, phi, dot_phi]);
Jb = jacobian([dot_theta ddot_theta dot_x ddot_x dot_phi ddot_phi], [T, T_p]);
A = simplify(vpa(subs(Ja, [theta, dot_theta, x, dot_x, phi, dot_phi], [0, 0, 0, 0, 0, 0])));
B = simplify(vpa(subs(Jb, [theta, dot_theta, x, dot_x, phi, dot_phi], [0, 0, 0, 0, 0, 0])));
end
%% LQR计算
leg_var = 0.096;
K=zeros(30,12);
leglen=zeros(30,1);
for i=1:30
leg_var=leg_var+0.01; % 10mm线性化一次
llm= 0.8424 * leg_var - 0.0272;
ll = 0.1576 * leg_var + 0.0272;
leglen(i)=leg_var;
i_p = 0.0328 * leg_var + 0.0216;
trans_A=double(subs(A,[L L_M I_p],[ll llm i_p]));
trans_B=double(subs(B,[L L_M I_p],[ll llm i_p]));
KK=lqrd(trans_A,trans_B,Q_cost,R_cost,0.001);
KK_t=KK.';
K(i,:)=KK_t(:);
end
%% 系数拟合
K_cons=zeros(12,3); %排列顺序是
for i=1:12
res=fit(leglen,K(:,i),'poly2');
K_cons(i,:)=[res.p1,res.p2,res.p3];
end
disp(K_cons)

86
utils/matlab/LQR_calc.m Normal file
View File

@ -0,0 +1,86 @@
clear;
clc;
syms x xd xdd T Tp thetadd thetad theta phidd phid phi P N PM NM L LM;
%% 参数设定
% 均为标准单位制
g = 9.81;
% 驱动轮
R = 0.075; %轮子半径
mw = 0.58; %轮子质量
Iw = 0.00823; %轮子转动惯量
% 大腿
l_active_leg = 0.14;
m_active_leg = 0.174;
% 小腿
l_slave_leg = 0.24;
m_slave_leg = 0.180;
% 关节间距
joint_distance = 0.11;
% 摆杆
mp = (m_active_leg + m_slave_leg)*2 + 0.728; % 需要加上定子质量
Ip = mp*L^2/3; %摆杆转动惯量
% 机体
M = 12; %机体重量
l = -0.014; %机体质心到关节电机转轴的距离
IM = 0.124;
%% 经典力学方程
fu1=N-NM==mp*(xdd+L*(thetadd*cos(theta)-thetad*thetad*sin(theta)));
fu2=P-PM-mp*g==mp*L*(-thetadd*sin(theta)-thetad*thetad*cos(theta));
fu3=NM==M*(xdd+(L+LM)*(thetadd*cos(theta)-thetad*thetad*sin(theta))-l*(phidd*cos(phi)-phid*phid*sin(phi)));
fu4=PM-M*g==M*((L+LM)*(-thetadd*sin(theta)-thetad*thetad*cos(theta))+l*(-phidd*sin(phi)-phid*phid*cos(phi)));
%% 不同部件之间的力求解
[N,NM,P,PM]=solve(fu1,fu2,fu3,fu4,N,NM,P,PM);
f1=xdd==(T-N*R)/(Iw/R+mw*R);
f2=Ip*thetadd==(P*L+PM*LM)*sin(theta)-(N*L+NM*LM)*cos(theta)-T+Tp;
f3=IM*phidd==Tp+NM*l*cos(phi)+PM*l*sin(phi);
[xdd,thetadd,phidd]=solve(f1,f2,f3,xdd,thetadd,phidd);
%% 计算雅可比矩阵A and B
func=[thetad,thetadd,xd,xdd,phid,phidd];
A_lin_model=jacobian(func,[theta,thetad,x,xd,phi,phid]);
temp_A=subs(A_lin_model,[theta,thetad,xd,phi,phid],zeros(1,5));
final_A=simplify(temp_A);
B_lin_model=jacobian(func,[T Tp]);
temp_B=subs(B_lin_model,[theta,thetad,xd,phi,phid],zeros(1,5));
final_B=simplify(temp_B);
%% 计算不同腿长下LQR增益K
L_var = 0.1; % 腿质心到机体转轴距离
Q_mat = diag([1,1,500,100,5000,1]);
R_mat = diag([1,0.25]);
K = zeros(20,12);
leg_len = zeros(20,1);
for i=1:20
L_var = L_var + 0.005;
leg_len(i) = L_var*2;
A = double(subs(final_A, [L LM], [L_var L_var]));
B = double(subs(final_B, [L LM], [L_var L_var]));
KK = lqrd(A, B, Q_mat, R_mat, 0.001);
KK_t=KK.';
K(i,:)=KK_t(:);
end
%% 不同腿长下二项式拟合K
K_cons=zeros(12,3);
for i=1:12
res=fit(leg_len,K(:,i),"poly2");
K_cons(i,:)=[res.p1, res.p2, res.p3];
end
for j=1:12
for i=1:3
fprintf("%f,",K_cons(j,i));
end
fprintf("\n");
end

103
utils/matlab/UseBodyVel.m Normal file
View File

@ -0,0 +1,103 @@
clear;
clc;
syms theta phi L x x_b N N_f T T_p M N_M P_M L_M
syms theta_dot x_dot phi_dot theta_ddot x_ddot phi_ddot
syms x_b_dot x_b_ddot
%% 参数设定
% 均为标准单位制
g = 9.81; %重力加速度
% 驱动轮
mw = 0.58; %轮子质量
R = 0.075; %轮子半径
Iw = 0.00823; %轮子转动惯量
% 大腿
l_active_leg = 0.14;
m_active_leg = 0.174;
% 小腿
l_slave_leg = 0.24;
m_slave_leg = 0.180;
% 关节间距
joint_distance = 0.11;
% 摆杆
mp = (m_active_leg + m_slave_leg)*2 + 0.728;
Ip = mp*L^2/3; % 摆杆转动惯量
% 机体
M = 12; %机体重量
IM = 0.124; %机体惯量,绕质心
l = -0.014; %机体质心到电机转轴的距离
% QR设置为相同的权重
Q_cost = diag([1,1,500,100,5000,1]);
R_cost = diag([1,0.25]);
useBodyVelocity = 1;
%% 经典力学方程
if useBodyVelocity
x_ddot = x_b_ddot - (L+L_M)*cos(theta)*theta_ddot+ (L+L_M)*sin(theta)*theta_dot^2;
end
N_M = M*(x_ddot+(L+L_M)*theta_ddot*cos(theta)-(L+L_M)*theta_dot^2*sin(theta)-l*phi_ddot*cos(phi)+l*phi_dot^2*sin(phi));
P_M = M*(g-(L+L_M)*theta_ddot*sin(theta)-(L+L_M)*theta_dot^2*cos(theta)-l*phi_ddot*sin(phi)-l*phi_dot^2*cos(phi));
N = mp*(x_ddot+L*theta_ddot*cos(theta)-L*theta_dot^2*sin(theta))+N_M;
P = mp*(g-L*theta_dot^2*cos(theta)-L*theta_ddot*sin(theta))+P_M;
eqA = x_ddot == (T-N*R)/(Iw/R+mw*R);
eqB = Ip*theta_ddot == (P*L+P_M*L_M)*sin(theta)-(N*L+N_M*L_M)*cos(theta) - T + T_p;
eqC = IM*phi_ddot == T_p + N_M*l*cos(phi) + P_M*l*sin(phi);
%% 计算雅可比矩阵
U = [T T_p].';
if useBodyVelocity
model_sol = solve([eqA eqB eqC],[theta_ddot,x_b_ddot,phi_ddot]);
X = [theta,theta_dot,x_b,x_b_dot,phi,phi_dot].';
dX = [theta_dot,simplify(model_sol.theta_ddot),...
x_b_dot,simplify(model_sol.x_b_ddot),...
phi_dot,simplify(model_sol.phi_ddot)].';
A_sym = subs(jacobian(dX,X),[theta theta_dot x_b_dot phi phi_dot],zeros(1,5));
B_sym = subs(jacobian(dX,U),[theta theta_dot x_b_dot phi phi_dot],zeros(1,5));
else
model_sol = solve([eqA eqB eqC],[theta_ddot,x_ddot,phi_ddot]);
X = [theta,theta_dot,x,x_dot,phi,phi_dot].';
dX = [theta_dot,simplify(model_sol.theta_ddot),...
x_dot,simplify(model_sol.x_ddot),...
phi_dot,simplify(model_sol.phi_ddot)].';
A_sym = subs(jacobian(dX,X),[theta theta_dot x_dot phi phi_dot],zeros(1,5));
B_sym = subs(jacobian(dX,U),[theta theta_dot x_dot phi phi_dot],zeros(1,5));
end
%% 计算变长度LQR
L_var = 0.1; % 腿质心到机体转轴距离
K=zeros(20,12);
leglen=zeros(20,1);
for i=1:20
L_var=L_var+0.005; % 10mm线性化一次
leglen(i)=L_var*2;
trans_A=double(subs(A_sym,[L L_M],[L_var L_var]));
trans_B=double(subs(B_sym,[L L_M],[L_var L_var]));
KK=lqrd(trans_A,trans_B,Q_cost,R_cost,0.001);
KK_t=KK.';
K(i,:)=KK_t(:);
end
%% 用二项式拟合K,一共12个参数
K_cons=zeros(12,3);
for i=1:12
res=fit(leglen,K(:,i),"poly2");
K_cons(i,:)=[res.p1,res.p2,res.p3];
end
for j=1:12
for i=1:3
fprintf("%f,",K_cons(j,i));
end
fprintf("\n");
end

39
utils/matlab/vmc.m Normal file
View File

@ -0,0 +1,39 @@
clear;
clc;
syms phi1(t) phi2(t) phi3(t) phi4(t) l5 phi0 L0 T_Leg F_Leg
syms phi_dot_1 phi_dot_4
syms l1 l2 l3 l4
%% 腿长解算
x_B = l1*cos(phi1);
y_B = l1*sin(phi1);
x_C = x_B+l2*cos(phi2);
y_C = y_B+l2*sin(phi2);
x_D = l5+l4*cos(phi4);
y_D = l4*sin(phi4);
x_dot_B = diff(x_B,t);
y_dot_B = diff(y_B,t);
x_dot_C = diff(x_C,t);
y_dot_C = diff(y_C,t);
x_dot_D = diff(x_D,t);
y_dot_D = diff(y_D,t);
%% 速度导数求解
phi_dot_2 = ((x_dot_D-x_dot_B)*cos(phi3)+(y_dot_D-y_dot_B)*sin(phi3))/l2/sin(phi3-phi2);
x_dot_C = subs(x_dot_C,diff(phi2,t),phi_dot_2);
x_dot_C = subs(x_dot_C,[diff(phi1,t),diff(phi4,t)],[phi_dot_1,phi_dot_4]);
y_dot_C = subs(y_dot_C,diff(phi2,t),phi_dot_2);
y_dot_C = subs(y_dot_C,[diff(phi1,t),diff(phi4,t)],[phi_dot_1,phi_dot_4]);
%% 运动映射(虚功原理)+极坐标转换
x_dot = [x_dot_C; y_dot_C];
q_dot = [phi_dot_1; phi_dot_4];
x_dot = collect(simplify(collect(x_dot,q_dot)),q_dot);
J = simplify(jacobian(x_dot,q_dot));
rotate = [cos(phi0-pi/2) -sin(phi0-pi/2);
sin(phi0-pi/2) cos(phi0-pi/2)];
map = [0 -1/L0;
1 0];
Trans_Jacobian = simplify(J.'*rotate*map);

BIN
utils/matlab/wheel_leg.slx Normal file
View File

Binary file not shown.

BIN
utils/matlab/wheel_leg.slxc Normal file
View File

Binary file not shown.

731
utils/mod_wheelleg_chassis.cpp Normal file
View File

@ -0,0 +1,731 @@
#include "mod_wheelleg_chassis.hpp"
#include <cmath>
#include <tuple>
using namespace Module;
Wheelleg::Wheelleg(Param &param)
: param_(param),
roll_pid_(param.roll_pid_param, 333.0f),
yaw_pid_(param.yaw_pid_param, 333.0f),
yaccl_pid_(param.yaccl_pid_param, 333.0f),
lowfilter_(333.0f, 50.0f),
acclfilter_(333.0f, 30.0f),
manfilter_(param.manfilter_param),
ctrl_lock_(true)
{
memset(&(this->cmd_), 0, sizeof(this->cmd_));
this->hip_motor_.at(0) =
new Device::MitMotor(param.hip_motor_param.at(0), "hip_left_front");
this->hip_motor_.at(1) =
new Device::MitMotor(param.hip_motor_param.at(1), "hip_left_back");
this->hip_motor_.at(2) =
new Device::MitMotor(param.hip_motor_param.at(2), "hip_right_front");
this->hip_motor_.at(3) =
new Device::MitMotor(param.hip_motor_param.at(3), "hip_left_back");
this->wheel_motor_.at(0) =
new Device::RMMotor(param.wheel_motor_param.at(0), "wheel_left");
this->wheel_motor_.at(1) =
new Device::RMMotor(param.wheel_motor_param.at(1), "wheel_right");
this->leglength_pid_.at(0) =
new Component::PID(param.leglength_pid_param.at(0), 333.0f);
this->leglength_pid_.at(1) =
new Component::PID(param.leglength_pid_param.at(1), 333.0f);
this->theta_pid_.at(0) =
new Component::PID(param.theta_pid_param.at(0), 333.0f);
this->theta_pid_.at(1) =
new Component::PID(param.theta_pid_param.at(1), 333.0f);
this->tp_pid_.at(0) = new Component::PID(param.Tp_pid_param.at(0), 333.0);
this->tp_pid_.at(1) = new Component::PID(param.Tp_pid_param.at(1), 333.0);
this->leg_.at(0) = new Component::VMC(param_.leg_param.at(0), 333.0f);
this->leg_.at(1) = new Component::VMC(param_.leg_param.at(1), 333.0f);
auto event_callback = [](Wheelleg::ChassisEvent event, Wheelleg *chassis)
{
chassis->ctrl_lock_.Wait(UINT32_MAX);
switch (event)
{
case SET_MODE_RELAX:
chassis->SetMode(RELAX);
break;
case SET_MODE_STAND:
chassis->SetMode(STAND);
break;
case SET_MODE_ROTOR:
chassis->SetMode(ROTOR);
break;
case SET_MODE_RESET:
chassis->SetMode(RESET);
break;
default:
break;
}
chassis->ctrl_lock_.Post();
};
Component::CMD::RegisterEvent<Wheelleg *, ChassisEvent>(
event_callback, this, this->param_.EVENT_MAP);
auto chassis_thread = [](Wheelleg *chassis)
{
auto cmd_sub =
Message::Subscriber<Component::CMD::ChassisCMD>("cmd_chassis");
auto eulr_sub =
Message::Subscriber<Component::Type::Eulr>("chassis_imu_eulr");
auto gyro_sub =
Message::Subscriber<Component::Type::Vector3>("chassis_gyro");
auto yaw_sub = Message::Subscriber<float>("chassis_yaw");
auto accl_sub =
Message::Subscriber<Component::Type::Vector3>("chassis_imu_accl_abs");
// auto yaw_sub = Message::Subscriber<float>("chassis_yaw");
uint32_t last_online_time = bsp_time_get_ms();
while (1)
{
eulr_sub.DumpData(chassis->eulr_); /* imu */
gyro_sub.DumpData(chassis->gyro_); /* imu */
accl_sub.DumpData(chassis->accl_);
yaw_sub.DumpData(chassis->yaw_);
cmd_sub.DumpData(chassis->cmd_);
// yaw_sub.DumpData(chassis->yaw_);
chassis->ctrl_lock_.Wait(UINT32_MAX);
chassis->MotorSetAble();
chassis->UpdateFeedback();
chassis->Calculate();
chassis->Control();
chassis->ctrl_lock_.Post();
/* 运行结束，等待下一次唤醒 */
chassis->thread_.SleepUntil(3, last_online_time);
}
};
this->thread_.Create(chassis_thread, this, "chassis_thread", 1536,
System::Thread::MEDIUM);
}
void Wheelleg::MotorSetAble()
{
if (this->hip_motor_flag_ == 0)
{
this->hip_motor_[0]->Relax();
this->hip_motor_[1]->Relax();
this->hip_motor_[2]->Relax();
this->hip_motor_[3]->Relax();
this->dm_motor_flag_ = 0;
}
else
{
if (this->dm_motor_flag_ == 0)
{
for (int i = 0; i < 5; i++)
{
this->hip_motor_[0]->Enable();
}
for (int i = 0; i < 5; i++)
{
this->hip_motor_[1]->Enable();
}
for (int i = 0; i < 5; i++)
{
this->hip_motor_[2]->Enable();
}
for (int i = 0; i < 5; i++)
{
this->hip_motor_[3]->Enable();
}
this->dm_motor_flag_ = 1;
}
};
}
void Wheelleg::UpdateFeedback()
{
this->now_ = bsp_time_get();
this->dt_ = TIME_DIFF(this->last_wakeup_, this->now_);
this->last_wakeup_ = this->now_;
this->wheel_motor_[0]->Update();
this->wheel_motor_[1]->Update();
this->leg_argu_[0].phi4_ =
this->hip_motor_[0]->GetAngle() -
this->param_.wheel_param.mech_zero[0]; // 前关节角度
this->leg_argu_[0].phi1_ =
this->hip_motor_[1]->GetAngle() -
this->param_.wheel_param.mech_zero[1]; // 后关节角度
this->leg_argu_[1].phi4_ =
(-this->hip_motor_[2]->GetAngle() +
this->param_.wheel_param.mech_zero[3]); // 前关节角度
if (leg_argu_[1].phi4_ > M_PI)
{
this->leg_argu_[1].phi4_ = this->leg_argu_[1].phi4_ - 2 * M_PI;
}
this->leg_argu_[1].phi1_ =
(-this->hip_motor_[3]->GetAngle() +
this->param_.wheel_param.mech_zero[2]); // 后关节角度
if (leg_argu_[1].phi1_ > M_PI)
{
this->leg_argu_[1].phi1_ = this->leg_argu_[1].phi1_ - 2 * M_PI;
}
this->pit_ = this->eulr_.pit;
if (this->pit_ > M_PI)
{
this->pit_ = this->eulr_.pit - 2 * M_PI;
}
/* 注意极性 */
std::tuple<float, float, float, float> result0 =
this->leg_[0]->VMCsolve(-leg_argu_[0].phi1_, -leg_argu_[0].phi4_,
this->pit_, -this->gyro_.x, this->dt_);
this->leg_argu_[0].L0 = std::get<0>(result0);
this->leg_argu_[0].d_L0 = std::get<1>(result0);
this->leg_argu_[0].theta = -std::get<2>(result0);
this->leg_argu_[0].d_theta = -std::get<3>(result0);
if (leg_argu_[0].theta > M_PI)
{
leg_argu_[0].theta = leg_argu_[0].theta - 2 * M_PI;
}
if (leg_argu_[0].theta < -M_PI)
{
leg_argu_[0].theta = leg_argu_[0].theta + 2 * M_PI;
}
std::tuple<float, float, float, float> result1 =
this->leg_[1]->VMCsolve(-leg_argu_[1].phi1_, -leg_argu_[1].phi4_,
this->pit_, -this->gyro_.x, this->dt_);
this->leg_argu_[1].L0 = std::get<0>(result1);
this->leg_argu_[1].d_L0 = std::get<1>(result1);
this->leg_argu_[1].theta = -std::get<2>(result1);
this->leg_argu_[1].d_theta = -std::get<3>(result1);
if (leg_argu_[1].theta > M_PI)
{
leg_argu_[1].theta = leg_argu_[1].theta - 2 * M_PI;
}
if (leg_argu_[1].theta < -M_PI)
{
leg_argu_[1].theta = leg_argu_[1].theta + 2 * M_PI;
}
/* 轮子转速近似于编码器速度 由此推测机体速度 */
this->leg_argu_[0].single_x_dot =
-wheel_motor_[0]->GetSpeed() * 2 * M_PI *
(this->param_.wheel_param.wheel_radius) / 60.f / 15.765 +
leg_argu_[0].L0 * leg_argu_[0].d_theta * cosf(leg_argu_[0].theta) +
leg_argu_[0].d_L0 * sinf(leg_argu_[0].theta);
this->leg_argu_[1].single_x_dot =
wheel_motor_[1]->GetSpeed() * 2 * M_PI *
(this->param_.wheel_param.wheel_radius) / 60.f / 15.765 +
leg_argu_[1].L0 * leg_argu_[1].d_theta * cosf(leg_argu_[1].theta) +
leg_argu_[1].d_L0 * sinf(leg_argu_[1].theta);
this->move_argu_.last_x_dot = this->move_argu_.x_dot;
this->move_argu_.x_dot =
(this->leg_argu_[0].single_x_dot + this->leg_argu_[1].single_x_dot) / 2;
this->move_argu_.x_dot =
(-wheel_motor_[0]->GetSpeed() + wheel_motor_[1]->GetSpeed()) * M_PI *
this->param_.wheel_param.wheel_radius / 60.f / 15.765;
this->move_argu_.x = this->move_argu_.x_dot * dt_ + move_argu_.x;
this->move_argu_.delta_speed =
lowfilter_.Apply((move_argu_.x_dot - move_argu_.last_x_dot) / dt_);
this->move_argu_.accl = acclfilter_.Apply(this->accl_.y * 9.8f);
if (now_ > 1000000)
{
this->move_argu_.x_dot_hat =
manfilter_.comp_filter(move_argu_.x_dot, move_argu_.delta_speed,
this->move_argu_.last_x_dot, accl_.y * 9.8f,
dt_) -
((leg_argu_[0].L0 * leg_argu_[0].d_theta * cosf(leg_argu_[0].theta) +
leg_argu_[0].d_L0 * sinf(leg_argu_[0].theta)) +
(leg_argu_[1].L0 * leg_argu_[1].d_theta * cosf(leg_argu_[1].theta) +
leg_argu_[1].d_L0 * sinf(leg_argu_[1].theta))) /
2;
this->move_argu_.xhat = dt_ * move_argu_.x_dot_hat + move_argu_.xhat;
}
}
void Wheelleg::Calculate()
{
switch (this->mode_)
{
case Wheelleg::RELAX:
// if (fabs(move_argu_.target_dot_x - cmd_.y * 1.5f) > 0.005) {
// if (move_argu_.target_dot_x > cmd_.y * 1.5f) {
// move_argu_.target_dot_x -= 0.004;
// }
// if (move_argu_.target_dot_x < cmd_.y * 1.5f) {
// move_argu_.target_dot_x += 0.004;
// }
// } else {
// move_argu_.target_dot_x = cmd_.y * 1.5f;
// }
// move_argu_.target_x = move_argu_.target_dot_x * dt_ +
// move_argu_.target_x;
move_argu_.target_x = 0;
move_argu_.target_dot_x = 0;
break;
case Wheelleg::STAND:
/* 0.002为最大加速度 即500hz*0.002 梯度式递增减 */
if (fabs(move_argu_.target_dot_x -
cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) > 0.005)
{
if (move_argu_.target_dot_x >
cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed)
{
move_argu_.target_dot_x -= 0.004;
}
if (move_argu_.target_dot_x <
cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed)
{
move_argu_.target_dot_x += 0.004;
}
}
else
{
move_argu_.target_dot_x =
cosf(yaw_) * cmd_.y * param_.wheel_param.max_speed;
}
move_argu_.target_x = move_argu_.target_dot_x * dt_ + move_argu_.target_x;
this->move_argu_.target_yaw = 0.0f;
/*双零点*/
if (this->yaw_ > M_PI_2)
{
this->move_argu_.target_yaw = 3.14158f;
}
if (this->yaw_ < -M_PI_2)
{
this->move_argu_.target_yaw = 3.14158f;
}
break;
case Wheelleg::ROTOR:
if (fabs(move_argu_.target_dot_x -
cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) > 0.005)
{
if (move_argu_.target_dot_x >
cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed)
{
move_argu_.target_dot_x -= 0.004;
}
if (move_argu_.target_dot_x <
cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed)
{
move_argu_.target_dot_x += 0.004;
}
}
else
{
move_argu_.target_dot_x =
cosf(yaw_) * cmd_.y * param_.wheel_param.max_speed;
}
move_argu_.target_x = move_argu_.target_dot_x * dt_ + move_argu_.target_x;
this->move_argu_.target_yaw = this->yaw_ + 0.01;
break;
// move_argu_.target_dot_x = cmd_.x;
// move_argu_.target_x =
// move_argu_.target_dot_x * dt_ + move_argu_.target_dot_x;
// // move_argu_.target_dot_x = sqrtf(cmd_.x * cmd_.x + cmd_.y *
// cmd_.y);
// // move_argu_.x_dot =
// // move_argu_.target_dot_x * dt_ + move_argu_.target_dot_x;
// // move_argu_.target_yaw = atan2(cmd_.x, cmd_.y);
// break;
case Wheelleg::RESET:
this->move_argu_.target_dot_x = 0;
move_argu_.target_x = 0;
this->move_argu_.target_yaw = this->eulr_.yaw;
this->move_argu_.xhat = 0;
// move_argu_.target_yaw - atan2(cmd_.x, cmd_.y);
// if ((fabs(move_argu_.target_yaw) - fabs(eulr_.yaw)) > M_PI / 2) {
// this->move_argu_.target_dot_x = -this->move_argu_.target_dot_x;
// }
break;
default:
XB_ASSERT(false);
return;
}
leg_argu_[0].Fn = leg_[0]->GndDetector(leg_argu_[0].Tp, leg_argu_[0].F0);
onground0_flag_ = false;
if (leg_argu_[0].Fn > 30)
{
onground0_flag_ = true;
}
leg_argu_[1].Fn = leg_[1]->GndDetector(leg_argu_[1].Tp, leg_argu_[1].F0);
onground1_flag_ = false;
if (leg_argu_[1].Fn > 30)
{
onground1_flag_ = true;
}
std::tuple<float, float> result3;
std::tuple<float, float> result4;
switch (this->mode_)
{
case Wheelleg::RELAX:
case Wheelleg::ROTOR:
case Wheelleg::STAND:
for (int i = 0; i < 12; i++)
{
leg_argu_[0].LQR_K[i] = this->leg_[0]->LQR_K_calc(
&this->param_.wheel_param.K_Poly_Coefficient_L[i][0],
leg_argu_[0].L0);
}
for (int i = 0; i < 12; i++)
{
leg_argu_[1].LQR_K[i] = this->leg_[1]->LQR_K_calc(
&this->param_.wheel_param.K_Poly_Coefficient_R[i][0],
leg_argu_[1].L0);
}
if (now_ > 1000000)
if (fabs(move_argu_.target_dot_x) > 0.5 ||
fabs(move_argu_.target_dot_x - move_argu_.x_dot_hat) > 0.7 ||
((!onground0_flag_) & (!onground1_flag_)))
{
leg_argu_[0].LQR_K[2] = 0;
leg_argu_[1].LQR_K[2] = 0;
this->move_argu_.xhat = 0;
move_argu_.target_x = 0;
}
if (onground0_flag_)
{
leg_argu_[0].Tw =
(leg_argu_[0].LQR_K[0] *
(-0.8845 * leg_argu_[0].L0 + 0.40663 - leg_argu_[0].theta) +
leg_argu_[0].LQR_K[1] * (-leg_argu_[0].d_theta) +
leg_argu_[0].LQR_K[2] *
(-move_argu_.xhat + move_argu_.target_x - 0.56) +
leg_argu_[0].LQR_K[3] *
(-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
leg_argu_[0].LQR_K[4] * (-this->pit_ + 0.16) +
leg_argu_[0].LQR_K[5] * (-this->gyro_.x + 0.0f));
leg_argu_[0].Tp =
(leg_argu_[0].LQR_K[6] *
(-0.8845 * leg_argu_[0].L0 + 0.40663 - leg_argu_[0].theta) +
leg_argu_[0].LQR_K[7] * (-leg_argu_[0].d_theta) +
leg_argu_[0].LQR_K[8] *
(-move_argu_.xhat + move_argu_.target_x - 0.56) +
leg_argu_[0].LQR_K[9] *
(-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
leg_argu_[0].LQR_K[10] * (-this->pit_ + 0.16) +
leg_argu_[0].LQR_K[11] * (-this->gyro_.x + 0.0f));
}
else
{
leg_argu_[0].Tw = 0;
leg_argu_[0].Tp =
(leg_argu_[0].LQR_K[6] * (-leg_argu_[0].theta + 0.0f) +
leg_argu_[0].LQR_K[7] * (-leg_argu_[0].d_theta + 0.0f));
}
if (onground1_flag_)
{
leg_argu_[1].Tw =
(leg_argu_[1].LQR_K[0] *
(-0.8845 * leg_argu_[1].L0 + 0.40663 - leg_argu_[1].theta) +
leg_argu_[1].LQR_K[1] * (-leg_argu_[1].d_theta + 0.0f) +
leg_argu_[1].LQR_K[2] *
(-move_argu_.xhat + move_argu_.target_x - 0.56) +
leg_argu_[1].LQR_K[3] *
(-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
leg_argu_[1].LQR_K[4] * (-this->pit_ + 0.16) +
leg_argu_[1].LQR_K[5] * (-this->gyro_.x + 0.0f));
leg_argu_[1].Tp =
(leg_argu_[1].LQR_K[6] *
(-0.8845 * leg_argu_[1].L0 + 0.40663 - leg_argu_[1].theta) +
leg_argu_[1].LQR_K[7] * (-leg_argu_[1].d_theta + 0.0f) +
leg_argu_[1].LQR_K[8] *
(-move_argu_.xhat + move_argu_.target_x - 0.56) +
leg_argu_[1].LQR_K[9] *
(-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
leg_argu_[1].LQR_K[10] * (-this->pit_ + 0.16) +
leg_argu_[1].LQR_K[11] * (-this->gyro_.x + 0.0f));
}
else
{
leg_argu_[1].Tw = 0;
leg_argu_[1].Tp =
(leg_argu_[1].LQR_K[6] * (-leg_argu_[1].theta + 0.0f) +
leg_argu_[1].LQR_K[7] * (-leg_argu_[1].d_theta + 0.0f));
}
this->leg_argu_[0].Delat_L0 = fabs(0.35 * cmd_.z) - fabs(gyro_.z) / 100 +
this->param_.wheel_param.static_L0[0] +
+roll_pid_.Calculate(0, eulr_.rol, dt_);
this->leg_argu_[1].Delat_L0 = fabs(0.35 * cmd_.z) - fabs(gyro_.z) / 100 +
this->param_.wheel_param.static_L0[1] -
roll_pid_.Calculate(0, eulr_.rol, dt_);
leg_argu_[0].F0 =
leg_argu_[0].Tp * sinf(leg_argu_[0].theta) +
this->param_.wheel_param.static_F0[0] +
leglength_pid_.at(0)->Calculate(this->leg_argu_[0].Delat_L0,
this->leg_argu_[0].L0, this->dt_);
leg_argu_[1].F0 =
leg_argu_[1].Tp * sinf(leg_argu_[1].theta) +
this->param_.wheel_param.static_F0[1] +
leglength_pid_.at(1)->Calculate(this->leg_argu_[1].Delat_L0,
this->leg_argu_[1].L0, this->dt_);
this->leg_argu_[0].Delta_Tp =
leg_argu_[0].L0 * 10.0f *
tp_pid_.at(0)->Calculate(this->leg_argu_[1].theta,
this->leg_argu_[0].theta, this->dt_);
this->leg_argu_[1].Delta_Tp =
-leg_argu_[1].L0 * 10.0f *
tp_pid_.at(0)->Calculate(this->leg_argu_[1].theta,
this->leg_argu_[0].theta, this->dt_);
result3 = leg_[0]->VMCinserve(
-leg_argu_[0].phi1_, -leg_argu_[0].phi4_,
-leg_argu_[0].Tp - this->leg_argu_[0].Delta_Tp, leg_argu_[0].F0);
this->leg_argu_[0].T1 = std::get<0>(result3);
this->leg_argu_[0].T2 = std::get<1>(result3);
result4 = leg_[1]->VMCinserve(
-leg_argu_[1].phi1_, -leg_argu_[1].phi4_,
-leg_argu_[1].Tp - this->leg_argu_[1].Delta_Tp, leg_argu_[1].F0);
this->leg_argu_[1].T1 = -std::get<0>(result4);
this->leg_argu_[1].T2 = -std::get<1>(result4);
if (onground0_flag_ & onground1_flag_)
{
move_argu_.yaw_force =
-this->yaw_pid_.Calculate(move_argu_.target_yaw, this->yaw_, dt_);
}
else
{
move_argu_.yaw_force = 0;
}
/*3508不带减速箱是Tw*3.2f*/
/*2006带减速是Tw/1.8f*/
/* 3508带15.7减速箱是Tw/4.9256 */
this->wheel_motor_out_[0] =
this->leg_argu_[0].Tw / 4.9256f - move_argu_.yaw_force;
this->wheel_motor_out_[1] =
this->leg_argu_[1].Tw / 4.9256f + move_argu_.yaw_force;
this->hip_motor_out_[0] = this->leg_argu_[0].T1;
this->hip_motor_out_[1] = this->leg_argu_[0].T2;
this->hip_motor_out_[2] = this->leg_argu_[1].T1;
this->hip_motor_out_[3] = this->leg_argu_[1].T2;
this->hip_motor_flag_ = 1;
break;
case Wheelleg::RESET:
if (fabs(pit_) > M_PI / 2 || fabs(leg_argu_[0].theta) > 1.57 ||
fabs(leg_argu_[1].theta) > 1.57)
{
leg_argu_[0].target_theta = leg_argu_[0].theta + cmd_.y / 1000;
if (fabs(pit_) > M_PI / 2)
{
if ((leg_argu_[0].theta - leg_argu_[1].theta) > 0.03)
{
leg_argu_[1].target_theta = leg_argu_[1].theta + 0.001;
}
if ((leg_argu_[0].theta - leg_argu_[1].theta) < -0.03)
{
leg_argu_[1].target_theta = leg_argu_[1].theta - 0.001;
}
leg_argu_[0].F0 = 10 * leglength_pid_.at(0)->Calculate(
0.65f, this->leg_argu_[0].L0, this->dt_);
leg_argu_[1].F0 = 10 * leglength_pid_.at(1)->Calculate(
0.65f, this->leg_argu_[1].L0, this->dt_);
}
if (fabs(leg_argu_[0].theta) < 1.57)
{
leg_argu_[1].target_theta = leg_argu_[1].theta + cmd_.y / 1000;
leg_argu_[0].target_theta = leg_argu_[0].theta;
}
if (fabs(leg_argu_[1].theta) < 1.57)
{
leg_argu_[0].target_theta = leg_argu_[0].theta + cmd_.y / 1000;
leg_argu_[1].target_theta = leg_argu_[1].theta;
}
if (leg_argu_[0].target_theta > M_PI)
{
leg_argu_[0].target_theta -= 2 * M_PI;
}
if (leg_argu_[0].target_theta < -M_PI)
{
leg_argu_[0].target_theta += 2 * M_PI;
}
if (leg_argu_[1].target_theta > M_PI)
{
leg_argu_[1].target_theta -= 2 * M_PI;
}
if (leg_argu_[1].target_theta < -M_PI)
{
leg_argu_[1].target_theta += 2 * M_PI;
}
leg_argu_[0].Tp =
500 * this->theta_pid_[0]->Calculate(leg_argu_[0].target_theta,
leg_argu_[0].theta, dt_);
leg_argu_[1].Tp =
500 * this->theta_pid_[1]->Calculate(leg_argu_[1].target_theta,
leg_argu_[1].theta, dt_);
}
else
{
leg_argu_[0].F0 = 3 * leglength_pid_.at(0)->Calculate(
0.10f, this->leg_argu_[0].L0, this->dt_);
leg_argu_[1].F0 = 3 * leglength_pid_.at(1)->Calculate(
0.10f, this->leg_argu_[1].L0, this->dt_);
if ((this->leg_argu_[0].L0 < 0.20) && (this->leg_argu_[1].L0 < 0.20))
{
leg_argu_[0].Tp = 5.5 * this->theta_pid_[0]->Calculate(
0.1, leg_argu_[0].theta, dt_);
leg_argu_[1].Tp = 5.5 * this->theta_pid_[1]->Calculate(
0.1, leg_argu_[1].theta, dt_);
clampf(&leg_argu_[0].Tp, -10, 10);
clampf(&leg_argu_[1].Tp, -10, 10);
}
else
{
leg_argu_[0].Tp = 0;
leg_argu_[1].Tp = 0;
}
}
result3 = leg_[0]->VMCinserve(-leg_argu_[0].phi1_, -leg_argu_[0].phi4_,
-leg_argu_[0].Tp, leg_argu_[0].F0);
this->leg_argu_[0].T1 = std::get<0>(result3);
this->leg_argu_[0].T2 = std::get<1>(result3);
result4 = leg_[1]->VMCinserve(-leg_argu_[1].phi1_, -leg_argu_[1].phi4_,
-leg_argu_[1].Tp, leg_argu_[1].F0);
this->leg_argu_[1].T1 = -std::get<0>(result4);
this->leg_argu_[1].T2 = -std::get<1>(result4);
this->hip_motor_out_[0] = this->leg_argu_[0].T1;
this->hip_motor_out_[1] = this->leg_argu_[0].T2;
this->hip_motor_out_[2] = this->leg_argu_[1].T1;
this->hip_motor_out_[3] = this->leg_argu_[1].T2;
this->hip_motor_flag_ = 1;
break;
}
}
void Wheelleg::Control()
{
clampf(&wheel_motor_out_[0], -0.8f, 0.8f);
clampf(&wheel_motor_out_[1], -0.8f, 0.8f);
clampf(&hip_motor_out_[0], -20.0f, 20.0f);
clampf(&hip_motor_out_[1], -20.0f, 20.0f);
clampf(&hip_motor_out_[2], -20.0f, 20.0f);
clampf(&hip_motor_out_[3], -20.0f, 20.0f);
// if (fabs(wheel_motor_[0]->GetSpeed()) > 5000 ||
// fabs(wheel_motor_[1]->GetSpeed()) > 5000) {
// wheel_motor_out_[0] = 0;
// wheel_motor_out_[1] = 0;
// if (fabs(this->pit_) > 0.5f) {
// this->hip_motor_flag_ = 0;
// }
// }
switch (this->mode_)
{
case Wheelleg::RELAX:
this->wheel_motor_[0]->Relax();
this->wheel_motor_[1]->Relax();
hip_motor_flag_ = 0;
hip_motor_[0]->SetMit(0.0f);
hip_motor_[1]->SetMit(0.0f);
hip_motor_[2]->SetMit(0.0f);
hip_motor_[3]->SetMit(0.0f);
break;
case Wheelleg::STAND:
case Wheelleg::ROTOR:
// this->wheel_motor_[0]->Relax();
// this->wheel_motor_[1]->Relax();
this->wheel_motor_[0]->Control(-wheel_motor_out_[0]);
this->wheel_motor_[1]->Control(wheel_motor_out_[1]);
hip_motor_[0]->SetMit(this->hip_motor_out_[0]);
hip_motor_[1]->SetMit(this->hip_motor_out_[1]);
hip_motor_[2]->SetMit(this->hip_motor_out_[2]);
hip_motor_[3]->SetMit(this->hip_motor_out_[3]);
// hip_motor_[0]->SetMit(0.0f);
// hip_motor_[1]->SetMit(0.0f);
// hip_motor_[2]->SetMit(0.0f);
// hip_motor_[3]->SetMit(0.0f);
break;
case Wheelleg::RESET:
this->wheel_motor_[0]->Relax();
this->wheel_motor_[1]->Relax();
hip_motor_[0]->SetMit(this->hip_motor_out_[0]);
hip_motor_[1]->SetMit(this->hip_motor_out_[1]);
hip_motor_[2]->SetMit(this->hip_motor_out_[2]);
hip_motor_[3]->SetMit(this->hip_motor_out_[3]);
break;
}
}
void Wheelleg::SetMode(Wheelleg::Mode mode)
{
if (mode == this->mode_)
{
return;
}
this->leg_[0]->Reset();
this->leg_[1]->Reset();
move_argu_.x = 0;
move_argu_.x_dot = 0;
move_argu_.last_x_dot = 0;
move_argu_.target_x = move_argu_.xhat;
move_argu_.target_yaw = this->eulr_.yaw;
move_argu_.target_dot_x = 0;
move_argu_.xhat = 0;
move_argu_.x_dot_hat = 0;
this->manfilter_.reset_comp();
this->mode_ = mode;
}

1347
utils/香港大学轮腿电控及建模开源-3/Chassis_Task.c Normal file
View File

File diff suppressed because it is too large Load Diff

299
utils/香港大学轮腿电控及建模开源-3/Chassis_Task.h Normal file
View File

@ -0,0 +1,299 @@
#ifndef _CHASSIS_TASK
#define _CHASSIS_TASK
#include "main.h"
#include "struct_typedef.h"
#include "pid.h"
#include "bsp_can.h"
// ------------- Limit info -------------
#define MAX_ACCL 13000.0f
#define MAX_ACCL_JOINT 15.0f
#define MAX_FOOT_OUTPUT 2048
// ------------- Target value info -------------
#define SIT_MODE_HEIGHT_SET 0.18f
#define NORMAL_MODE_HEIGHT_SET 0.10f
#define HIGH_MODE_HEIGHT_SET 0.27f
#define EXTREMELY_HIGH_MODE_HEIGHT_SET 0.30f
// ------------- Mech info -------------
#define L1 0.15f
#define L2 0.25f
#define L3 0.25f
#define L4 0.15f
#define L5 0.1f
#define WHEEL_PERIMETER 0.56547
#define WHEEL_RADIUS 0.09f
#define LEG_OFFSET 0.3790855135f // 水平位置到上限位的夹角
#define LOWER_SUPPORT_FORCE_FOR_JUMP 5.0f
#define LOWER_SUPPORT_FORCE 0.0f
#define MOVE_LOWER_BOUND 0.3f
#define EXIT_PITCH_ANGLE 0.1f
#define DANGER_PITCH_ANGLE 0.25f
#define FEED_f 7.5f
#define FEED_f_1 3.5f
#define NORMAL_MODE_WEIGHT_DISTANCE_OFFSET -0.0f
#define MOTOR_POS_UPPER_BOUND 0.05f
#define MOTOR_POS_LOWER_BOUND 1.4f
#define LIMITED_TORQUE 0.5f
#define UNLIMITED_TORQUE 200.0f
// ------------- Time info -------------
#define CHASSIS_TASK_INIT_TIME 500
#define TASK_RUN_TIME 0.002f
// ------------- Transfer info -------------
#define MOTOR_ECD_TO_RAD 0.00019174779 // 2*PI / 32767
#define HALF_ECD_RANGE 14383
#define HALF_POSITION_RANGE 3.0f
// #define CC 0.00512f
// #define CC 1/494.0f
#define TORQ_K 494.483818182
// ------------- Math info -------------
#define PI2 6.28318530717959f
#define PI 3.14159265358979f
#define PI_2 1.57079632679489f
#define PI_4 0.78539816339744f
typedef enum
{
ENABLE_CHASSIS = 0,
DISABLE_CHASSIS,
} chassis_mode_e;
typedef enum
{
NO_FORCE,
FOOT_LAUNCHING,
JOINT_LAUNCHING,
BALANCING_READY,
JOINT_REDUCING,
} chassis_balancing_mode_e;
typedef enum
{
NONE,
NORMAL_MOVING_MODE,
ABNORMAL_MOVING_MODE,
JUMPING_MODE,
CAP_MODE,
FLY_MODE,
TK_MODE,
} sport_mode_e;
typedef enum
{
READY_TO_JUMP,
PREPARING_LANDING,
PREPARING_STAND_JUMPING,
CONSTACTING_LEGS,
EXTENDING_LEGS,
CONSTACTING_LEGS_2,
FINISHED,
} jumping_stage_e;
typedef enum
{
NOT_DEFINE,
STANDING_JUMP,
MOVING_JUMP,
} jumping_mode_e;
typedef enum
{
SIT_MODE = 0,
NORMAL_MODE,
HIGH_MODE,
EXTREMELY_HIGH_MODE,
CHANGING_HIGH,
} chassis_high_mode_e;
typedef enum
{
MOTOR_NO_FORCE = 0,
MOTOR_FORCE,
} chassis_motor_mode_e;
typedef enum
{
ON_GROUND = 0,
OFF_GROUND = 1,
} suspend_flag_e;
typedef struct
{
chassis_mode_e chassis_mode, last_chassis_mode;
chassis_balancing_mode_e chassis_balancing_mode, last_chassis_balancing_mode;
sport_mode_e sport_mode, last_sport_mode;
jumping_mode_e jumping_mode, last_jumping_mode;
jumping_stage_e jumping_stage, last_jumping_stage;
chassis_high_mode_e chassis_high_mode, last_chassis_high_mode;
} mode_t;
typedef struct
{
const fp32 *chassis_INS_angle_point;
const fp32 *chassis_INS_gyro_point;
const fp32 *chassis_INS_accel_point;
fp32 yaw_angle, pitch_angle, roll_angle;
fp32 yaw_gyro, pitch_gyro, roll_gyro;
fp32 yaw_accel, pitch_accel, roll_accel;
fp32 yaw_angle_sett, pitch_angle_set, roll_angle_set;
fp32 yaw_gyro_set, pitch_gyro_set, roll_gyro_set;
fp32 ideal_high;
fp32 leg_length_L, last_leg_length_L, leg_length_L_set;
fp32 leg_length_R, last_leg_length_R, leg_length_R_set;
fp32 leg_dlength_L;
fp32 leg_dlength_R;
fp32 foot_roll_angle;
fp32 leg_angle_L, last_leg_angle_L, leg_angle_L_set;
fp32 leg_angle_R, last_leg_angle_R, leg_angle_R_set;
fp32 leg_gyro_L, leg_gyro_R;
fp32 foot_distance, foot_distance_K, foot_distance_set;
fp32 foot_speed, foot_speed_K, foot_speed_set;
fp32 supportive_force_L, supportive_force_R;
} chassis_posture_info_t;
typedef struct
{
// -------- horizontal force --------
fp32 joint_balancing_torque_L, joint_balancing_torque_R;
fp32 foot_balancing_torque_L, foot_balancing_torque_R;
fp32 foot_moving_torque_L, foot_moving_torque_R;
fp32 joint_moving_torque_L, joint_moving_torque_R;
fp32 joint_prevent_splits_torque_L, joint_prevent_splits_torque_R;
fp32 joint_horizontal_torque_L, joint_horizontal_torque_R;
fp32 foot_horizontal_torque_L, foot_horizontal_torque_R;
fp32 joint_horizontal_torque_temp1_R, joint_horizontal_torque_temp2_R;
fp32 joint_horizontal_torque_temp1_L, joint_horizontal_torque_temp2_L;
fp32 yaw_torque;
// -------- vertical force --------
fp32 joint_roll_torque_L, joint_roll_torque_R;
fp32 joint_stand_torque_L, joint_stand_torque_R;
fp32 joint_vertical_torque_L, joint_vertical_torque_R;
fp32 joint_real_vertical_torque_L, joint_real_vertical_torque_R;
fp32 joint_vertical_torque_temp1_R, joint_vertical_torque_temp2_R;
fp32 joint_vertical_torque_temp1_L, joint_vertical_torque_temp2_L;
} torque_info_t;
typedef struct
{
fp32 J1_L,J2_L;
fp32 J3_L,J4_L;
fp32 J1_R,J2_R;
fp32 J3_R,J4_R;
fp32 invJ1_L,invJ2_L;
fp32 invJ3_L,invJ4_L;
fp32 invJ1_R,invJ2_R;
fp32 invJ3_R,invJ4_R;
} mapping_info_t;
typedef struct
{
const HT_motor_measure_t *motor_measure;
chassis_motor_mode_e motor_mode, last_motor_mode;
bool_t offline_flag;
fp32 position;
fp32 init_position;
fp32 position_offset;
fp32 velocity;
fp32 torque_out, torque_get;
fp32 max_torque, min_torque;
} joint_motor_t;
typedef struct
{
motor_measure_t motor_measure;
chassis_motor_mode_e motor_mode, last_motor_mode;
bool_t offline_flag;
fp32 distance, distance_offset, last_position, position, turns;
fp32 speed;
fp32 torque_out, torque_get;
} foot_motor_t;
typedef struct
{
bool_t init_flag;
suspend_flag_e suspend_flag_L, last_suspend_flag_L;
suspend_flag_e suspend_flag_R, last_suspend_flag_R;
bool_t Ignore_Off_Ground;
bool_t abnormal_flag;
bool_t static_flag, last_static_flag;
bool_t moving_flag, last_moving_flag;
bool_t rotation_flag;
bool_t controlling_flag;
bool_t set_pos_after_moving;
bool_t overpower_warning_flag;
bool_t last_overpower_warning_flag;
bool_t stablize_high_flag;
bool_t last_stablize_high_flag;
} flag_info_t;
typedef struct
{
pid_type_def buffer_control_pid;
pid_type_def cap_pid;
pid_type_def scale_down_pid;
} pid_info_t;
typedef struct
{
mode_t mode;
chassis_posture_info_t chassis_posture_info;
torque_info_t torque_info;
mapping_info_t mapping_info;
flag_info_t flag_info;
pid_info_t pid_info;
const Gimbal_ctrl_t *chassis_rc_ctrl;
joint_motor_t joint_motor_1, joint_motor_2, joint_motor_3, joint_motor_4;
foot_motor_t foot_motor_L, foot_motor_R;
}chassis_control_t;
enum Chassis_Mode
{
Chassis_No_Force = 0,
Follow_Gimbal,
Follow_Gimbal_90Deg,
No_Follow,
Rotate,
// TK_MODE,
};
extern enum Chassis_Mode chassis_mode;
extern chassis_control_t chassis_control;
extern fp32 roll_PD[2];
#endif

321
utils/香港大学轮腿电控及建模开源-3/HerKules_VOCAL_SJ_LQR_v4_with_data.m Normal file
View File

@ -0,0 +1,321 @@
% v1：这份LQR程序是参考我之前写的哈工程LQR程序以及小周写的AB矩阵求解器优化后写出来的，感谢周神（2024/05/07）
% v2：添加了可以专门调试定腿长的功能（2024/05/08）
% v3：优化部分注释，添加单位说明（2024/05/15）
% v4: 优化了输出，输出矩阵K的系数可以真正的复制到C里（2024/05/16）
% 以下所有变量含义参考2023上交轮腿电控开源（https://bbs.robomaster.com/forum.php?mod=viewthread&tid=22756）所使用符号含义
%%%%%%%%%%%%%%%%%%%%%%%%%Step 0：重置程序，定义变量%%%%%%%%%%%%%%%%%%%%%%%%%
tic
clear all
clc
% 定义机器人机体参数
syms R_w % 驱动轮半径
syms R_l % 驱动轮轮距/2
syms l_l l_r % 左右腿长
syms l_wl l_wr % 驱动轮质心到左右腿部质心距离
syms l_bl l_br % 机体质心到左右腿部质心距离
syms l_c % 机体质心到腿部关节中心点距离
syms m_w m_l m_b % 驱动轮质量 腿部质量 机体质量
syms I_w % 驱动轮转动惯量 (自然坐标系法向)
syms I_ll I_lr % 驱动轮左右腿部转动惯量 (自然坐标系法向，实际上会变化)
syms I_b % 机体转动惯量 (自然坐标系法向)
syms I_z % 机器人z轴转动惯量 (简化为常量)
% 定义其他独立变量并补充其导数
syms theta_wl theta_wr % 左右驱动轮转角
syms dtheta_wl dtheta_wr
syms ddtheta_wl ddtheta_wr ddtheta_ll ddtheta_lr ddtheta_b
% 定义状态向量
syms s ds phi dphi theta_ll dtheta_ll theta_lr dtheta_lr theta_b dtheta_b
% 定义控制向量
syms T_wl T_wr T_bl T_br
% 输入物理参数：重力加速度
syms g
%%%%%%%%%%%%%%%%%%%%%%%Step 1：解方程，求控制矩阵A，B%%%%%%%%%%%%%%%%%%%%%%%
% 通过原文方程组(3.11)-(3.15)，求出ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b表达式
eqn1 = (I_w*l_l/R_w+m_w*R_w*l_l+m_l*R_w*l_bl)*ddtheta_wl+(m_l*l_wl*l_bl-I_ll)*ddtheta_ll+(m_l*l_wl+m_b*l_l/2)*g*theta_ll+T_bl-T_wl*(1+l_l/R_w)==0;
eqn2 = (I_w*l_r/R_w+m_w*R_w*l_r+m_l*R_w*l_br)*ddtheta_wr+(m_l*l_wr*l_br-I_lr)*ddtheta_lr+(m_l*l_wr+m_b*l_r/2)*g*theta_lr+T_br-T_wr*(1+l_r/R_w)==0;
eqn3 = -(m_w*R_w*R_w+I_w+m_l*R_w*R_w+m_b*R_w*R_w/2)*ddtheta_wl-(m_w*R_w*R_w+I_w+m_l*R_w*R_w+m_b*R_w*R_w/2)*ddtheta_wr-(m_l*R_w*l_wl+m_b*R_w*l_l/2)*ddtheta_ll-(m_l*R_w*l_wr+m_b*R_w*l_r/2)*ddtheta_lr+T_wl+T_wr==0;
eqn4 = (m_w*R_w*l_c+I_w*l_c/R_w+m_l*R_w*l_c)*ddtheta_wl+(m_w*R_w*l_c+I_w*l_c/R_w+m_l*R_w*l_c)*ddtheta_wr+m_l*l_wl*l_c*ddtheta_ll+m_l*l_wr*l_c*ddtheta_lr-I_b*ddtheta_b+m_b*g*l_c*theta_b-(T_wl+T_wr)*l_c/R_w-(T_bl+T_br)==0;
eqn5 = ((I_z*R_w)/(2*R_l)+I_w*R_l/R_w)*ddtheta_wl-((I_z*R_w)/(2*R_l)+I_w*R_l/R_w)*ddtheta_wr+(I_z*l_l)/(2*R_l)*ddtheta_ll-(I_z*l_r)/(2*R_l)*ddtheta_lr-T_wl*R_l/R_w+T_wr*R_l/R_w==0;
[ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b] = solve(eqn1,eqn2,eqn3,eqn4,eqn5,ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b);
% 通过计算雅可比矩阵的方法得出控制矩阵A，B所需要的全部偏导数
J_A = jacobian([ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b],[theta_ll,theta_lr,theta_b]);
J_B = jacobian([ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b],[T_wl,T_wr,T_bl,T_br]);
% 定义矩阵A，B，将指定位置的数值根据上述偏导数计算出来并填入
A = sym('A',[10 10]);
B = sym('B',[10 4]);
% 填入A数据：a25,a27,a29,a45,a47,a49,a65,a67,a69,a85,a87,a89,a105,a107,a109
for p = 5:2:9
A_index = (p - 3)/2;
A(2,p) = R_w*(J_A(1,A_index) + J_A(2,A_index))/2;
A(4,p) = (R_w*(- J_A(1,A_index) + J_A(2,A_index)))/(2*R_l) - (l_l*J_A(3,A_index))/(2*R_l) + (l_r*J_A(4,A_index))/(2*R_l);
for q = 6:2:10
A(q,p) = J_A(q/2,A_index);
end
end
% A的以下数值为1：a12,a34,a56,a78,a910，其余数值为0
for r = 1:10
if rem(r,2) == 0
A(r,1) = 0; A(r,2) = 0; A(r,3) = 0; A(r,4) = 0; A(r,6) = 0; A(r,8) = 0; A(r,10) = 0;
else
A(r,:) = zeros(1,10);
A(r,r+1) = 1;
end
end
% 填入B数据：b21,b22,b23,b24,b41,b42,b43,b44,b61,b62,b63,b64,b81,b82,b83,b84,b101,b102,b103,b104,
for h = 1:4
B(2,h) = R_w*(J_B(1,h) + J_B(2,h))/2;
B(4,h) = (R_w*(- J_B(1,h) + J_B(2,h)))/(2*R_l) - (l_l*J_B(3,h))/(2*R_l) + (l_r*J_B(4,h))/(2*R_l);
for f = 6:2:10
B(f,h) = J_B(f/2,h);
end
end
% B的其余数值为0
for e = 1:2:9
B(e,:) = zeros(1,4);
end
%%%%%%%%%%%%%%%%%%%%%Step 2：输入参数（可以修改的部分）%%%%%%%%%%%%%%%%%%%%%
% 物理参数赋值（唯一此处不可改变！），后面的数据通过增加后缀_ac区分模型符号和实际数据
g_ac = 9.81;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 此处可以输入机器人机体基本参数 %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%机器人机体与轮部参数%%%%%%%%%%%%%%%%%%%%%%%%%%%%
R_w_ac = 0.9; % 驱动轮半径 （单位：m）
R_l_ac = 0.25; % 两个驱动轮之间距离/2 （单位：m）
l_c_ac = 0.037; % 机体质心到腿部关节中心点距离 （单位：m）
m_w_ac = 0.8; m_l_ac = 1.6183599; m_b_ac = 11.542; % 驱动轮质量 腿部质量 机体质量 （单位：kg）
I_w_ac = (3510000)*10^(-7); % 驱动轮转动惯量 （单位：kg m^2）
I_b_ac = 0.260; % 机体转动惯量(自然坐标系法向) （单位：kg m^2）
I_z_ac = 0.226; % 机器人z轴转动惯量 （单位：kg m^2）
%%%%%%%%%%%%%%%%%%%%%%机器人腿部参数（定腿长调试用）%%%%%%%%%%%%%%%%%%%%%%%%
% 如果需要使用此部分，请去掉120-127行以及215-218行注释，然后将224行之后的所有代码注释掉
% 或者点击左侧数字"224"让程序只运行行之前的内容并停止
l_l_ac = 0.18; % 左腿摆杆长度 （左腿对应数据） （单位：m）
l_wl_ac = 0.05; % 左驱动轮质心到左腿摆杆质心距离 （单位：m）
l_bl_ac = 0.13; % 机体转轴到左腿摆杆质心距离 （单位：m）
I_ll_ac = 0.02054500; % 左腿摆杆转动惯量 （单位：kg m^2）
l_r_ac = 0.18; % 右腿摆杆长度 （右腿对应数据） （单位：m）
l_wr_ac = 0.05; % 右驱动轮质心到右腿摆杆质心距离 （单位：m）
l_br_ac = 0.13; % 机体转轴到右腿摆杆质心距离 （单位：m）
I_lr_ac = 0.02054500; % 右腿摆杆转动惯量 （单位：kg m^2）
% 机体转轴定义参考哈工程开源（https://zhuanlan.zhihu.com/p/563048952），是左右
% 两侧两个关节电机之间的中间点相连所形成的轴
% （如果目的是小板凳，考虑使左右腿相关数据一致）
%%%%%%%%%%%%%%%%%%%%%%%%%%%机器人腿部参数数据集%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 根据不同腿长长度，先针对左腿测量出对应的l_wl，l_bl，和I_ll
% 通过以下方式记录数据: 矩阵分4列，
% 第一列为左腿腿长范围区间中所有小数点精度0.01的长度，例如：0.09，0.18，单位：m
% 第二列为l_wl，单位：m
% 第三列为l_bl，单位：m
% 第四列为I_ll，单位：kg m^2
% （注意单位别搞错！）
% 行数根据L_0范围区间（，单位cm时）的整数数量进行调整
Leg_data_l = [0.11, 0.0480, 0.0620, 0.01819599;
0.12, 0.0470, 0.0730, 0.01862845;
0.13, 0.0476, 0.0824, 0.01898641;
0.14, 0.0480, 0.0920, 0.01931342;
0.15, 0.0490, 0.1010, 0.01962521;
0.16, 0.0500, 0.1100, 0.01993092;
0.17, 0.0510, 0.1190, 0.02023626;
0.18, 0.0525, 0.1275, 0.02054500;
0.19, 0.0539, 0.1361, 0.02085969;
0.20, 0.0554, 0.1446, 0.02118212;
0.21, 0.0570, 0.1530, 0.02151357;
0.22, 0.0586, 0.1614, 0.02185496;
0.23, 0.0600, 0.1700, 0.02220695;
0.24, 0.0621, 0.1779, 0.02256999;
0.25, 0.0639, 0.1861, 0.02294442;
0.26, 0.0657, 0.1943, 0.02333041;
0.27, 0.0676, 0.2024, 0.02372806;
0.28, 0.0700, 0.2100, 0.02413735;
0.29, 0.0713, 0.2187, 0.02455817;
0.30, 0.0733, 0.2267, 0.02499030];
% 以上数据应通过实际测量或sw图纸获得
% 由于左右腿部数据通常完全相同，我们通过复制的方式直接定义右腿的全部数据集
% 矩阵分4列，第一列为右腿腿长范围区间中（，单位cm时）的整数腿长l_r*0.01，第二列为l_wr，第三列为l_br，第四列为I_lr）
Leg_data_r = Leg_data_l;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 此处可以输入QR矩阵 %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 矩阵Q中，以下列分别对应：
% s ds phi dphi theta_ll dtheta_ll theta_lr dtheta_lr theta_b dtheta_b
lqr_Q = [1, 0, 0, 0, 0, 0, 0, 0, 0, 0;
0, 2, 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, 12000, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 200, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 1000, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 1, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 1000, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 1, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 20000, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 1];
% 其中：
% s : 自然坐标系下机器人水平方向移动距离，单位：m，ds为其导数
% phi ：机器人水平方向移动时yaw偏航角度，dphi为其导数
% theta_ll：左腿摆杆与竖直方向（自然坐标系z轴）夹角，dtheta_ll为其导数
% theta_lr：右腿摆杆与竖直方向（自然坐标系z轴）夹角，dtheta_lr为其导数
% theta_b ：机体与自然坐标系水平夹角，dtheta_b为其导数
% 矩阵中，以下列分别对应：
% T_wl T_wr T_bl T_br
lqr_R = [0.25, 0, 0, 0;
0, 0.25, 0, 0;
0, 0, 1.5, 0;
0, 0, 0, 1.5];
% 其中：
% T_wl: 左侧驱动轮输出力矩
% T_wr：右侧驱动轮输出力矩
% T_bl：左侧髋关节输出力矩
% T_br：右腿髋关节输出力矩
% 单位皆为Nm
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%Step 2.5：求解矩阵（定腿长调试用）%%%%%%%%%%%%%%%%%%%%%
% 如果需要使用此部分，请去掉120-127行以及215-218行注释，然后将224行之后的所有代码注释掉，
% 或者点击左侧数字"224"让程序只运行行之前的内容并停止
K = get_K_from_LQR(R_w,R_l,l_l,l_r,l_wl,l_wr,l_bl,l_br,l_c,m_w,m_l,m_b,I_w,I_ll,I_lr,I_b,I_z,g, ...
R_w_ac,R_l_ac,l_l_ac,l_r_ac,l_wl_ac,l_wr_ac,l_bl_ac,l_br_ac, ...
l_c_ac,m_w_ac,m_l_ac,m_b_ac,I_w_ac,I_ll_ac,I_lr_ac,I_b_ac,I_z_ac,g_ac, ...
A,B,lqr_Q,lqr_R)
K = sprintf([strjoin(repmat({'%.5g'},1,size(K,2)),', ') '\n'], K.')
%%%%%%%%%%%%%%%%%%%%%%%%%%Step 3：拟合控制律函数%%%%%%%%%%%%%%%%%%%%%%%%%%
sample_size = size(Leg_data_l,1)^2; % 单个K_ij拟合所需要的样本数
length = size(Leg_data_l,1); % 测量腿部数据集的行数
% 定义所有K_ij依据l_l,l_r变化的表格，每一个表格有3列，第一列是l_l，第二列
% 是l_r，第三列是对应的K_ij的数值
K_sample = zeros(sample_size,3,40); % 40是因为增益矩阵K应该是4行10列。
for i = 1:length
for j = 1:length
index = (i - 1)*length + j;
l_l_ac = Leg_data_l(i,1); % 提取左腿对应的数据
l_wl_ac = Leg_data_l(i,2);
l_bl_ac = Leg_data_l(i,3);
I_ll_ac = Leg_data_l(i,4);
l_r_ac = Leg_data_r(j,1); % 提取右腿对应的数据
l_wr_ac = Leg_data_r(j,2);
l_br_ac = Leg_data_r(j,3);
I_lr_ac = Leg_data_r(j,4);
for k = 1:40
K_sample(index,1,k) = l_l_ac;
K_sample(index,2,k) = l_r_ac;
end
K = get_K_from_LQR(R_w,R_l,l_l,l_r,l_wl,l_wr,l_bl,l_br,l_c,m_w,m_l,m_b,I_w,I_ll,I_lr,I_b,I_z,g, ...
R_w_ac,R_l_ac,l_l_ac,l_r_ac,l_wl_ac,l_wr_ac,l_bl_ac,l_br_ac, ...
l_c_ac,m_w_ac,m_l_ac,m_b_ac,I_w_ac,I_ll_ac,I_lr_ac,I_b_ac,I_z_ac,g_ac, ...
A,B,lqr_Q,lqr_R);
% 根据指定的l_l,l_r输入对应的K_ij的数值
for l = 1:4
for m = 1:10
K_sample(index,3,(l - 1)*10 + m) = K(l,m);
end
end
end
end
% 创建收集全部K_ij的多项式拟合的全部系数的集合
K_Fit_Coefficients = zeros(40,6);
for n = 1:40
K_Surface_Fit = fit([K_sample(:,1,n),K_sample(:,2,n)],K_sample(:,3,n),'poly22');
K_Fit_Coefficients(n,:) = coeffvalues(K_Surface_Fit); % 拟合并提取出拟合的系数结果
end
Polynomial_expression = formula(K_Surface_Fit)
% 最终返回的结果K_Fit_Coefficients是一个40行6列矩阵，每一行分别对应一个K_ij的多项式拟合的全部系数
% 每一行和K_ij的对应关系如下：
% - 第1行对应K_1,1
% - 第14行对应K_2,4
% - 第22行对应K_3,2
% - 第37行对应K_4,7
% ... 其他行对应关系类似
% 拟合出的函数表达式为 p(x,y) = p00 + p10*x + p01*y + p20*x^2 + p11*x*y + p02*y^2
% 其中x对应左腿腿长l_l，y对应右腿腿长l_r
% K_Fit_Coefficients每一列分别对应全部K_ij的多项式拟合的单个系数
% 每一列和系数pij的对应关系如下：
% - 第1列对应p00
% - 第2列对应p10
% - 第3列对应p01
% - 第4列对应p20
% - 第5列对应p11
% - 第6列对应p02
K_Fit_Coefficients = sprintf([strjoin(repmat({'%.5g'},1,size(K_Fit_Coefficients,2)),', ') '\n'], K_Fit_Coefficients.')
% 正确食用方法：
% 1.在C代码中写出控制律K矩阵的全部多项式，其中每一个多项式的表达式为：
% p(l_l,l_r) = p00 + p10*l_l + p01*l_r + p20*l_l^2 + p11*l_l*l_r + p02*l_r^2
% 2.并填入对应系数即可
toc
%%%%%%%%%%%%%%%%%%%%%%%%%以下信息仅供参考，可忽略%%%%%%%%%%%%%%%%%%%%%%%%%%
% 如有需要可以把所有K_ij画出图来参考，可以去掉以下注释
% 此版本只能同时查看其中一个K_ij，同时查看多个的功能下次更新
% （前面的蛆，以后再来探索吧（bushi
%%%%%%%%%%%%%%%%%%%%%%%%%%得出控制矩阵K使用的函数%%%%%%%%%%%%%%%%%%%%%%%%%%
function K = get_K_from_LQR(R_w,R_l,l_l,l_r,l_wl,l_wr,l_bl,l_br,l_c,m_w,m_l,m_b,I_w,I_ll,I_lr,I_b,I_z,g, ...
R_w_ac,R_l_ac,l_l_ac,l_r_ac,l_wl_ac,l_wr_ac,l_bl_ac,l_br_ac, ...
l_c_ac,m_w_ac,m_l_ac,m_b_ac,I_w_ac,I_ll_ac,I_lr_ac,I_b_ac,I_z_ac,g_ac, ...
A,B,lqr_Q,lqr_R)
% 基于机体以及物理参数，获得控制矩阵A，B的全部数值
A_ac = subs(A,[R_w R_l l_l l_r l_wl l_wr l_bl l_br l_c m_w m_l m_b I_w I_ll I_lr I_b I_z g], ...
[R_w_ac R_l_ac l_l_ac l_r_ac l_wl_ac l_wr_ac l_bl_ac l_br_ac l_c_ac ...
m_w_ac m_l_ac m_b_ac I_w_ac I_ll_ac I_lr_ac I_b_ac I_z_ac g_ac]);
B_ac = subs(B,[R_w R_l l_l l_r l_wl l_wr l_bl l_br l_c m_w m_l m_b I_w I_ll I_lr I_b I_z g], ...
[R_w_ac R_l_ac l_l_ac l_r_ac l_wl_ac l_wr_ac l_bl_ac l_br_ac l_c_ac ...
m_w_ac m_l_ac m_b_ac I_w_ac I_ll_ac I_lr_ac I_b_ac I_z_ac g_ac]);
% 根据以上信息和提供的矩阵Q和R求解Riccati方程，获得增益矩阵K
% P为Riccati方程的解，矩阵L可以无视
[P,K,L_k] = icare(A_ac,B_ac,lqr_Q,lqr_R,[],[],[]);
end

304
utils/香港大学轮腿电控及建模开源-3/some_functions.c Normal file
View File

@ -0,0 +1,304 @@
// Front
// | 1 4 |
// (1)Left| |Right(2)
// | 2 3 |
#define HT_SLAVE_ID1 0x01
#define HT_SLAVE_ID2 0x02
#define HT_SLAVE_ID3 0x03
#define HT_SLAVE_ID4 0x04
#define CAN_BM_M1_ID 0x97
#define CAN_BM_M2_ID 0x98
#define BM_CAN hcan2
#define get_HT_motor_measure(ptr, data) \
{ \
(ptr)->last_ecd = (ptr)->ecd; \
(ptr)->ecd = uint_to_float((uint16_t)((data)[1] << 8 | (data)[2] ),P_MIN, P_MAX, 16); \
(ptr)->speed_rpm = uint_to_float((uint16_t)(data[3]<<4)|(data[4]>>4), V_MIN, V_MAX, 12);\
(ptr)->real_torque = uint_to_float((uint16_t)(((data[4] & 0x0f) << 8) | (data)[5]), -18, +18, 12); \
}
#define get_BM_motor_measure(ptr, data) \
{ \
(ptr)->last_ecd = (ptr)->ecd; \
(ptr)->ecd = (uint16_t)((data)[4] << 8 | (data)[5]); \
(ptr)->speed_rpm = (int16_t)((data)[0] << 8 | (data)[1]); \
(ptr)->given_current = (int16_t)((data)[2] << 8 | (data)[3]); \
(ptr)->error = (data)[6]; \
(ptr)->mode = (data)[7]; \
}
void CAN_BM_ENABLE_CMD(void)
{
CAN_TxHeaderTypeDef bm_tx_measure;
uint8_t bm_can_send_data[8];
uint32_t send_mail_box;
bm_tx_measure.StdId = 0x105; // 120
bm_tx_measure.IDE = CAN_ID_STD;
bm_tx_measure.RTR = CAN_RTR_DATA;
bm_tx_measure.DLC = 0x08;
bm_can_send_data[0] = 0x0A;
bm_can_send_data[1] = 0x0A;
bm_can_send_data[2] = 0x00;
bm_can_send_data[3] = 0x00;
bm_can_send_data[4] = 0x00;
bm_can_send_data[5] = 0x00;
bm_can_send_data[6] = 0x00;
bm_can_send_data[7] = 0x00;
HAL_CAN_AddTxMessage(&BM_CAN, &bm_tx_measure, bm_can_send_data, &send_mail_box);
}
void CAN_BM_MODE_SET_CMD(void)
{
CAN_TxHeaderTypeDef bm_tx_measure;
uint8_t bm_can_send_data[8];
uint32_t send_mail_box;
bm_tx_measure.StdId = 0x106; // 120
bm_tx_measure.IDE = CAN_ID_STD;
bm_tx_measure.RTR = CAN_RTR_DATA;
bm_tx_measure.DLC = 0x08;
bm_can_send_data[0] = 0x01;
bm_can_send_data[1] = 0x01;
bm_can_send_data[2] = 0x00;
bm_can_send_data[3] = 0x00;
bm_can_send_data[4] = 0x00;
bm_can_send_data[5] = 0x00;
bm_can_send_data[6] = 0x00;
bm_can_send_data[7] = 0x00;
HAL_CAN_AddTxMessage(&BM_CAN, &bm_tx_measure, bm_can_send_data, &send_mail_box);
}
void CAN_BM_CONTROL_CMD( int16_t motor1, int16_t motor2 )
{
CAN_TxHeaderTypeDef bm_tx_measure;
uint8_t bm_can_send_data[8];
uint32_t send_mail_box;
bm_tx_measure.StdId = 0x32; // 120
bm_tx_measure.IDE = CAN_ID_STD;
bm_tx_measure.RTR = CAN_RTR_DATA;
bm_tx_measure.DLC = 0x08;
bm_can_send_data[0] = motor1 >> 8;
bm_can_send_data[1] = motor1;
bm_can_send_data[2] = motor2 >> 8;
bm_can_send_data[3] = motor2;
bm_can_send_data[4] = 0x00;
bm_can_send_data[5] = 0x00;
bm_can_send_data[6] = 0x00;
bm_can_send_data[7] = 0x00;
HAL_CAN_AddTxMessage(&BM_CAN, &bm_tx_measure, bm_can_send_data, &send_mail_box);
}
#define LIMIT_MIN_MAX(x,min,max) (x) = (((x)<=(min))?(min):(((x)>=(max))?(max):(x)))
#define P_MIN -95.5f// Radians
#define P_MAX 95.5f
#define V_MIN -45.0f// Rad/s
#define V_MAX 45.0f
#define KP_MIN 0.0f// N-m/rad
#define KP_MAX 500.0f
#define KD_MIN 0.0f// N-m/rad/s
#define KD_MAX 5.0f
#define T_MIN -18.0f
#define T_MAX 18.0f
void CAN_HT_CMD( uint8_t id, fp32 f_t )
{
uint32_t canTxMailbox = CAN_TX_MAILBOX0;
fp32 f_p = 0.0f, f_v = 0.0f, f_kp = 0.0f, f_kd = 0.0f;
uint16_t p, v, kp, kd, t;
uint8_t buf[8];
LIMIT_MIN_MAX(f_p, P_MIN, P_MAX);
LIMIT_MIN_MAX(f_v, V_MIN, V_MAX);
LIMIT_MIN_MAX(f_kp, KP_MIN, KP_MAX);
LIMIT_MIN_MAX(f_kd, KD_MIN, KD_MAX);
LIMIT_MIN_MAX(f_t, T_MIN, T_MAX);
p = float_to_uint(f_p, P_MIN, P_MAX, 16);
v = float_to_uint(f_v, V_MIN, V_MAX, 12);
kp = float_to_uint(f_kp, KP_MIN, KP_MAX, 12);
kd = float_to_uint(f_kd, KD_MIN, KD_MAX, 12);
t = float_to_uint(f_t, T_MIN, T_MAX, 12);
buf[0] = p>>8;
buf[1] = p&0xFF;
buf[2] = v>>4;
buf[3] = ((v&0xF)<<4)|(kp>>8);
buf[4] = kp&0xFF;
buf[5] = kd>>4;
buf[6] = ((kd&0xF)<<4)|(t>>8);
buf[7] = t&0xff;
chassis_tx_message.StdId = id;
chassis_tx_message.IDE = CAN_ID_STD;
chassis_tx_message.RTR = CAN_RTR_DATA;
chassis_tx_message.DLC = 0x08;
// while (HAL_CAN_GetTxMailboxesFreeLevel(&hcan1) == 0);
if ((hcan1.Instance->TSR & CAN_TSR_TME0) != RESET)
{canTxMailbox = CAN_TX_MAILBOX0;}
else if ((hcan1.Instance->TSR & CAN_TSR_TME1) != RESET)
{canTxMailbox = CAN_TX_MAILBOX1;}
else if ((hcan1.Instance->TSR & CAN_TSR_TME2) != RESET)
{canTxMailbox = CAN_TX_MAILBOX2;}
if(HAL_CAN_AddTxMessage(&hcan1, &chassis_tx_message, buf, (uint32_t *)&canTxMailbox)==HAL_OK){};
}
void CAN_CMD_HT_Enable(uint8_t id, uint8_t unterleib_motor_send_data[8] )
{
uint32_t canTxMailbox= CAN_TX_MAILBOX0;
chassis_tx_message.StdId = id;
chassis_tx_message.IDE = CAN_ID_STD;
chassis_tx_message.RTR = CAN_RTR_DATA;
chassis_tx_message.DLC = 0x08;
// while (HAL_CAN_GetTxMailboxesFreeLevel(&hcan1) == 0);
if ((hcan1.Instance->TSR & CAN_TSR_TME0) != RESET)
{canTxMailbox = CAN_TX_MAILBOX0;}
else if ((hcan1.Instance->TSR & CAN_TSR_TME1) != RESET)
{canTxMailbox = CAN_TX_MAILBOX1;}
else if ((hcan1.Instance->TSR & CAN_TSR_TME2) != RESET)
{canTxMailbox = CAN_TX_MAILBOX2;}
if(HAL_CAN_AddTxMessage(&hcan1, &chassis_tx_message, unterleib_motor_send_data, (uint32_t *)&canTxMailbox)==HAL_OK){};
}
void Forward_kinematic_solution(chassis_control_t *feedback_update,
fp32 Q1,fp32 S1,fp32 Q4,fp32 S4, uint8_t ce)
{
fp32 dL0=0,L0=0,Q0=0,S0=0;
fp32 xb,xd,yb,yd,Lbd,xc,yc;
fp32 A0,B0,C0,Q2,Q3,S2,S3;
fp32 vxb,vxd,vyb,vyd,vxc,vyc;
fp32 cos_Q1,cos_Q4,sin_Q1,sin_Q4;
fp32 sin_Q2,cos_Q2,sin_Q3,cos_Q3;
fp32 axb,ayb,axd,ayd,a2,axc;
/******************************/
Q1 = PI + Q1;
cos_Q1 = arm_cos_f32(Q1);
sin_Q1 = arm_sin_f32(Q1);
cos_Q4 = arm_cos_f32(Q4);
sin_Q4 = arm_sin_f32(Q4);
xb = -L5/2.0f + L1*cos_Q1;
xd = L5/2.0f + L4*cos_Q4;
yb = L1*sin_Q1;
yd = L4*sin_Q4;
Lbd=(xd-xb)*(xd-xb)+(yd-yb)*(yd-yb);
A0 = 2.0f*L2*(xd-xb);
B0 = 2.0f*L2*(yd-yb);
C0 = L2*L2+Lbd-L3*L3;
Q2 = 2.0f *atan_tl((B0+Sqrt(A0*A0 + B0*B0 -C0*C0))/(A0+C0));
xc = xb + arm_cos_f32(Q2)*L2;
yc = yb + arm_sin_f32(Q2)*L2;
L0=Sqrt( xc*xc + yc*yc );
Q0 = atan(yc/xc);
vxb = -S1*L1*sin_Q1;
vyb = S1*L1*cos_Q1;
vxd = -S4*L4*sin_Q4;
vyd = S4*L4*cos_Q4;
Q3 = atan_tl((yc-yd)/(xc-xd));
S2 = ((vxd-vxb)*arm_cos_f32(Q3) + (vyd-vyb)*arm_sin_f32(Q3))/(L2*arm_sin_f32(Q3-Q2));
S3 = ((vxd-vxb)*arm_cos_f32(Q2) + (vyd-vyb)*arm_sin_f32(Q2))/(L2*arm_sin_f32(Q3-Q2));
vxc = vxb - S2*L2*arm_sin_f32(Q2);
vyc = vyb + S2*L2*arm_cos_f32(Q2);
S0 = 3*(-arm_sin_f32(ABS(Q0))*vxc-arm_cos_f32(Q0)*vyc);
if( Q0 < 0 )
Q0 += PI;
/*******************************/
if (ce)
{
feedback_update->chassis_posture_info .leg_length_L = L0;
feedback_update->chassis_posture_info .leg_angle_L = Q0;
// feedback_update->chassis_posture_info .leg_gyro_L = S0;
}
else
{
feedback_update->chassis_posture_info .leg_length_R = L0;
feedback_update->chassis_posture_info .leg_angle_R = Q0;
// feedback_update->chassis_posture_info .leg_gyro_R = S0;
}
}
void Supportive_Force_Cal( chassis_control_t *Suspend_Detect, fp32 Q1, fp32 Q4, fp32 ce )
{
fp32 dL0=0,L0=0,Q0=0,S0=0;
fp32 xb,xd,yb,yd,Lbd,xc,yc;
fp32 A0,B0,C0,Q2,Q3,S2,S3;
fp32 vxb,vxd,vyb,vyd,vxc,vyc;
fp32 cos_Q1,cos_Q4,sin_Q1,sin_Q4;
fp32 sin_Q2,cos_Q2,sin_Q3,cos_Q3;
fp32 axb,ayb,axd,ayd,a2,axc;
/******************************/
Q1 = PI + Q1;
cos_Q1 = arm_cos_f32(Q1);
sin_Q1 = arm_sin_f32(Q1);
cos_Q4 = arm_cos_f32(Q4);
sin_Q4 = arm_sin_f32(Q4);
xb = -L5/2.0f + L1*cos_Q1;
xd = L5/2.0f + L4*cos_Q4;
yb = L1*sin_Q1;
yd = L4*sin_Q4;
Lbd=sqrt( (xd-xb)*(xd-xb)+(yd-yb)*(yd-yb) );
A0 = 2.0f*L2*(xd-xb);
B0 = 2.0f*L2*(yd-yb);
C0 = L2*L2+Lbd*Lbd-L3*L3;
Q2 = 2.0f *atan_tl((B0+Sqrt(A0*A0 + B0*B0 -C0*C0))/(A0+C0));
xc = xb + arm_cos_f32(Q2)*L2;
yc = yb + arm_sin_f32(Q2)*L2;
L0 = Sqrt( xc*xc + yc*yc );
Q0 = atan_tl(yc/xc);
if( Q0 < 0 )
Q0 += PI;
Q3 = atan_tl((yc-yd)/(xc-xd))+PI;
fp32 J1 = ( L1 * arm_sin_f32(Q0 - Q3) * arm_sin_f32( Q1 - Q2 ) ) / (arm_sin_f32( Q3 - Q2 ));
fp32 J2 = ( L1 * arm_cos_f32(Q0 - Q3) * arm_sin_f32( Q1 - Q2 ) ) / (arm_sin_f32( Q3 - Q2 ) * L0);
fp32 J3 = ( L4 * arm_sin_f32(Q0 - Q2) * arm_sin_f32( Q3 - Q4 ) ) / (arm_sin_f32( Q3 - Q2 ));
fp32 J4 = ( L4 * arm_cos_f32(Q0 - Q2) * arm_sin_f32( Q3 - Q4 ) ) / (arm_sin_f32( Q3 - Q2 ) * L0);
fp32 dett = J1 * J4 - J2 * J3;
fp32 Inv_J1 = J4 / dett;
fp32 Inv_J2 = -J2 / dett;
fp32 Inv_J3 = -J3 / dett;
fp32 Inv_J4 = J1 / dett;
ddebug = yc;
if( ce == 1.0f )
{
Suspend_Detect->chassis_posture_info.supportive_force_L =
Inv_J1 * Suspend_Detect->joint_motor_1.torque_get +
Inv_J2 * Suspend_Detect->joint_motor_2.torque_get;
// if( Suspend_Detect->chassis_posture_info.supportive_force_L < 0.0f )
// Suspend_Detect->chassis_posture_info.supportive_force_L += 40.0f;
}
else
{
Suspend_Detect->chassis_posture_info.supportive_force_R =
Inv_J1 * Suspend_Detect->joint_motor_4.torque_get +
Inv_J2 * Suspend_Detect->joint_motor_3.torque_get;
Suspend_Detect->chassis_posture_info.supportive_force_R *= -1.0f;
// if( Suspend_Detect->chassis_posture_info.supportive_force_R < 0.0f )
// Suspend_Detect->chassis_posture_info.supportive_force_R += 40.0f;
}
}

BIN
utils/香港大学轮腿电控及建模开源-3/香港大学轮腿平衡步兵机械&电控解决方案概括.pdf Normal file
View File

Binary file not shown.