diff --git a/MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.axf b/MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.axf index a99993e..367fcdf 100644 Binary files a/MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.axf and b/MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.axf differ diff --git a/MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.hex b/MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.hex index ec48da3..ad2c533 100644 --- a/MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.hex +++ b/MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.hex @@ -1,6 +1,6 @@ :020000040800F2 :1000000040820124AD03000889FF000869D8000878 -:1000100081FF0008D92B0008D12E01080000000044 +:1000100081FF0008D92B0008C92E0108000000004C :1000200000000000000000000000000011060108B0 :100030006142000800000000D1010108210F010801 :10004000CB030008CB030008CB030008CB03000858 @@ -12,18 +12,18 @@ :1000A000CB030008CB030008CB030008CB030008F8 :1000B000CB030008CB030008CB030008CB030008E8 :1000C000CB030008CB030008CB030008CB030008D8 -:1000D00089040108A1280108B1280108C1280108E4 +:1000D0008904010899280108A9280108B9280108FC :1000E00069420008CB03000800000000CB030008B1 :1000F000CB030008CB030008CB030008CB030008A8 :10010000CB030008CB030008CB030008CB03000897 -:10011000CB030008A9180108CB030008CB03000893 +:10011000CB030008A1180108CB030008CB0300089B :10012000CB030008CB030008CB030008CB03000877 :10013000CB030008CB030008CB030008CB03000867 :1001400000000000000000000000000000000000AF :10015000CB030008CB030008CB030008CB03000847 :10016000CB030008CB030008CB030008CB03000837 :10017000CB030008CB030008CB03000800000000FD -:10018000CB030008CB030008B9180108CB03000813 +:10018000CB030008CB030008B1180108CB0300081B :10019000CB030008CB030008CB030008CB03000807 :1001A000CB030008CB030008CB03000800000000CD :1001B000CB030008CB030008CB030008CB030008E7 @@ -42,7 +42,7 @@ :10028000CB030008CB030008CB030008CB03000816 :1002900000000000CB030008CB03000800000000B2 :1002A00000000000CB030008CB030008CB030008CC -:1002B00091280108CB030008CB0300080143000884 +:1002B00089280108CB030008CB030008014300088C :1002C0001143000801110108CB03000800F002F8F7 :1002D00000F05CF80AA090E8000C82448344AAF184 :1002E0000107DA4501D100F051F8AFF2090EBAE882 @@ -729,9 +729,9 @@ :102D70005C63C4E9D866C4F898630AE00220207551 :102D800007E000BF03202075002084F85C03C4E93D :102D9000D80004F1C40794ED060AD4ED0A0A94EDB4 -:102DA000291AD4ED261A384610F02EF904F5C27609 +:102DA000291AD4ED261A384610F02AF904F5C2760D :102DB000D4ED0E0A94ED120A94ED291AD4ED261AD8 -:102DC000304610F021F9207D042813D8DFE800F008 +:102DC000304610F01DF9207D042813D8DFE800F00C :102DD00018340468040000BF2046294600F0B8F902 :102DE000204600F039FD002040B2BDEC0A8B01B056 :102DF000F0BD00BFFD2040B2BDEC0A8B01B0F0BDBC @@ -940,7 +940,7 @@ :103AA00005D194F85C0300F0FE00022807D094EDE5 :103AB000021A94EDDC2A00EE012A84EDDC2A0020B3 :103AC00040B210BDFF2040B210BD00BFDB0FC9C027 -:103AD000DB0F494000003443BC74933D00000000FC +:103AD000DB0F49400000344396438B3D000000005B :103AE00030EE600AB5EE401AF1EE10FA16DDB6EED1 :103AF000002A21EE022AB4EE420AF1EE10FA03DDAA :103B000030EE410A704700BFB1EE422AB4EE420ADD @@ -2178,8 +2178,8 @@ :1088000003024A6570470000F0B581B0002800F00F :108810002781044690F8950428B9002084F8940430 :10882000204600F035F9032084F8950420680AF00A -:108830000FFBD4E90001267CD4E9022300960AF05C -:1088400047F8002840F00981206800210AF008FB61 +:108830000BFBD4E90001267CD4E9022300960AF060 +:1088400043F8002840F00981206800210AF004FB69 :10885000002840F002812179002900F0F580A1F183 :10886000010C01F0030EBCF1030F4FF0000345D3E0 :1088700001F0FC0204F19A000126002500F8865C54 @@ -2212,10 +2212,10 @@ :108A20009700C3F864020FD0901C002200EBC00135 :108A300004EB810181F8552281F8540281F8582213 :108A4000C1E99722C1F86422D4E90001277CD4E966 -:108A5000022300970AF064F830B1022084F89504EC +:108A5000022300970AF060F830B1022084F89504F0 :108A6000012001B0F0BD00BF00206074012084F837 :108A70009504207B012802D12046FFF7B1FE206833 -:108A80000AF03EF8002001B0F0BD00000000000038 +:108A80000AF03AF8002001B0F0BD0000000000003C :108A900010B5B0B002A9044601F10800B021F7F703 :108AA000C2FD00212068C4F20401884202D030B027 :108AB00010BD00BF002003904FF4802002904FF4BF @@ -2706,11 +2706,11 @@ :10A90000D0F888001E4615460F4628B9002084F866 :10A910008400204602F094FC2420C4F888002068BB :10A92000A16A0268002922F00101016002D02046DC -:10A9300006F0CAFF204607F0D7FA012802D101200D +:10A9300006F0C6FF204607F0D3FA012802D1012015 :10A9400001B0F0BD2068816841F480418160816878 :10A9500021F40041394381603104026841EA4551E4 :10A960006FF3194211430160016841F00101016078 -:10A97000204601B0BDE8F04007F016B80000000026 +:10A97000204601B0BDE8F04007F012B8000000002A :10A9800043F600010068C4F20001884208D140F299 :10A990008460C2F200408069002808BF7047004709 :10A9A00070470000000000002DE9F04381B004462C @@ -3352,17 +3352,17 @@ :10D16000B040FFF73DBC00BF2046FFF7D9FC0020D0 :10D17000C4F89000B0BD00BF2020C4F88800002093 :10D18000A0672046BDE8B04000F05ABBD4F880004C -:10D1900080B141F62131C0F601010165F8F734F99B +:10D1900080B141F61931C0F601010165F8F734F9A3 :10D1A00000283FF420ADD4F88000016DBDE8B04008 :10D1B000084700BF2046BDE8B040FFF7B1BC00BF44 :10D1C000D0017FF510ADB5F1FF3FC8BFB0BD20461F :10D1D000BDE8B040FFF708BC10B5E0B10446D0F898 :10D1E000880028B9002084F88400204600F028F840 :10D1F0002420C4F888002068A16A0268002922F06F -:10D200000101016002D0204604F05EFB204604F0DC -:10D210006BFE012802D1012010BD00BF20684168CB +:10D200000101016002D0204604F05AFB204604F0E0 +:10D2100067FE012802D1012010BD00BF20684168CF :10D2200021F490414160816821F02A018160016808 -:10D2300041F0010101602046BDE8104004F0B4BB9C +:10D2300041F0010101602046BDE8104004F0B0BBA0 :10D240002DE9F043B5B000210446CDE93311CDE915 :10D250003111309102A8B821F3F7E5F947F2FF71D7 :10D260002068C4F20001884200F3808044F200414B @@ -3434,8 +3434,8 @@ :10D68000216840F0806041E800034BB1206850E819 :10D69000000F216840F0806041E80003002BD7D1E3 :10D6A000A2652220A4F85C50C4F890C0C4F88C0095 -:10D6B000D4F88000F0B141F6495141F6C963C0F693 -:10D6C0000101C0F60103C0E90F1341F63131002317 +:10D6B000D4F88000F0B141F6415141F6C163C0F6A3 +:10D6C0000101C0F60103C0E90F1341F6293100231F :10D6D000C0F60101C1642168036524312B46F8F7C7 :10D6E00057FF38B11020C4F890002020C4F88C00F7 :10D6F0000120B0BD206938B3206850E8000F2168D0 @@ -4393,16 +4393,16 @@ :1012600005D040F20042C4F20102904201D1096966 :101270000163016841F004010160012141612EF028 :1012800080011943016010BD10B502F0B1FA41F2BE -:10129000A17048F68C42C0F60100C0F601020021A0 +:10129000997048F68C42C0F60100C0F601020021A8 :1012A00002F08EFD42F2FC4448F69032C2F2004455 :1012B000C0F601020021206041F29130C0F6010029 -:1012C00002F07EFD606041F2915048F6D832C0F6DF +:1012C00002F07EFD606041F2895048F6D832C0F6E7 :1012D0000100C0F60102002102F072FDA06041F29F -:1012E000796048F6FC32C0F60100C0F60102002128 -:1012F00002F066FDE06041F2617048F66842C0F6B7 +:1012E000716048F6FC32C0F60100C0F60102002130 +:1012F00002F066FDE06041F2597048F66842C0F6BF :101300000100C0F60102002102F05AFD206141F205 -:10131000D94048F6B432C0F60100C0F601020021FF -:1013200002F04EFD606141F2D16048F62042C0F605 +:10131000D14048F6B432C0F60100C0F60102002107 +:1013200002F04EFD606141F2C96048F62042C0F60D :101330000100C0F60102002102F042FDA06102207E :101340000A21002202F0F8FAE061022003210022C3 :1013500002F0F2FA606302202421002202F0ECFA8B @@ -4416,532 +4416,532 @@ :1013D000001A48F6045204F13000C0F60102002160 :1013E000019081EE000AFEF783FE0320F1F7C0F9B9 :1013F00042F2FC4804F1100904F11C0A07F10C0B3D -:101400009FED32AAC2F200489FED318A9FED319ADA +:101400009FED2FAAC2F200489FED2E8A9FED2E9AE3 :10141000F0F72EF9EFF788FEEFF7A2FEEFF7B8FE30 :10142000EFF7D2FE02F0E4F92046F0F779F8204613 :10143000F0F7BEF83046494652463B46EFF7FCFA15 :1014400058463146EFF7E4F93D4694ED040A6069E9 :10145000B1EE400A45F82C0FA06985ED010A94ED24 -:10146000070AB1EE400A15ED061A85ED040A97ED5C -:10147000030AD4E90812C5E9020155F81C0C2862D8 -:1014800030EE0A0AD8F82000B1EE411A6A6185ED03 -:10149000060A85ED071A02F0EFFAD8F820000022BC -:1014A0000023294602F094FAB0EE480AB0EE491A39 -:1014B000F0EE491AD4ED0A0A0198FEF769FD0320FF -:1014C000F1F7CEF802F0CCF9A2E700BFDB0FC9BFFD -:1014D000000022420000000002F06EF900EE100A47 -:1014E00000209FED271BB8EE400B80EE010BBCEEF9 -:1014F000C00B10EE106A02F017F902F055F940F235 -:10150000E0470546C2F20047042107F108042046DF -:10151000F1F71AFA00212046F1F72EFAB6EE008A0A -:1015200044F6D3583544C1F262084FF47A7940F258 -:10153000E93A9FED159A06E02046F1F731FB28467F -:1015400002F002F935443888411C8AB2A2FB080334 -:10155000524508BF0121980900FB19203980322823 -:10156000EAD00028EBD1B0EE490AF0EE480A204656 -:10157000F1F7CAFA2046F1F7F7FAE0E700BF00BF3B -:101580000000000000407F40A4D082430000000023 -:1015900002F012F900EE100A00209FED331BB8EEA6 -:1015A000400B80EE010BBCEEC00B10EE109A02F067 -:1015B000BBF802F0F9F80546F2F7B2FA40F2F86427 -:1015C00000F1B401C2F200449FED290A2046F1F770 -:1015D000CFFC40F2005642F2FC47C2F200464D44B6 -:1015E000C2F2004706F1140804F1780A11E000BFC6 -:1015F000B86A00220023514602F056F92046F2F75D -:1016000097F920463146F1F7EBFA284602F09CF8AC -:101610004D44786A00220023314602F045F9386AC9 -:1016200000220023414602F03FF90028E0D1706912 -:10163000C4F88800B069C4F88C00F069C4F8900060 -:10164000306AC4F89400706AC4F89800B06AC4F8AC -:101650009C00F06AC4F8A000306BC4F8A400706B62 -:10166000C4F8A800C4E700BF0000000000407F40AD -:101670000000FA430000000002F09EF800EE100A9D -:1016800000209FED111BB8EE400B80EE010BBCEE6D -:10169000C00B10EE106A02F047F802F085F805461C -:1016A000F0F74EF840F22004C2F200442046FCF766 -:1016B0001BFC35442046FCF7CBFB284602F044F8DF -:1016C0003544F7E700BF00BF0000000000407F4046 -:1016D00002F072F800EE100A00209FED1D1BB8EE1C -:1016E000400B80EE010BBCEEC00B10EE107A02F046 -:1016F0001BF802F059F80546F2F712FA42F22024DC -:101700000146C2F200449FED140A2046FFF758FA42 -:1017100042F2FC4842F2F8463D44C2F20048C2F2AE -:10172000004600BFD8F8340000220023314602F002 -:10173000BBF82046FFF724FB20463146FEF77CFF2E -:10174000284602F001F83D44ECE700BF00BF00BFAF -:101750000000000000407F400000FA43000000004D -:1017600002F02AF800EE100A00209FED0B1BB8EEE5 -:10177000400B80EE010BBCEEC00B10EE105A01F0D6 -:10178000D3FF02F011F84419204601F0DDFF2C448C -:10179000FAE700BF00BF00BF0000000000407F402C -:1017A00002F00AF800EE100A00209FED3D1BB8EE93 -:1017B000400B80EE010BBCEEC00B10EE108A01F066 -:1017C000B3FF01F0F1FF40F238540646C2F2004484 -:1017D00004F1180A5046F2F7EFFB42F2FC4546448A -:1017E0004FF48139C2F2004510E000BFA070686B71 -:1017F00084F801B002F040F9686B00220023214612 -:1018000002F0E6F8304601F09FFF46445046F2F7FA -:101810000BFD1420F2F718FD20B15046F2F700FC42 -:1018200003E000BF5046F2F7EBFB94F85100274667 -:101830004FF0000B0138C0B2C10003284FF0000088 -:1018400038BF29FA01F007F8040FD4E90F01A160AD -:10185000616CE060E06CC4E90410686A02F00CF9A5 -:10186000686A00220023394602F0B2F894F850006A -:10187000227E411E2270C8B202284FF00000B5D867 -:101880004FF4FF6000EAC10040F20011C0F2010114 -:1018900021FA00FB4FF4807121FA00F0A6E700BFA7 -:1018A0000000000000407F4041F69030C2F200404E -:1018B000FBF74AB90000000041F62440C2F20040A4 -:1018C000FBF742B900000000816A0A070CD4CA077E -:1018D00015D18A071ED44A0727D4CA0630D48A06EF -:1018E00039D44A0655D541E00268D0F838C053686B -:1018F00023F4004343EA0C035360CA07E9D00268AB -:10190000D0F82CC0536823F4003343EA0C0353602F -:101910008A07E0D50268D0F830C0536823F48033DA -:1019200043EA0C0353604A07D7D50268D0F834C0A5 -:10193000536823F4802343EA0C035360CA06CED5D0 -:101940000268D0F83CC0936823F4805343EA0C0348 -:1019500093608A06C5D50268D0F840C0936823F426 -:10196000005343EA0C0393604A0612D50268D0F88C -:1019700044C0BCF5801F536823F4801343EA0C0372 -:10198000536006D1536823F4C00C836C43EA0C0304 -:101990005360090658BF70470168C06C4A6822F45A -:1019A0000022104348607047B0B582B00446002062 -:1019B000C4F89000F6F774F9216805460A68120722 -:1019C00037D54FF4001100226FF07E400090204682 -:1019D0002B4600F0A9FD58B3206850E8000F21689D -:1019E00020F0800041E80002002A1FBF206850E874 -:1019F000000F216820F080001CBF41E80002002A8F -:101A000012D0206850E8000F216820F0800041E8E3 -:101A100000024AB1206850E8000F216820F08000E1 -:101A200041E80002002AD7D12020C4F8880063E0F2 -:101A300021680868400765D54FF4800100226FF0E7 -:101A40007E40009020462B4600F06EFD002859D0C5 -:101A5000206850E8000F216820F4907041E80002EF -:101A6000002A1FBF206850E8000F216820F4907002 -:101A70001CBF41E80002002A26D0206850E8000F71 -:101A8000216820F4907041E80002EAB1206850E833 -:101A9000000F216820F4907041E80002002AD7D19D -:101AA00012E000BF206850E8020F216820F001001A -:101AB00041E80202EAB1206850E8020F216820F0F4 -:101AC000010041E80202A2B1206850E8020F21683B -:101AD00020F0010041E80202002A1FBF206850E800 -:101AE000020F216820F001001CBF41E80202002A19 -:101AF000D8D12020C4F88C000320002184F8841061 -:101B000002B0B0BD2020C4F88800C4F88C000020CA -:101B1000E0662067002184F8841002B0B0BD0000A8 -:101B2000806B0021A0F85E10FAF7FABF00000000F9 -:101B3000806B0268D0F88810D0F88CC02129936897 -:101B400063D113F0800360D00023A0F8563052E830 -:101B5000002F036822F0C00243E80021E1B300BF78 -:101B6000016851E8001F026821F0C00142E800133B -:101B7000002B1FBF016851E8001F026821F0C0015F -:101B80001CBF42E80013002B26D0016851E8001F5B -:101B9000026821F0C00142E80013EBB1016851E88E -:101BA000001F026821F0C00142E80013002BD7D1CA -:101BB00012E000BF016851E8021F026821F4000131 -:101BC00042E80213EBB1016851E8021F026821F4F8 -:101BD000000142E80213A3B1016851E8021F026844 -:101BE00021F4000142E80213002B1FBF016851E8F5 -:101BF000021F026821F400011CBF42E80213002BFF -:101C0000D8D12021C0F888100268BCF1220F916859 -:101C100040F0908011F0400100F08C800021A0F88D -:101C20005E1052E8001F00BF026821F4907142E884 -:101C300000130BB3016851E8001F026821F4907192 -:101C400042E80013002B1FBF016851E8001F026823 -:101C500021F490711CBF42E80013002B0CD00168E6 -:101C600051E8001F026821F4907142E800131BB193 -:101C7000016851E8001FD7E74FF6FE7CCEF6FF7CE7 -:101C8000026852E8022F036802EA0C0243E80221CC -:101C900000291FBF016851E8021F026801EA0C0118 -:101CA0001CBF42E80213002B12D0016851E8021F4A -:101CB000026801EA0C0142E802134BB1016851E8E5 -:101CC000021F026801EA0C0142E80213002BD7D17F -:101CD000C16E012928D100BF016851E8001F0268C8 -:101CE00021F0100142E80013002B1FBF016851E8EA -:101CF000001F026821F010011CBF42E80013002BF6 -:101D000012D0016851E8001F026821F0100142E87A -:101D100000134BB1016851E8001F026821F0100167 -:101D200042E80013002BD7D12021C0F88C100021ED -:101D3000C1664167D0F8901041F01001C0F89010D2 -:101D4000FAF7EEBE00000000C169806BB1F5807F3C -:101D500000F0AC800021A0F85E1000BF016851E8DF -:101D6000001F026821F4807142E80013002B1FBF9E -:101D7000016851E8001F026821F480711CBF42E82D -:101D80000013002B12D0016851E8001F026821F4F3 -:101D9000807142E800134BB1016851E8001F0268EE -:101DA00021F4807142E80013002BD7D1016851E87B -:101DB000021F026821F0010142E80213002B1FBF3D -:101DC000016851E8021F026821F001011CBF42E8CE -:101DD0000213002B26D0016851E8021F026821F08F -:101DE000010142E80213EBB1016851E8021F0268E9 -:101DF00021F0010142E80213002BD7D112E000BF0D -:101E0000016851E8021F026821F0400142E8021314 -:101E1000EBB1016851E8021F026821F0400142E87D -:101E20000213A3B1016851E8021F026821F04001CA -:101E300042E80213002B1FBF016851E8021F02682D -:101E400021F040011CBF42E80213002BD8D1202111 -:101E5000C0F88C10C16E012928D100BF016851E87B -:101E6000001F026821F0100142E80013002B1FBF81 -:101E7000016851E8001F026821F010011CBF42E810 -:101E80000013002B12D0016851E8001F026821F0F6 -:101E9000100142E800134BB1016851E8001F0268CD -:101EA00021F0100142E80013002BD7D10021016777 -:101EB000C16E012904D1B0F85C10FAF791BD00BFE2 -:101EC000FBF796BC00000000806B01210167C16E2A -:101ED000012905D1B0F85C104908FAF781BD00BFAF -:101EE000FBF79ABC0000000070B584B0044600689F -:101EF00004F1080340F6004EE669C5F6000E0ECB6D -:101F0000194363690B4346F6F3110568CCF6FF717C -:101F1000334329401943016041684FF2FF4321F4E4 -:101F20004051704541EA02014160A269C1F2FF13CC -:101F30001CBF216A0A43816844F2544C1940C5F61B -:101F4000020C1143816047F6FF32C16AC4F20002FD -:101F500021F00F01656A90424FF0800341EA0501CC -:101F6000C1622BDD41F2FF71C4F20101884251DCF4 -:101F700047F60041C4F20001884200F08B8041F234 -:101F80000001C4F20101884200F0948041F2004156 -:101F9000C4F20101884240F00E81DCF80010C1F368 -:101FA000C202052A00F2EE800123DFE812F00401EC -:101FB000E300F300FB000301AB0100BF44F6FF3177 -:101FC000C4F20001884247DD44F60041C4F200013A -:101FD000884200F07F8045F20001C4F2000188428F -:101FE00000F0888047F60001C4F20001884240F00A -:101FF000E280DCF8001001F00702052A00F2C2803E -:102000000023DFE812F0D800B700C700CF00D700E8 -:102010007F0100BF41F60001C4F20101884200F0D7 -:10202000798041F60041C4F20101884200F08280CB -:10203000704540F0C080DCF8041001F00702052A6A -:1020400000F2A0800223DFE812F0B6009500A500A0 -:10205000AD00B5005D0100BF44F20041C4F20001D3 -:10206000884200F0778044F60001C4F20001884203 -:1020700040F0A180DCF8001001F00702052A00F210 -:1020800081800023DFE812F09700760086008E0042 -:1020900096003E01DCF8001001F00702052A00F26C -:1020A00071800023DFE812F08700660076007E0072 -:1020B00086002E01DCF80010C1F3C202052A00F2EE -:1020C00061800123DFE812F07700560066006E00A1 -:1020D00076001E01DCF8001001F00702052A00F26C -:1020E00051800023DFE812F06700460056005E00D2 -:1020F00066000E01DCF8001001F00702052A41D845 -:102100000023DFE812F05800370047004F00570067 -:10211000FF0000BFDCF80010C1F3C202052A31D86D -:102120000123DFE812F04800270037003F00470096 -:10213000EF0000BFDCF80010C1F3C202052A21D86D -:102140000123DFE812F03800170027002F003700C6 -:10215000DF0000BFDCF8001001F00702052A11D8EB -:102160000023DFE812F02800070017001F002700F7 -:10217000CF0000BF0423704540F60010C0F23D00C0 -:1021800020D1C7E08023704540F60010C0F23D002A -:1021900018D1BFE00823704540F60010C0F23D00A2 -:1021A00010D1B7E01023704540F60010C0F23D009A -:1021B00008D1AFE02023704540F60010C0F23D008A -:1021C00000F0A880B6F5004F40F04E80402B4FF055 -:1021D000010100F29E81DFE813F0410017019C012C -:1021E0009C011D019C019C019C0125019C019C01FD -:1021F0009C019C019C019C019C012D019C019C0166 +:10146000070AB1EE400A85ED040A97ED030AD4E9B4 +:101470000812C5E9020155E90701286230EE0A0A9F +:10148000D8F820006A61E96185ED060A02F0F4FAF5 +:10149000D8F8200000220023294602F099FAB0EE85 +:1014A000480AB0EE491AF0EE491AD4ED0A0A01983A +:1014B000FEF76EFD0320F1F7D3F802F0D1F9A7E7AC +:1014C000DB0FC9BF00002242000000000000000046 +:1014D00002F072F900EE100A00209FED271BB8EE13 +:1014E000400B80EE010BBCEEC00B10EE106A02F058 +:1014F0001BF902F059F940F2E0470546C2F20047F5 +:10150000042107F108042046F1F71EFA00212046C5 +:10151000F1F732FAB6EE008A44F6D3583544C1F2F8 +:1015200062084FF47A7940F2E93A9FED159A06E0A5 +:101530002046F1F735FB284602F006F93544388895 +:10154000411C8AB2A2FB0803524508BF0121980939 +:1015500000FB192039803228EAD00028EBD1B0EE08 +:10156000490AF0EE480A2046F1F7CEFA2046F1F794 +:10157000FBFAE0E700BF00BF0000000000407F4032 +:10158000A4D082430000000002F016F900EE100A19 +:1015900000209FED331BB8EE400B80EE010BBCEE3C +:1015A000C00B10EE109A02F0BFF802F0FDF80546ED +:1015B000F2F7B6FA40F2F86400F1B401C2F2004466 +:1015C0009FED290A2046F1F7D3FC40F2005642F283 +:1015D000FC47C2F200464D44C2F2004706F114082F +:1015E00004F1780A11E000BFB86A002200235146D6 +:1015F00002F05AF92046F2F79BF920463146F1F7FE +:10160000EFFA284602F0A0F84D44786A0022002341 +:10161000314602F049F9386A00220023414602F0BF +:1016200043F90028E0D17069C4F88800B069C4F8B3 +:101630008C00F069C4F89000306AC4F89400706AB5 +:10164000C4F89800B06AC4F89C00F06AC4F8A0001E +:10165000306BC4F8A400706BC4F8A800C4E700BFE6 +:101660000000000000407F400000FA43000000003E +:1016700002F0A2F800EE100A00209FED111BB8EE58 +:10168000400B80EE010BBCEEC00B10EE106A02F0B6 +:101690004BF802F089F80546F0F752F840F22004C2 +:1016A000C2F200442046FCF71FFC35442046FCF7FC +:1016B000CFFB284602F048F83544F7E700BF00BFEB +:1016C0000000000000407F4002F076F800EE100AB3 +:1016D00000209FED1D1BB8EE400B80EE010BBCEE11 +:1016E000C00B10EE107A02F01FF802F05DF805460C +:1016F000F2F716FA42F220240146C2F200449FEDAE +:10170000140A2046FFF75CFA42F2FC4842F2F8461F +:101710003D44C2F20048C2F2004600BFD8F834008F +:1017200000220023314602F0BFF82046FFF728FBD5 +:1017300020463146FEF780FF284602F005F83D447A +:10174000ECE700BF00BF00BF0000000000407F408A +:101750000000FA430000000002F02EF800EE100A2C +:1017600000209FED0B1BB8EE400B80EE010BBCEE92 +:10177000C00B10EE105A01F0D7FF02F015F8441913 +:10178000204601F0E1FF2C44FAE700BF00BF00BF94 +:101790000000000000407F4002F00EF800EE100A4A +:1017A00000209FED3D1BB8EE400B80EE010BBCEE20 +:1017B000C00B10EE108A01F0B7FF01F0F5FF40F208 +:1017C00038540646C2F2004404F1180A5046F2F7B3 +:1017D000F3FB42F2FC4546444FF48139C2F2004526 +:1017E00010E000BFA070686B84F801B002F044F90B +:1017F000686B00220023214602F0EAF8304601F02F +:10180000A3FF46445046F2F70FFD1420F2F71CFDEB +:1018100020B15046F2F704FC03E000BF5046F2F757 +:10182000EFFB94F8510027464FF0000B0138C0B28F +:10183000C10003284FF0000038BF29FA01F007F873 +:10184000040FD4E90F01A160616CE060E06CC4E9B1 +:101850000410686A02F010F9686A00220023394611 +:1018600002F0B6F894F85000227E411E2270C8B2F1 +:1018700002284FF00000B5D84FF4FF6000EAC10025 +:1018800040F20011C0F2010121FA00FB4FF4807117 +:1018900021FA00F0A6E700BF0000000000407F40F2 +:1018A00041F69030C2F20040FBF74EB90000000054 +:1018B00041F62440C2F20040FBF746B900000000A8 +:1018C000816A0A070CD4CA0715D18A071ED44A07B1 +:1018D00027D4CA0630D48A0639D44A0655D541E001 +:1018E0000268D0F838C0536823F4004343EA0C037D +:1018F0005360CA07E9D00268D0F82CC0536823F4BB +:10190000003343EA0C0353608A07E0D50268D0F83D +:1019100030C0536823F4803343EA0C0353604A0712 +:10192000D7D50268D0F834C0536823F4802343EA43 +:101930000C035360CA06CED50268D0F83CC0936849 +:1019400023F4805343EA0C0393608A06C5D50268EA +:10195000D0F840C0936823F4005343EA0C0393602B +:101960004A0612D50268D0F844C0BCF5801F5368FF +:1019700023F4801343EA0C03536006D1536823F425 +:10198000C00C836C43EA0C035360090658BF7047D0 +:101990000168C06C4A6822F4002210434860704716 +:1019A000B0B582B004460020C4F89000F6F778F98C +:1019B000216805460A68120737D54FF40011002246 +:1019C0006FF07E40009020462B4600F0A9FD58B3F2 +:1019D000206850E8000F216820F0800041E80002F4 +:1019E000002A1FBF206850E8000F216820F0800007 +:1019F0001CBF41E80002002A12D0206850E8000F06 +:101A0000216820F0800041E800024AB1206850E8D7 +:101A1000000F216820F0800041E80002002AD7D1A1 +:101A20002020C4F8880063E021680868400765D575 +:101A30004FF4800100226FF07E40009020462B463C +:101A400000F06EFD002859D0206850E8000F216892 +:101A500020F4907041E80002002A1FBF206850E87F +:101A6000000F216820F490701CBF41E80002002A9A +:101A700026D0206850E8000F216820F4907041E8DB +:101A80000002EAB1206850E8000F216820F490704D +:101A900041E80002002AD7D112E000BF206850E8D8 +:101AA000020F216820F0010041E80202EAB120683B +:101AB00050E8020F216820F0010041E80202A2B1C3 +:101AC000206850E8020F216820F0010041E802027E +:101AD000002A1FBF206850E8020F216820F0010093 +:101AE0001CBF41E80202002AD8D12020C4F88C0093 +:101AF0000320002184F8841002B0B0BD2020C4F877 +:101B00008800C4F88C000020E0662067002184F87B +:101B1000841002B0B0BD0000806B0021A0F85E1000 +:101B2000FAF7FEBF00000000806B0268D0F8881052 +:101B3000D0F88CC02129936863D113F0800360D062 +:101B40000023A0F8563052E8002F036822F0C002AC +:101B500043E80021E1B300BF016851E8001F0268BB +:101B600021F0C00142E80013002B1FBF016851E8BB +:101B7000001F026821F0C0011CBF42E80013002BC7 +:101B800026D0016851E8001F026821F0C00142E838 +:101B90000013EBB1016851E8001F026821F0C00199 +:101BA00042E80013002BD7D112E000BF016851E8D2 +:101BB000021F026821F4000142E80213EBB1016840 +:101BC00051E8021F026821F4000142E80213A3B1A8 +:101BD000016851E8021F026821F4000142E8021383 +:101BE000002B1FBF016851E8021F026821F40001A9 +:101BF0001CBF42E80213002BD8D12021C0F8881066 +:101C00000268BCF1220F916840F0908011F0400111 +:101C100000F08C800021A0F85E1052E8001F00BF89 +:101C2000026821F4907142E800130BB3016851E897 +:101C3000001F026821F4907142E80013002B1FBFBF +:101C4000016851E8001F026821F490711CBF42E84E +:101C50000013002B0CD0016851E8001F026821F42A +:101C6000907142E800131BB1016851E8001FD7E7EB +:101C70004FF6FE7CCEF6FF7C026852E8022F036826 +:101C800002EA0C0243E8022100291FBF016851E863 +:101C9000021F026801EA0C011CBF42E80213002B7C +:101CA00012D0016851E8021F026801EA0C0142E803 +:101CB00002134BB1016851E8021F026801EA0C01EE +:101CC00042E80213002BD7D1C16E012928D100BFF1 +:101CD000016851E8001F026821F0100142E800137A +:101CE000002B1FBF016851E8001F026821F010019E +:101CF0001CBF42E80013002B12D0016851E8001FFE +:101D0000026821F0100142E800134BB1016851E86C +:101D1000001F026821F0100142E80013002BD7D108 +:101D20002021C0F88C100021C1664167D0F89010C6 +:101D300041F01001C0F89010FAF7F2BE0000000068 +:101D4000C169806BB1F5807F00F0AC800021A0F804 +:101D50005E1000BF016851E8001F026821F4807125 +:101D600042E80013002B1FBF016851E8001F026802 +:101D700021F480711CBF42E80013002B12D00168CF +:101D800051E8001F026821F4807142E800134BB152 +:101D9000016851E8001F026821F4807142E80013D5 +:101DA000002BD7D1016851E8021F026821F0010120 +:101DB00042E80213002B1FBF016851E8021F0268AE +:101DC00021F001011CBF42E80213002B26D001685C +:101DD00051E8021F026821F0010142E80213EBB151 +:101DE000016851E8021F026821F0010142E8021374 +:101DF000002BD7D112E000BF016851E8021F026832 +:101E000021F0400142E80213EBB1016851E8021FE2 +:101E1000026821F0400142E80213A3B1016851E8D1 +:101E2000021F026821F0400142E80213002B1FBF8D +:101E3000016851E8021F026821F040011CBF42E81E +:101E40000213002BD8D12021C0F88C10C16E0129BB +:101E500028D100BF016851E8001F026821F010017D +:101E600042E80013002B1FBF016851E8001F026801 +:101E700021F010011CBF42E80013002B12D00168B2 +:101E800051E8001F026821F0100142E800134BB135 +:101E9000016851E8001F026821F0100142E80013B8 +:101EA000002BD7D100210167C16E012904D1B0F800 +:101EB0005C10FAF795BD00BFFBF79ABC000000006C +:101EC000806B01210167C16E012905D1B0F85C105A +:101ED0004908FAF785BD00BFFBF79EBC0000000073 +:101EE00070B584B00446006804F1080340F6004E63 +:101EF000E669C5F6000E0ECB194363690B4346F63F +:101F0000F3110568CCF6FF71334329401943016092 +:101F100041684FF2FF4321F44051704541EA02010C +:101F20004160A269C1F2FF131CBF216A0A438168A4 +:101F300044F2544C1940C5F6020C1143816047F637 +:101F4000FF32C16AC4F2000221F00F01656A9042BB +:101F50004FF0800341EA0501C1622BDD41F2FF71C0 +:101F6000C4F20101884251DC47F60041C4F200018D +:101F7000884200F08B8041F20001C4F201018842E6 +:101F800000F0948041F20041C4F20101884240F027 +:101F90000E81DCF80010C1F3C202052A00F2EE80C7 +:101FA0000123DFE812F00401E300F300FB0003016A +:101FB000AB0100BF44F6FF31C4F20001884247DDA7 +:101FC00044F60041C4F20001884200F07F8045F2EF +:101FD0000001C4F20001884200F0888047F6000149 +:101FE000C4F20001884240F0E280DCF8001001F009 +:101FF0000702052A00F2C2800023DFE812F0D800B1 +:10200000B700C700CF00D7007F0100BF41F6000135 +:10201000C4F20101884200F0798041F60041C4F227 +:102020000101884200F08280704540F0C080DCF8F9 +:10203000041001F00702052A00F2A0800223DFE865 +:1020400012F0B6009500A500AD00B5005D0100BF1F +:1020500044F20041C4F20001884200F0778044F667 +:102060000001C4F20001884240F0A180DCF80010B9 +:1020700001F00702052A00F281800023DFE812F058 +:102080009700760086008E0096003E01DCF8001076 +:1020900001F00702052A00F271800023DFE812F048 +:1020A0008700660076007E0086002E01DCF80010B6 +:1020B000C1F3C202052A00F261800123DFE812F0B9 +:1020C0007700560066006E0076001E01DCF80010F6 +:1020D00001F00702052A00F251800023DFE812F028 +:1020E0006700460056005E0066000E01DCF8001036 +:1020F00001F00702052A41D80023DFE812F058005A +:10210000370047004F005700FF0000BFDCF8001009 +:10211000C1F3C202052A31D80123DFE812F04800DA +:10212000270037003F004700EF0000BFDCF8001039 +:10213000C1F3C202052A21D80123DFE812F03800DA +:10214000170027002F003700DF0000BFDCF8001069 +:1021500001F00702052A11D80023DFE812F0280059 +:10216000070017001F002700CF0000BF04237045A1 +:1021700040F60010C0F23D0020D1C7E0802370453A +:1021800040F60010C0F23D0018D1BFE008237045B2 +:1021900040F60010C0F23D0010D1B7E010237045AA +:1021A00040F60010C0F23D0008D1AFE0202370459A +:1021B00040F60010C0F23D0000F0A880B6F5004FD8 +:1021C00040F04E80402B4FF0010100F29E81DFE88D +:1021D00013F0410017019C019C011D019C019C0111 +:1021E0009C0125019C019C019C019C019C019C017E +:1021F0009C012D019C019C019C019C019C019C0166 :102200009C019C019C019C019C019C019C019C01E6 -:102210009C019C019C019C019C013D019C019C0135 +:102210009C013D019C019C019C019C019C019C0135 :102220009C019C019C019C019C019C019C019C01C6 :102230009C019C019C019C019C019C019C019C01B6 :102240009C019C019C019C019C019C019C019C01A6 -:102250009C019C019C019C019C013B01F7F784FFC0 -:10226000002840F0D480E3E0402B4FF0010100F261 -:102270005081DFE813F041000F014E014E011501BE -:102280004E014E014E011D014E014E014E014E0107 -:102290004E014E014E0125014E014E014E014E01EF -:1022A0004E014E014E014E014E014E014E014E01B6 -:1022B0004E014E014E0135014E014E014E014E01BF +:102250009C013B01F7F788FF002840F0D480E3E0C1 +:10226000402B4FF0010100F25081DFE813F04100F4 +:102270000F014E014E0115014E014E014E011D018F +:102280004E014E014E014E014E014E014E012501FF +:102290004E014E014E014E014E014E014E014E01C6 +:1022A0004E014E014E014E014E014E014E013501CF +:1022B0004E014E014E014E014E014E014E014E01A6 :1022C0004E014E014E014E014E014E014E014E0196 :1022D0004E014E014E014E014E014E014E014E0186 -:1022E0004E014E014E014E014E014E014E014E0176 -:1022F0004E014E014E013301F7F736FF002840F042 -:10230000CC80DBE04023704540F60010C0F23D0079 -:102310007FF458AF9A1E01211E2A27D8DFE812F059 -:102320002000F9002C00F900F900F9003400F90050 -:10233000F900F900F900F900F900F9003C00F90092 +:1022E0004E014E014E014E014E014E014E01330191 +:1022F000F7F73AFF002840F0CC80DBE04023704540 +:1023000040F60010C0F23D007FF458AF9A1E012144 +:102310001E2A27D8DFE812F02000F9002C00F9006F +:10232000F900F9003400F900F900F900F900F900AA +:10233000F900F9003C00F900F900F900F900F90092 :10234000F900F900F900F900F900F900F900F900C5 -:10235000F900F900F900F900F900F900490000BF9F -:10236000F6F7FEFBB0B1656A23E000BF402B40F0FA -:10237000D0804FF400401CE001A8F6F79DFC0298C5 -:102380000028F0D106E000BF01A8F6F72DFD029865 -:102390000028E8D10021BCE05CF8540C800649F22A -:1023A0000000C0F2D03004D55CF8541CC1F3C10168 -:1023B000C84048F678316268C0F6010131F815105E -:1023C000B0FBF1F002EB420188424FF00101C0F096 -:1023D000A0801303984200F29C80010E00230002AB -:1023E00010EB520041F10001EEF704F84FF6FF4206 -:1023F000A0F54071C0F20F02914203D82168C86075 -:10240000002186E0012184E0F7F7DEFE80B1656AF5 -:1024100020E000BF01A8F6F74FFC02980028F6D193 -:1024200006E000BF01A8F6F7DFFC02980028EED115 -:1024300000216EE05CF8540C800649F20000C0F206 -:10244000D03007D55CF8541CC1F3C101C84001E08D -:102450004FF4004048F67831C0F6010131F815100C -:10246000B0FBF1F061684A0802EB40004FF6EF72F2 -:10247000B0FBF1F0A0F11001914209D84FF6F071D4 -:1024800022680140C0F342000844D060002140E0CF -:1024900001213EE0F7F798FE80B1656A20E000BFB9 -:1024A00001A8F6F709FC02980028F6D106E000BF63 -:1024B00001A8F6F799FC02980028EED1002128E047 -:1024C0005CF8540C800649F20000C0F2D03007D509 -:1024D0005CF8541CC1F3C101C84001E04FF4004056 -:1024E00048F678316268C0F6010131F81510B0FB8A -:1024F000F1F000EB5200B0FBF2F0A0F110014FF64A -:10250000EF72914204D82168C860002101E000BF49 -:1025100001214FF00110A0660020C4E91D0008460B -:1025200004B070BD000000002DE9F0418046006855 -:102530000D46C7693940491BB1FA81F149099142F9 -:1025400025D1069C34B31646611C29D1C1692940A6 -:10255000491BB1FA81F14909B14218D1C169294039 -:10256000491BB1FA81F14909B14201BFC169294052 -:10257000491BB1FA81F104BF4909B14207D1C169D0 -:102580002940491BB1FA81F14909B142DED000204E -:10259000BDE8F081F5F784FB0320BDE8F08100BFC2 -:1025A0001F4607E0C0692840401BB0FA80F0400990 -:1025B000B042ECD1F5F774FBC01BA04244D8D8F868 -:1025C000000001684907EDD5402D18BF802DE9D0E6 -:1025D000C16909073CD4C1690905E3D54FF400611D -:1025E000016250E8000FD8F8001020F4907041E824 -:1025F0000002002A62D0D8F8000050E8000FD8F896 -:10260000001020F4907041E80002002A1FBFD8F8A3 -:10261000000050E8000FD8F8001020F490701CBFA4 -:1026200041E80002002A49D0D8F8000050E8000F25 -:10263000D8F8001020F4907041E80002002A3DD044 -:10264000D8F80000CDE700BF0320BDE8F08100BF4F -:102650000821016250E8000FD8F8001020F49070B3 -:1026600041E80002002A00F09D80D8F8000050E800 -:10267000000FD8F8001020F4907041E80002002A02 -:102680001FBFD8F8000050E8000FD8F8001020F461 -:1026900090701CBF41E80002002A00F08380D8F847 -:1026A000000050E8000FD8F8001020F4907041E8C6 -:1026B0000002002A76D0D8F80000CBE74FF6FE7073 -:1026C000CEF6FF70D8F8001051E8021FD8F80020AD -:1026D000014042E80213002B1FBFD8F8001051E858 -:1026E000021FD8F8002001401CBF42E80213002B53 -:1026F00014D0D8F8001051E8021FD8F8002001408B -:1027000042E8021353B1D8F8001051E8021FD8F87C -:102710000020014042E80213002BD3D1D8F86C000E -:1027200001282FD1D8F8000050E8000FD8F8001089 -:1027300020F0100041E80002002A1FBFD8F8000076 -:1027400050E8000FD8F8001020F010001CBF41E83E -:102750000002002A16D0D8F8000050E8000FD8F880 -:10276000001020F0100041E800025AB1D8F8000033 -:1027700050E8000FD8F8001020F0100041E80002E7 -:10278000002ACFD12020C8F88C000021C8F86C1096 -:10279000C8F87410C8F89000032088F88410BDE8C9 -:1027A000F08100BF4FF6FE70CEF6FF70D8F8001033 -:1027B00051E8021FD8F80020014042E80213002B24 -:1027C0001FBFD8F8001051E8021FD8F800200140C0 -:1027D0001CBF42E80213002B14D0D8F8001051E8B7 -:1027E000021FD8F80020014042E8021353B1D8F884 -:1027F000001051E8021FD8F80020014042E80213FF -:10280000002BD3D1D8F86C0001282FD1D8F80000C4 -:1028100050E8000FD8F8001020F0100041E8000246 -:10282000002A1FBFD8F8000050E8000FD8F80010A9 -:1028300020F010001CBF41E80002002A16D0D8F892 -:10284000000050E8000FD8F8001020F0100041E818 -:1028500000025AB1D8F8000050E8000FD8F8001074 -:1028600020F0100041E80002002ACFD12020C8F853 -:102870008C000020C8F86C00C8F874000821C8F863 -:10288000901088F884000120BDE8F081000000006D -:1028900041F6D410C2F20040FAF756B90000000029 -:1028A00041F64010C2F20040FAF74EB900000000B5 -:1028B00041F66820C2F20040FAF746B90000000075 -:1028C00041F6FC20C2F20040FAF73EB900000000D9 -:1028D00082B002F47F42B2F5807F40F08180836B4A -:1028E0004FF6BF7223F480338363DDF808C0C368FA -:1028F000CFF6BD721A40C2605FFA8CF2C368012A3B -:1029000023F44012C26003D1C26842F48012C26054 -:1029100000220092009A01320092009AB2F1706F88 -:1029200000F2C4800269B2F1FF3F21DD009A01325A -:102930000092009AB2F1706F00F2B8800269002A2A -:1029400016D4009A01320092009AB2F1706F00F230 -:10295000AD800269002A0BD4009A01320092009ADD -:10296000B2F1706F00F2A2800269B2F1FF3FD1DCD8 -:1029700000220092026942F001020261009A0132D3 -:102980000092009AB2F1706F00F290800269D20753 -:102990001ED0009A01320092009AB2F1706F00F2DC -:1029A00085800269D20713D0009A01320092009A02 -:1029B000B2F1706F7AD80269D20709D0009A013259 -:1029C0000092009AB2F1706F70D80269D207D5D127 -:1029D000002201F47F01B1F5803F40F08D807AE064 -:1029E000C26842F04002C26000220192019A0132A4 -:1029F0000192019AB2F1706F60D80269B2F1FF3FA3 -:102A00001EDD019A01320192019AB2F1706F55D820 -:102A10000269002A14D4019A01320192019AB2F19A -:102A2000706F4BD80269002A0AD4019A01320192D0 -:102A3000019AB2F1706F41D80269B2F1FF3FD5DC63 -:102A400000220192026942F001020261019A013200 -:102A50000192019AB2F1706F30D80269D2071DD08D -:102A6000019A01320192019AB2F1706F26D802697F -:102A7000D20713D0019A01320192019AB2F1706F1C -:102A80001CD80269D20709D0019A01320192019A39 -:102A9000B2F1706F12D80269D207D7D1002213F4B5 -:102AA0007F4F836B0FD143F480330EE0032201F498 -:102AB0007F01B1F5803F1FD10DE000BF032213F469 -:102AC0007F4F836BEFD023F48033836301F47F0166 -:102AD000B1F5803F10D1C16D89B2C165C16D41F0C2 -:102AE000007141F0F771C165816841F006018160B4 -:102AF000816841F020018160104602B070470000FB -:102B00000146D0F8000E20F00300C1F8000E0020AE -:102B1000D1F8042842F00202C1F8042870470000EE -:102B20002DE9F04F82B0C3F3074C0027BCF1000F32 -:102B3000C0F80471C0F80871C0F80C71C0F81071C9 -:102B4000C0F81471C0F81871C0F81C71C0F8207179 -:102B5000C0F82471C0F82871C0F82C71C0F8307129 -:102B6000C0F83471C0F83871C0F83C7104D000F17D -:102B700038034FF400170FE0D0F80438802743F0F3 -:102B80000203C0F80438836B23F4001383630368E3 -:102B900043F04003036003461E68374302F47F4658 -:102BA0001F60B6F5807F4FF00003C0F8003E07D1EC -:102BB000B3EB116FD0F8003806D043F0010303E007 -:102BC000D0F8003843F00303C0F800380023019325 -:102BD000019B01330193019BB3F1706F52D80369DC -:102BE000B3F1FF3F1EDD019B01330193019BB3F164 -:102BF000706F47D80369002B14D4019B01330193F4 -:102C0000019BB3F1706F3DD80369002B0AD4019B7F -:102C100001330193019BB3F1706F33D80369B3F1B2 -:102C2000FF3FD5DC002301934FF484630361019BD4 -:102C300001330193019BB3F1706F23D803699B06A5 -:102C40001DD5019B01330193019BB3F1706F19D81E -:102C500003699B0613D5019B01330193019BB3F1DB -:102C6000706F0FD803699B0609D5019B013301934F -:102C7000019BB3F1706F05D803699B06D7D44FF061 -:102C8000000E01E04FF0010E00230093009B013382 -:102C90000093009BB3F1706F50D80369B3F1FF3F0D -:102CA0001EDD009B01330093009BB3F1706F45D88C -:102CB0000369002B14D4009B01330093009BB3F1F4 -:102CC000706F3BD80369002B0AD4009B013300933B -:102CD000009BB3F1706F31D80369B3F1FF3FD5DCCE -:102CE0000023009310230361009B01330093009B9A -:102CF000B3F1706F22D80369DB0621D5009B013345 -:102D00000093009BB3F1706F18D80369DB0617D5E9 -:102D1000009B01330093009BB3F1706F0ED80369E1 -:102D2000DB060DD5009B01330093009BB3F1706F60 -:102D300004D80369DB06D7D402E000BF4FF0010ED0 -:102D40005FFA81FA4FF00009BAF1000FC0F810984D -:102D5000C0F81498C0F81C984AD0D0F800394FF049 -:102D60000068BAF1010F4FF67F3B08EA1313C0F871 -:102D70000039C0F81099C0F808B916D0CAF1000798 -:102D800000F512654FF09043002400BF2E68013714 -:102D900003EAE6762E602C617E1CC5F808B005F1CA -:102DA0002005F3D1BAF1000F22D0D0F8003BBAF1E0 -:102DB000010F08EA13134FF00008C0F8003BC0F8F9 -:102DC000108BC0F808BB13D0CAF1000400F53266BE -:102DD0004FF090434FF67F373568013403EAE575CD -:102DE0003560C6F81080651CB76006F12006F3D187 -:102DF000D0F8103811F47F0F23F48073C0F8103826 -:102E0000C0F818906FF08041416103D1816941F0B1 -:102E10001001816143F600038169C8F23C0312F49A -:102E20007F0F41EA0301816103D0816941F008010C -:102E30008161BCF1010F05D1816941F0804141F010 -:102E400004018161704602B0BDE8F08F000000000F -:102E500001468268002022F001028A60704700006B -:102E6000B0B50446C06820F0C040E060A1B10129BF -:102E70002AD1E06840F00050E060002005460A20BA -:102E8000F3F75AFDBD2D05F10A00616916D811F05E -:102E90000101F3D012E000BFE06840F08040E06044 -:102EA000002000BF05460A20F3F746FDBD2D05F1C1 -:102EB0000A00616902D811F00101F3D1C838B0FAF3 -:102EC00080F04009B0BD00BF0120B0BD000000008F -:102ED000FEE700000000000010B52DED048BB0B13E -:102EE000044690F8BC0090B194ED080A94ED091ADC -:102EF00031EE400A04F0D2FFB0EE408AB0EEC00AD4 -:102F00009FED401AB4EE410AF1EE10FA04DAFF2008 -:102F100040B2BDEC048B10BD94ED090A94ED241A67 -:102F200031EE400A94ED019A04F0B8FF29EE009AC0 -:102F300094ED070A94ED081A30EE410A04F0AEFF52 -:102F400029EE001A94ED090A94ED242A94ED019AD1 -:102F500032EE400A81EE081A84ED141A04F032F9B8 -:102F600029EE009A94ED070A94ED081A30EE410A12 -:102F700004F094FF29EE001A94ED0D0A28EE002AC1 -:102F800094ED080A94ED243A33EE400A94ED049A45 -:102F900081EE021A84ED151A04F080FF29EE009AE2 -:102FA00094ED090A94ED0A1A30EE410A04F076FF16 -:102FB00029EE001A94ED080A94ED242A32EE400A14 -:102FC00094ED049A81EE081A84ED161A04F0FAF8CA -:102FD00029EE009A94ED090A94ED0A1A30EE410A9E -:102FE00004F05CFF94ED0D1A29EE000A28EE011A98 -:102FF000002080EE010A84ED170A40B2BDEC048B7C -:1030000010BD00BFBD378635B0B52DED0C8B002847 -:103010006ED0044690F8BC00002800F09480B0EE1A -:10302000618AB0EE419AB0EE60AAB0EE40BAD4F830 -:103030009000C4F8A000606BC4F8A400A06BC4F8B2 -:10304000A800206CC4F8AC0084ED070A94ED01CA16 -:10305000C4ED0A0A04F0B6F82CEE000A94ED01CA99 -:1030600084ED1A0AB0EE4B0A04F018FF2CEE000AA9 -:1030700094ED00BA84ED1B0AB0EE4A0A94ED04CA3E -:1030800004F0A0F80CEE00BAB0EE4A0A84ED1EBAC5 -:1030900094ED04BA04F002FF2BEE000A94ED1B1A23 -:1030A00094ED1E2A84ED1F0A30EE414A94ED1A1A5F -:1030B00032EE413A24EE041A03EE031AB5EE401A3A -:1030C000F1EE10FA1ADDB1EEC10AB4EE400AF1EEEB -:1030D00010FA80F1E58194ED1A1A94ED1B2A94ED13 -:1030E0001E3A94ED1F4A33EE413A34EE424A07E06D -:1030F000FF2568B2BDEC0C8BB0BD00BF9FEDF10A9F -:1031000094ED021A94ED035A21EE012A84ED200A6F -:1031100031EE011A00EE002A21EE044A21EE033AB4 -:1031200084ED224A05EE452A84ED213A24EE041A64 -:1031300003EE031A84ED232A02EE421AB5EE401A7A -:10314000F1EE10FA04DAFF2568B2BDEC0C8BB0BDCD -:103150009FEDDCAAB5EE401AB0EE4A0AF1EE10FA85 -:103160000DDDB1EEC10AB4EE400AF1EE10FA80F1C5 -:103170009D8194ED213A94ED224A94ED232A34EE78 -:10318000000A72EE030A03F0C7FE30EE000A94ED67 -:103190001B1A94ED1F2A84ED080A31EE42BA94ED11 -:1031A00002CA04F07BFE0CEE00BA94ED1A0A94ED0C -:1031B0001E1A30EE41CA94ED080A94ED02DA04F0CA -:1031C00001F80DEE00CAB0EE4B0AF0EE4C0A03F027 -:1031D000A3FE84ED090A94ED080A94ED02BA94ED79 -:1031E0001ACA03F0EFFF0BEE00CA94ED080A94ED43 -:1031F0001BBA84ED1CCA94ED02CA04F04FFE0CEE1B -:1032000000BABEEE000A94ED001AD4ED1C0A41EE9D -:10321000000A84ED1DBA84ED0CBA2BEE0B0AC4ED46 -:103220000B0A00EEA00AB5EE400AF1EE10FA0BDD33 -:10323000B1EEC0AAB4EE4AAAF1EE10FA80F13C81D8 -:10324000D4ED0B0A94ED0CBAB0EE4B0A84ED0DAA46 -:1032500003F062FE9FED9C3AB0EE401A39EE032A6D -:1032600084ED241A33EE413A30EE420A9FED972A5C -:1032700084ED253A30EE020A84ED0F0AB4EE420ADC -:10328000F1EE10FA52DD9FED923A00BF30EE030AE4 -:10329000B4EE420AF1EE10FAC1BF30EE030AB4EE0A -:1032A000420AF1EE10FA30EE030AC4BFB4EE420A4D -:1032B000F1EE10FA06DD30EE030AB4EE420AF1EE4A -:1032C00010FAE3DC84ED0F0A9FED823AB4EE430A74 -:1032D000F1EE10FA31DBB4EE421AF1EE10FA52DDE3 -:1032E0009FED7B4A31EE041AB4EE421AF1EE10FA69 -:1032F000C1BF31EE041AB4EE421AF1EE10FA31EE0B -:10330000041AC4BFB4EE421AF1EE10FA06DD31EE33 -:10331000041AB4EE421AF1EE10FAE3DC84ED241A3A -:10332000B4EE431AF1EE10FA52DA31E09FED693A49 -:10333000B4EE430AF1EE10FACDDA9FED674A00BF12 -:1033400030EE040AB4EE430AF1EE10FABFBF30EEDD -:10335000040AB4EE430AF1EE10FA30EE040ABCBFE0 -:10336000B4EE430AF1EE10FA06DA30EE040AB4EED7 -:10337000430AF1EE10FAE3DB84ED0F0AB4EE421AD1 -:10338000F1EE10FAACDCB4EE431AF1EE10FA1FDAEB -:103390009FED512A31EE021AB4EE431AF1EE10FA03 -:1033A000BFBF31EE021AB4EE431AF1EE10FA31EE5D -:1033B000021ABCBFB4EE431AF1EE10FA06DA31EE8F -:1033C000021AB4EE431AF1EE10FAE3DB84ED241A8C -:1033D00094ED282A9FED3B3A31EE421A94ED2E2AC5 -:1033E000B5EE402A94ED0D4AF1EE10FA81EE021A84 -:1033F00094ED295A34EE454A94ED2B6AB5EE402AF5 -:1034000094F8BC0031FE031A002584ED261A84EEE0 -:10341000024AF1EE10FA31EE087AB5EE402AB1EE2A -:10342000415A84ED107A34FE034A84ED275A37EE70 -:10343000461A84ED0E4AF1EE10FA81EE021A31FEC0 -:10344000031A002884ED111A3FF453AE94ED129A3A -:1034500003F0B8FEB0EE408A94ED0F0A94ED13AA83 -:1034600004F01CFD2AEE000A94ED0D1A002080EEF7 -:10347000010AB2EE041A09EE080A30EE010AB3EEB0 -:10348000041A84ED2C0AB4EE410AF1EE10FAC8BF1A -:10349000012084F8B40068B2BDEC0C8BB0BD00BF55 -:1034A000B0EE410A04F0C2FD15E600BFB0EE410ADD -:1034B00004F0BCFD5DE600BF04F0B8FDB0EE40AA2C -:1034C000BEE600BF00000000DB0FC93FDB0F494034 -:1034D000DB0FC9C0DB0F49C0DB0FC9400000000093 +:10235000F900F900490000BFF6F702FCB0B1656A68 +:1023600023E000BF402B40F0D0804FF400401CE041 +:1023700001A8F6F7A1FC02980028F0D106E000BF02 +:1023800001A8F6F731FD02980028E8D10021BCE051 +:102390005CF8540C800649F20000C0F2D03004D53D +:1023A0005CF8541CC1F3C101C84048F6783162683A +:1023B000C0F6010131F81510B0FBF1F002EB42015B +:1023C00088424FF00101C0F0A0801303984200F250 +:1023D0009C80010E0023000210EB520041F100012D +:1023E000EEF708F84FF6FF42A0F54071C0F20F0279 +:1023F000914203D82168C860002186E0012184E071 +:10240000F7F7E2FE80B1656A20E000BF01A8F6F7A9 +:1024100053FC02980028F6D106E000BF01A8F6F7A9 +:10242000E3FC02980028EED100216EE05CF8540C29 +:10243000800649F20000C0F2D03007D55CF8541C89 +:10244000C1F3C101C84001E04FF4004048F67831C3 +:10245000C0F6010131F81510B0FBF1F061684A08CF +:1024600002EB40004FF6EF72B0FBF1F0A0F110016B +:10247000914209D84FF6F07122680140C0F3420042 +:102480000844D060002140E001213EE0F7F79CFEC7 +:1024900080B1656A20E000BF01A8F6F70DFC029844 +:1024A0000028F6D106E000BF01A8F6F79DFC0298CF +:1024B0000028EED1002128E05CF8540C800649F297 +:1024C0000000C0F2D03007D55CF8541CC1F3C10144 +:1024D000C84001E04FF4004048F678316268C0F629 +:1024E000010131F81510B0FBF1F000EB5200B0FB28 +:1024F000F2F0A0F110014FF6EF72914204D821687A +:10250000C860002101E000BF01214FF00110A0666A +:102510000020C4E91D00084604B070BD00000000A2 +:102520002DE9F041804600680D46C7693940491BD6 +:10253000B1FA81F14909914225D1069C34B316467E +:10254000611C29D1C1692940491BB1FA81F14909AE +:10255000B14218D1C1692940491BB1FA81F1490939 +:10256000B14201BFC1692940491BB1FA81F104BFE1 +:102570004909B14207D1C1692940491BB1FA81F12A +:102580004909B142DED00020BDE8F081F5F788FBB3 +:102590000320BDE8F08100BF1F4607E0C069284066 +:1025A000401BB0FA80F04009B042ECD1F5F778FB5F +:1025B000C01BA04244D8D8F8000001684907EDD5F7 +:1025C000402D18BF802DE9D0C16909073CD4C169ED +:1025D0000905E3D54FF40061016250E8000FD8F817 +:1025E000001020F4907041E80002002A62D0D8F870 +:1025F000000050E8000FD8F8001020F4907041E877 +:102600000002002A1FBFD8F8000050E8000FD8F8D9 +:10261000001020F490701CBF41E80002002A49D04D +:10262000D8F8000050E8000FD8F8001020F490709F +:1026300041E80002002A3DD0D8F80000CDE700BFF5 +:102640000320BDE8F08100BF0821016250E8000FBF +:10265000D8F8001020F4907041E80002002A00F041 +:102660009D80D8F8000050E8000FD8F8001020F442 +:10267000907041E80002002A1FBFD8F8000050E81F +:10268000000FD8F8001020F490701CBF41E8000241 +:10269000002A00F08380D8F8000050E8000FD8F836 +:1026A000001020F4907041E80002002A76D0D8F89B +:1026B0000000CBE74FF6FE70CEF6FF70D8F80010A2 +:1026C00051E8021FD8F80020014042E80213002B15 +:1026D0001FBFD8F8001051E8021FD8F800200140B1 +:1026E0001CBF42E80213002B14D0D8F8001051E8A8 +:1026F000021FD8F80020014042E8021353B1D8F875 +:10270000001051E8021FD8F80020014042E80213EF +:10271000002BD3D1D8F86C0001282FD1D8F80000B5 +:1027200050E8000FD8F8001020F0100041E8000237 +:10273000002A1FBFD8F8000050E8000FD8F800109A +:1027400020F010001CBF41E80002002A16D0D8F883 +:10275000000050E8000FD8F8001020F0100041E809 +:1027600000025AB1D8F8000050E8000FD8F8001065 +:1027700020F0100041E80002002ACFD12020C8F844 +:102780008C000021C8F86C10C8F87410C8F89000CC +:10279000032088F88410BDE8F08100BF4FF6FE707A +:1027A000CEF6FF70D8F8001051E8021FD8F80020CC +:1027B000014042E80213002B1FBFD8F8001051E877 +:1027C000021FD8F8002001401CBF42E80213002B72 +:1027D00014D0D8F8001051E8021FD8F800200140AA +:1027E00042E8021353B1D8F8001051E8021FD8F89C +:1027F0000020014042E80213002BD3D1D8F86C002E +:1028000001282FD1D8F8000050E8000FD8F80010A8 +:1028100020F0100041E80002002A1FBFD8F8000095 +:1028200050E8000FD8F8001020F010001CBF41E85D +:102830000002002A16D0D8F8000050E8000FD8F89F +:10284000001020F0100041E800025AB1D8F8000052 +:1028500050E8000FD8F8001020F0100041E8000206 +:10286000002ACFD12020C8F88C000020C8F86C00C6 +:10287000C8F874000821C8F8901088F88400012076 +:10288000BDE8F0810000000041F6D410C2F2004023 +:10289000FAF75AB90000000041F64010C2F20040B9 +:1028A000FAF752B90000000041F66820C2F2004079 +:1028B000FAF74AB90000000041F6FC20C2F20040DD +:1028C000FAF742B90000000082B002F47F42B2F58C +:1028D000807F40F08180836B4FF6BF7223F480339A +:1028E0008363DDF808C0C368CFF6BD721A40C260CA +:1028F0005FFA8CF2C368012A23F44012C26003D14C +:10290000C26842F48012C26000220092009A013232 +:102910000092009AB2F1706F00F2C4800269B2F1C5 +:10292000FF3F21DD009A01320092009AB2F1706FF0 +:1029300000F2B8800269002A16D4009A013200928F +:10294000009AB2F1706F00F2AD800269002A0BD4D8 +:10295000009A01320092009AB2F1706F00F2A280E8 +:102960000269B2F1FF3FD1DC00220092026942F01D +:1029700001020261009A01320092009AB2F1706F76 +:1029800000F290800269D2071ED0009A01320092B4 +:10299000009AB2F1706F00F285800269D20713D0FD +:1029A000009A01320092009AB2F1706F7AD80269EF +:1029B000D20709D0009A01320092009AB2F1706FEA +:1029C00070D80269D207D5D1002201F47F01B1F598 +:1029D000803F40F08D807AE0C26842F04002C260E1 +:1029E00000220192019A01320192019AB2F1706FB4 +:1029F00060D80269B2F1FF3F1EDD019A01320192F7 +:102A0000019AB2F1706F55D80269002A14D4019A64 +:102A100001320192019AB2F1706F4BD80269002A1B +:102A20000AD4019A01320192019AB2F1706F41D831 +:102A30000269B2F1FF3FD5DC00220192026942F047 +:102A400001020261019A01320192019AB2F1706FA2 +:102A500030D80269D2071DD0019A01320192019A41 +:102A6000B2F1706F26D80269D20713D0019A0132F1 +:102A70000192019AB2F1706F1CD80269D20709D095 +:102A8000019A01320192019AB2F1706F12D8026973 +:102A9000D207D7D1002213F47F4F836B0FD143F4B9 +:102AA00080330EE0032201F47F01B1F5803F1FD196 +:102AB0000DE000BF032213F47F4F836BEFD023F4AC +:102AC0008033836301F47F01B1F5803F10D1C16D84 +:102AD00089B2C165C16D41F0007141F0F771C16506 +:102AE000816841F006018160816841F020018160C8 +:102AF000104602B0704700000146D0F8000E20F0EA +:102B00000300C1F8000E0020D1F8042842F00202B0 +:102B1000C1F80428704700002DE9F04F82B0C3F3DC +:102B2000074C0027BCF1000FC0F80471C0F8087111 +:102B3000C0F80C71C0F81071C0F81471C0F81871A9 +:102B4000C0F81C71C0F82071C0F82471C0F8287159 +:102B5000C0F82C71C0F83071C0F83471C0F8387109 +:102B6000C0F83C7104D000F138034FF400170FE0B7 +:102B7000D0F80438802743F00203C0F80438836B90 +:102B800023F400138363036843F0400303600346A8 +:102B90001E68374302F47F461F60B6F5807F4FF012 +:102BA0000003C0F8003E07D1B3EB116FD0F8003836 +:102BB00006D043F0010303E0D0F8003843F00303EC +:102BC000C0F8003800230193019B01330193019B5E +:102BD000B3F1706F52D80369B3F1FF3F1EDD019B63 +:102BE00001330193019BB3F1706F47D80369002B48 +:102BF00014D4019B01330193019BB3F1706F3DD855 +:102C00000369002B0AD4019B01330193019BB3F1AB +:102C1000706F33D80369B3F1FF3FD5DC0023019314 +:102C20004FF484630361019B01330193019BB3F172 +:102C3000706F23D803699B061DD5019B0133019357 +:102C4000019BB3F1706F19D803699B0613D5019BE3 +:102C500001330193019BB3F1706F0FD803699B0699 +:102C600009D5019B01330193019BB3F1706F05D826 +:102C700003699B06D7D44FF0000E01E04FF0010E20 +:102C800000230093009B01330093009BB3F1706F0E +:102C900050D80369B3F1FF3F1EDD009B0133009361 +:102CA000009BB3F1706F45D80369002B14D4009BCF +:102CB00001330093009BB3F1706F3BD80369002B85 +:102CC0000AD4009B01330093009BB3F1706F31D89D +:102CD0000369B3F1FF3FD5DC0023009310230361A8 +:102CE000009B01330093009BB3F1706F22D80369FE +:102CF000DB0621D5009B01330093009BB3F1706F7D +:102D000018D80369DB0617D5009B01330093009B9D +:102D1000B3F1706F0ED80369DB060DD5009B01334C +:102D20000093009BB3F1706F04D80369DB06D7D41E +:102D300002E000BF4FF0010E5FFA81FA4FF0000988 +:102D4000BAF1000FC0F81098C0F81498C0F81C9899 +:102D50004AD0D0F800394FF00068BAF1010F4FF6B1 +:102D60007F3B08EA1313C0F80039C0F81099C0F887 +:102D700008B916D0CAF1000700F512654FF090436C +:102D8000002400BF2E68013703EAE6762E602C612E +:102D90007E1CC5F808B005F12005F3D1BAF1000F8B +:102DA00022D0D0F8003BBAF1010F08EA13134FF01C +:102DB0000008C0F8003BC0F8108BC0F808BB13D067 +:102DC000CAF1000400F532664FF090434FF67F37AA +:102DD0003568013403EAE5753560C6F81080651C76 +:102DE000B76006F12006F3D1D0F8103811F47F0F48 +:102DF00023F48073C0F81038C0F818906FF0804149 +:102E0000416103D1816941F01001816143F6000302 +:102E10008169C8F23C0312F47F0F41EA030181612A +:102E200003D0816941F008018161BCF1010F05D136 +:102E3000816941F0804141F004018161704602B036 +:102E4000BDE8F08F0000000001468268002022F0FB +:102E500001028A6070470000B0B50446C06820F0E7 +:102E6000C040E060A1B101292AD1E06840F00050E3 +:102E7000E060002005460A20F3F75EFDBD2D05F158 +:102E80000A00616916D811F00101F3D012E000BF09 +:102E9000E06840F08040E060002000BF05460A2066 +:102EA000F3F74AFDBD2D05F10A00616902D811F062 +:102EB0000101F3D1C838B0FA80F04009B0BD00BFBD +:102EC0000120B0BD00000000FEE70000000000008F +:102ED00010B52DED048BB0B1044690F8BC0090B154 +:102EE00094ED080A94ED091A31EE400A04F0D6FF79 +:102EF000B0EE408AB0EEC00A9FED401AB4EE410A2F +:102F0000F1EE10FA04DAFF2040B2BDEC048B10BDE4 +:102F100094ED090A94ED241A31EE400A94ED019AD9 +:102F200004F0BCFF29EE009A94ED070A94ED081A0C +:102F300030EE410A04F0B2FF29EE001A94ED090ABE +:102F400094ED242A94ED019A32EE400A81EE081A9B +:102F500084ED141A04F036F929EE009A94ED070A6C +:102F600094ED081A30EE410A04F098FF29EE001A99 +:102F700094ED0D0A28EE002A94ED080A94ED243A07 +:102F800033EE400A94ED049A81EE021A84ED151A8C +:102F900004F084FF29EE009A94ED090A94ED0A1AD0 +:102FA00030EE410A04F07AFF29EE001A94ED080A87 +:102FB00094ED242A32EE400A94ED049A81EE081A28 +:102FC00084ED161A04F0FEF829EE009A94ED090A31 +:102FD00094ED0A1A30EE410A04F060FF94ED0D1AE8 +:102FE00029EE000A28EE011A002080EE010A84ED85 +:102FF000170A40B2BDEC048B10BD00BFBD3786354B +:10300000B0B52DED0C8B00286ED0044690F8BC00B6 +:10301000002800F09480B0EE618AB0EE419AB0EEE4 +:1030200060AAB0EE40BAD4F89000C4F8A000606B7B +:10303000C4F8A400A06BC4F8A800206CC4F8AC00CD +:1030400084ED070A94ED01CAC4ED0A0A04F0BAF847 +:103050002CEE000A94ED01CA84ED1A0AB0EE4B0A78 +:1030600004F01CFF2CEE000A94ED00BA84ED1B0A5C +:10307000B0EE4A0A94ED04CA04F0A4F80CEE00BACB +:10308000B0EE4A0A84ED1EBA94ED04BA04F006FFCD +:103090002BEE000A94ED1B1A94ED1E2A84ED1F0AF4 +:1030A00030EE414A94ED1A1A32EE413A24EE041AF7 +:1030B00003EE031AB5EE401AF1EE10FA1ADDB1EE86 +:1030C000C10AB4EE400AF1EE10FA80F1E78194ED06 +:1030D0001A1A94ED1B2A94ED1E3A94ED1F4A33EE12 +:1030E000413A34EE424A07E0FF2568B2BDEC0C8B52 +:1030F000B0BD00BF9FEDEC0A94ED021A94ED035AA7 +:1031000021EE012A84ED200A31EE011A00EE002A98 +:1031100021EE044A21EE033A84ED224A05EE452AC7 +:1031200084ED213A24EE041A03EE031A84ED232AD7 +:1031300002EE421AB5EE401AF1EE10FA04DAFF255B +:1031400068B2BDEC0C8BB0BD9FEDDEAAB5EE401AA7 +:10315000B0EE4A0AF1EE10FA0DDDB1EEC10AB4EE9E +:10316000400AF1EE10FA80F1A18194ED213A94ED3C +:10317000224A94ED232A34EE000A72EE030A03F089 +:10318000CBFE30EE000A94ED1B1A94ED1F2A84ED5D +:10319000080A31EE42BA94ED02CA04F07FFE0CEE4A +:1031A00000BA94ED1A0A94ED1E1A30EE41CA94ED5D +:1031B000080A94ED02DA04F005F80DEE00CAB0EE4C +:1031C0004B0AF0EE4C0A03F0A7FE84ED090A94EDD9 +:1031D000080A94ED02BA94ED1ACA03F0F3FF0BEE5D +:1031E00000CA94ED080A94ED1BBA84ED1CCA94ED54 +:1031F00002CA04F053FE0CEE00BABEEE000A94EDD3 +:10320000001AD4ED1C0A41EE000A84ED1DBA84EDCB +:103210000CBA2BEE0B0AC4ED0B0A00EEA00AB5EEB9 +:10322000400AF1EE10FA0BDDB1EEC0AAB4EE4AAAE4 +:10323000F1EE10FA80F14081D4ED0B0A94ED0CBA56 +:10324000B0EE4B0A84ED0DAA03F066FE9FED9F1AC7 +:1032500039EE012AB0EE401A9FED9B3A84ED241A14 +:1032600032EE000A9FED9A2A33EE413A84ED253A78 +:1032700030EE020A84ED0F0AB4EE420AF1EE10FAC3 +:1032800052DD9FED943A00BF30EE030AB4EE420ADD +:10329000F1EE10FAC1BF30EE030AB4EE420AF1EECD +:1032A00010FA30EE030AC4BFB4EE420AF1EE10FA8F +:1032B00006DD30EE030AB4EE420AF1EE10FAE3DC6A +:1032C00084ED0F0A9FED843AB4EE430AF1EE10FA52 +:1032D00031DBB4EE421AF1EE10FA52DD9FED7D4A79 +:1032E00031EE041AB4EE421AF1EE10FAC1BF31EE1B +:1032F000041AB4EE421AF1EE10FA31EE041AC4BF09 +:10330000B4EE421AF1EE10FA06DD31EE041AB4EE14 +:10331000421AF1EE10FAE3DC84ED241AB4EE431AFB +:10332000F1EE10FA52DA31E09FED6B3AB4EE430A57 +:10333000F1EE10FACDDA9FED694A00BF30EE040AD3 +:10334000B4EE430AF1EE10FABFBF30EE040AB4EE59 +:10335000430AF1EE10FA30EE040ABCBFB4EE430AA1 +:10336000F1EE10FA06DA30EE040AB4EE430AF1EE9A +:1033700010FAE3DB84ED0F0AB4EE421AF1EE10FA14 +:10338000ACDCB4EE431AF1EE10FA1FDA9FED532ACB +:1033900031EE021AB4EE431AF1EE10FABFBF31EE6D +:1033A000021AB4EE431AF1EE10FA31EE021ABCBF63 +:1033B000B4EE431AF1EE10FA06DA31EE021AB4EE68 +:1033C000431AF1EE10FAE3DB84ED241A94ED282A77 +:1033D0009FED3C3A31EE421A94ED2E2AB5EE402A8A +:1033E00094ED0D4AF1EE10FA81EE021A94ED295A8D +:1033F00034EE454A94ED2B6AB5EE402A94F8BC00B1 +:1034000031FE031A002584ED261A84EE024AF1EEFD +:1034100010FA31EE087AB5EE402AB1EE415A84ED49 +:10342000107A34FE034A84ED275A37EE461A84EDAB +:103430000E4AF1EE10FA81EE021A31FE031A00284C +:1034400084ED111A3FF451AE94ED129A03F0BAFED6 +:10345000B0EE408A94ED0F0A94ED13AA04F01EFD1D +:103460002AEE000A94ED0D1A002080EE010AB2EE59 +:10347000041A09EE080A30EE010AB3EE041A84EDCC +:103480002C0AB4EE410AF1EE10FAC8BF012084F80C +:10349000B40068B2BDEC0C8BB0BD00BFB0EE410A09 +:1034A00004F0C4FD13E600BF00000000B0EE410AC6 +:1034B00004F0BCFD59E600BF04F0B8FDB0EE40AA30 +:1034C000BAE600BF00000000DB0FC93FDB0FC9BF39 +:1034D000DB0F4940DB0FC9C0DB0F49C0DB0FC94020 :1034E00040B14FF0FF0C002A18BF002908D14FFA55 :1034F0008CF070474FF0FF0C4FFA8CF0704700BF14 :1035000090F8BC30002BF2D0036E0B604FF0000C33 @@ -4958,7 +4958,7 @@ :1035B0003AF849F69A100121C3F6196060630020B9 :1035C000E06384F8BC1040B2B0BD00000000000011 :1035D00010B568B1044690F8BC0048B1B1EE601A6D -:1035E000204684ED120A84ED131AFFF775FC18B11A +:1035E000204684ED120A84ED131AFFF771FC18B11E :1035F000FF2040B210BD00BF94ED130A94ED163ABF :1036000094ED171A94ED144A94ED152A20EE011A40 :1036100022EE000A94ED122A002040B202EE031AB4 @@ -6361,7 +6361,7 @@ :108D600061736B5F496E697400617474695F6573E8 :108D70006974006374726C5F73686F6F74000000D5 :108D8000A08D010800000024C002000008030008B4 -:108D9000808F0108C0020024807F01006403000866 +:108D9000788F0108C0020024807F0100640300086E :108DA000320142102C90D003042011022B073D63A6 :108DB000ADC1BA503A33BBD3F2C93A1211292428B3 :108DC00009DB0F493FCDCC4C3D0C2B010206213C69 @@ -6373,24 +6373,24 @@ :108E20003E60393C1904296889041D027CFF03ADAA :108E30003A7D0638087EFF030101027F06111A42BF :108E4000AF12022B4101DB49E0413808F6285C3EB5 -:108E500093188404290CC109161C105FFC515DC3D2 -:108E6000DA646B431B6AD3C20661CABFEBE812C166 -:108E7000ED82084161251BC18D9F74BE32227DC2E7 -:108E800070CE7A4223ABAAC1C7248FBE2F9094C262 -:108E9000427093428219D1C1F17219BFF1608BC245 -:108EA0007199B842F3F530C227920141C95374C198 -:108EB000A371A241328621C13FEB103040D82D7AF8 -:108EC0004227981041D2AE33C24BA98D4107A4016D -:108ED0003D123BEB40A11DD3C0BA1A3340D21C69EE -:108EE000C2C7CC9B425BA519C2748D60324151C28E -:108EF000B2C2E8C9E142E7BE53C2ABCB2B412BD98A -:108F00007743315D76C33D31A7421E300541DA49D2 -:108F10007C420DA77AC21F5BAA41FCC76C3F3A8016 -:108F2000085C9A9999D82910280AF041DB0FC940AA -:108F3000CDCC4C142E3F0AD7233C044910492019AC -:108F4000081ABF0422401A4004C11A201C692023B9 -:108F500020422BA0413C3159285B48433C19042A4C -:108F6000A014B94019646A3D407C4CE88A1C4DCD80 -:108F7000CCCCBD181CF041AA01410000000000004B +:108E500093188404290CC109161C60C1C1368EC345 +:108E60007EBB96438DABFDC2243C45C0EC2B63C05A +:108E7000025B8C404AB203C1FDE50DBE608996C21B +:108E800099119642FAE6CEC1A79193BE3D4681C2A2 +:108E90003A7E814208A7BBC1FF51EABE14F489C1E2 +:108EA00083DED141A0495AC1C0550A4055AA85C1A7 +:108EB000CF42B541EEC239C16EC41F402D45944228 +:108EC000BEE2C2C0EE9321C2B44784417C2CCD40A7 +:108ED000DF6340C0E2B296BFA504D63F4DA06CC28E +:108EE00057CB9E42F96220C26AE70D4186717EC26D +:108EF000D061A2428DE61CC28B810741C9E6624265 +:108F0000296468C28F29A341DD5DB4405B57944258 +:108F10006BCD94C2F437CE41D5BB613F3A80085C3B +:108F20009A9999D82910280AF041DB0FC940CDCC75 +:108F30004C142E3F0AD7233C044910492019081A23 +:108F4000BF0422401A4004C11A201C692023204279 +:108F50002BA0413C3159285B48433C19042AA014FA +:108F6000B94019646A3D407C4CE88A1C4DCDCCCC9C +:088F7000BD181CF041AA0141EB :04000005080002CD20 :00000001FF diff --git a/User/component/vmc.c b/User/component/vmc.c index ddbfc53..f819c98 100644 --- a/User/component/vmc.c +++ b/User/component/vmc.c @@ -80,7 +80,7 @@ int8_t VMC_ForwardSolve(VMC_t *vmc, float phi1, float phi4, float body_pitch, fl if (vmc == NULL || !vmc->initialized) { return -1; } - // body_pitch = body_pitch; // 机体俯仰角取反 + body_pitch = -body_pitch; // 机体俯仰角取反 VMC_Leg_t *leg = &vmc->leg; diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index 3260e27..4ae3f9d 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -129,7 +129,7 @@ static void Chassis_UpdateChassisState(Chassis_t *c) { 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.072f; + 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; diff --git a/User/module/config.c b/User/module/config.c index a51fecd..629dc41 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -235,19 +235,19 @@ Config_RobotParam_t robot_config = { }, .lqr_gains = { - .k11_coeff = { -2.213202553185133e+02f, 2.353939463356143e+02f, -1.057072351438971e+02f, -1.581085937677281e+00f }, // theta - .k12_coeff = { -9.181864404672975e+00f, 8.531964722737065e+00f, -9.696625432480346e+00f, -2.388898921230960e-01f }, // d_theta - .k13_coeff = { -6.328339397527442e+01f, 6.270159865929592e+01f, -2.133356351416681e+01f, -2.795774497769496e-01f }, // x - .k14_coeff = { -7.428160824353201e+01f, 7.371925049068537e+01f, -2.613745545093503e+01f, -5.994101373770330e-01f }, // d_x - .k15_coeff = { -6.968934105907989e+01f, 9.229969229361623e+01f, -4.424018428098277e+01f, 8.098181536555296e+00f }, // phi - .k16_coeff = { -1.527045508038401e+01f, 2.030548630730375e+01f, -1.009526207086012e+01f, 2.014358176738665e+00f }, // d_phi - .k21_coeff = { 6.254476937997669e+01f, 9.037146968574660e+00f, -4.492072460618583e+01f, 1.770766202994207e+01f }, // theta - .k22_coeff = { 3.165057029795604e-02f, 7.350960766534424e+00f, -6.597366624137901e+00f, 2.798506180182324e+00f }, // d_theta - .k23_coeff = { -5.827814614802593e+01f, 7.789995488757775e+01f, -3.841148024725668e+01f, 8.034534049078013e+00f }, // x - .k24_coeff = { -8.937952443465080e+01f, 1.128943502182752e+02f, -5.293642666103645e+01f, 1.073722383888271e+01f }, // d_x - .k25_coeff = { 2.478483065877546e+02f, -2.463640234149189e+02f, 8.359617215530402e+01f, 8.324247402653134e+00f }, // phi - .k26_coeff = { 6.307211927250707e+01f, -6.266313408748906e+01f, 2.129449351279647e+01f, 9.249265186231070e-01f }, // d_phi - }, + .k11_coeff = { -2.844277590546524e+02f, 3.014647915495374e+02f, -1.268350617667978e+02f, -3.081795747371510e+00f }, // theta + .k12_coeff = { -3.549555833965762e+00f, 4.386109489290642e+00f, -8.231027185636849e+00f, -1.385726427113662e-01f }, // d_theta + .k13_coeff = { -7.526831317875141e+01f, 7.503437037461828e+01f, -2.586278102813240e+01f, -2.882206207079063e-01f }, // x + .k14_coeff = { -6.463718727551078e+01f, 6.474653251665229e+01f, -2.345655849302561e+01f, -4.576568180320017e-01f }, // d_x + .k15_coeff = { -1.724417956662031e+01f, 2.623364741718471e+01f, -1.364297468210267e+01f, 2.161483799244492e+00f }, // phi + .k16_coeff = { -1.670817080995841e+01f, 2.265762117831611e+01f, -1.161008984543497e+01f, 2.496364045937191e+00f }, // d_phi + .k21_coeff = { 7.413511085596214e+01f, -6.090178313593463e+00f, -4.039446413615985e+01f, 1.653501139374634e+01f }, // theta + .k22_coeff = { 6.411680140494978e+00f, -3.006095633385133e+00f, -1.177334020684990e+00f, 1.672016772859867e+00f }, // d_theta + .k23_coeff = { -5.915654230562277e+01f, 7.939714736323532e+01f, -4.009665367872189e+01f, 8.868997496924505e+00f }, // x + .k24_coeff = { -6.361086354133386e+01f, 8.119104026481590e+01f, -3.922514760493152e+01f, 8.469126805164274e+00f }, // d_x + .k25_coeff = { 5.672537764809064e+01f, -5.809781196995141e+01f, 2.039529228765693e+01f, 5.636457827688945e+00f }, // phi + .k26_coeff = { 7.417061748885547e+01f, -7.440121006735944e+01f, 2.577732090617134e+01f, 8.817723457684845e-01f }, // d_phi +}, .theta = 0.0f, .x = 0.0f, .phi = -0.1f, diff --git a/User/task/atti_esit.c b/User/task/atti_esit.c index 770756e..4cee07c 100644 --- a/User/task/atti_esit.c +++ b/User/task/atti_esit.c @@ -92,7 +92,7 @@ void Task_atti_esit(void *argument) { 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.pit = eulr_to_send.rol; imu_for_chassis.euler.yaw = eulr_to_send.yaw - 1.57079632679f; // -90度 osMessageQueueReset( task_runtime.msgq.chassis.imu); // 重置消息队列，防止阻塞 diff --git a/utils/.DS_Store b/utils/.DS_Store new file mode 100644 index 0000000..668813e Binary files /dev/null and b/utils/.DS_Store differ diff --git a/utils/Simulation-master/.DS_Store b/utils/Simulation-master/.DS_Store new file mode 100644 index 0000000..461374a Binary files /dev/null and b/utils/Simulation-master/.DS_Store differ diff --git a/utils/Simulation-master/.gitignore b/utils/Simulation-master/.gitignore new file mode 100644 index 0000000..be82517 --- /dev/null +++ b/utils/Simulation-master/.gitignore @@ -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 diff --git a/utils/Simulation-master/LICENSE b/utils/Simulation-master/LICENSE new file mode 100644 index 0000000..60acd7e --- /dev/null +++ b/utils/Simulation-master/LICENSE @@ -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. diff --git a/utils/Simulation-master/balance/.DS_Store b/utils/Simulation-master/balance/.DS_Store new file mode 100644 index 0000000..285a701 Binary files /dev/null and b/utils/Simulation-master/balance/.DS_Store differ diff --git a/utils/Simulation-master/balance/parallel_legs/blance_leg.slx b/utils/Simulation-master/balance/parallel_legs/blance_leg.slx new file mode 100644 index 0000000..1e6fc9c Binary files /dev/null and b/utils/Simulation-master/balance/parallel_legs/blance_leg.slx differ diff --git a/utils/Simulation-master/balance/parallel_legs/blance_leg.slx.original b/utils/Simulation-master/balance/parallel_legs/blance_leg.slx.original new file mode 100644 index 0000000..7e89fe3 Binary files /dev/null and b/utils/Simulation-master/balance/parallel_legs/blance_leg.slx.original differ diff --git a/utils/Simulation-master/balance/parallel_legs/blance_leg.slx.r2022b b/utils/Simulation-master/balance/parallel_legs/blance_leg.slx.r2022b new file mode 100644 index 0000000..0f66126 Binary files /dev/null and b/utils/Simulation-master/balance/parallel_legs/blance_leg.slx.r2022b differ diff --git a/utils/Simulation-master/balance/parallel_legs/blance_leg.slx.r2023a b/utils/Simulation-master/balance/parallel_legs/blance_leg.slx.r2023a new file mode 100644 index 0000000..0f8003c Binary files /dev/null and b/utils/Simulation-master/balance/parallel_legs/blance_leg.slx.r2023a differ diff --git a/utils/Simulation-master/balance/parallel_legs/d_phi0.m b/utils/Simulation-master/balance/parallel_legs/d_phi0.m new file mode 100644 index 0000000..2657604 --- /dev/null +++ b/utils/Simulation-master/balance/parallel_legs/d_phi0.m @@ -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 + diff --git a/utils/Simulation-master/balance/parallel_legs/display_polynomial.m b/utils/Simulation-master/balance/parallel_legs/display_polynomial.m new file mode 100644 index 0000000..44467e6 --- /dev/null +++ b/utils/Simulation-master/balance/parallel_legs/display_polynomial.m @@ -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 \ No newline at end of file diff --git a/utils/Simulation-master/balance/parallel_legs/get_k.m b/utils/Simulation-master/balance/parallel_legs/get_k.m new file mode 100644 index 0000000..46e1806 --- /dev/null +++ b/utils/Simulation-master/balance/parallel_legs/get_k.m @@ -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 diff --git a/utils/Simulation-master/balance/parallel_legs/get_k_length.m b/utils/Simulation-master/balance/parallel_legs/get_k_length.m new file mode 100644 index 0000000..47347c7 --- /dev/null +++ b/utils/Simulation-master/balance/parallel_legs/get_k_length.m @@ -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 \ No newline at end of file diff --git a/utils/Simulation-master/balance/parallel_legs/physical_calc.mlx b/utils/Simulation-master/balance/parallel_legs/physical_calc.mlx new file mode 100644 index 0000000..78bd3d3 Binary files /dev/null and b/utils/Simulation-master/balance/parallel_legs/physical_calc.mlx differ diff --git a/utils/Simulation-master/balance/series_legs/blance_leg.slx b/utils/Simulation-master/balance/series_legs/blance_leg.slx new file mode 100644 index 0000000..f1fb734 Binary files /dev/null and b/utils/Simulation-master/balance/series_legs/blance_leg.slx differ diff --git a/utils/Simulation-master/balance/series_legs/calc.mlx b/utils/Simulation-master/balance/series_legs/calc.mlx new file mode 100644 index 0000000..fc0f109 Binary files /dev/null and b/utils/Simulation-master/balance/series_legs/calc.mlx differ diff --git a/utils/Simulation-master/balance/series_legs/doc/2_link.png b/utils/Simulation-master/balance/series_legs/doc/2_link.png new file mode 100644 index 0000000..035b0fa Binary files /dev/null and b/utils/Simulation-master/balance/series_legs/doc/2_link.png differ diff --git a/utils/Simulation-master/balance/series_legs/doc/2连杆分析.md b/utils/Simulation-master/balance/series_legs/doc/2连杆分析.md new file mode 100644 index 0000000..a1667bd --- /dev/null +++ b/utils/Simulation-master/balance/series_legs/doc/2连杆分析.md @@ -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{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] +$$ diff --git a/utils/Simulation-master/balance/series_legs/get_k.m b/utils/Simulation-master/balance/series_legs/get_k.m new file mode 100644 index 0000000..73f8a65 --- /dev/null +++ b/utils/Simulation-master/balance/series_legs/get_k.m @@ -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 diff --git a/utils/Simulation-master/balance/series_legs/get_k_length.m b/utils/Simulation-master/balance/series_legs/get_k_length.m new file mode 100644 index 0000000..aeb653b --- /dev/null +++ b/utils/Simulation-master/balance/series_legs/get_k_length.m @@ -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([100 100 2000 200 5000 600]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1 + R=[240 0;0 40]; %T Tp + + K=lqr(A,B,Q,R); + +end \ No newline at end of file diff --git a/utils/Simulation-master/balance/series_legs/leg_sim.slx b/utils/Simulation-master/balance/series_legs/leg_sim.slx new file mode 100644 index 0000000..5afa555 Binary files /dev/null and b/utils/Simulation-master/balance/series_legs/leg_sim.slx differ diff --git a/utils/Simulation-master/balance/series_parallel_legs/README.md b/utils/Simulation-master/balance/series_parallel_legs/README.md new file mode 100644 index 0000000..19bad54 --- /dev/null +++ b/utils/Simulation-master/balance/series_parallel_legs/README.md @@ -0,0 +1,3 @@ +# 串并联腿仿真总结 + +这次仿真对小企鹅来说最重要的一点就是确定了替换为串联腿时不需要替换模型，直接移植现有并联腿模型即可。 \ No newline at end of file diff --git a/utils/Simulation-master/balance/series_parallel_legs/blance_leg.slx b/utils/Simulation-master/balance/series_parallel_legs/blance_leg.slx new file mode 100644 index 0000000..741bffd Binary files /dev/null and b/utils/Simulation-master/balance/series_parallel_legs/blance_leg.slx differ diff --git a/utils/Simulation-master/balance/series_parallel_legs/blance_leg.slx.original b/utils/Simulation-master/balance/series_parallel_legs/blance_leg.slx.original new file mode 100644 index 0000000..7e89fe3 Binary files /dev/null and b/utils/Simulation-master/balance/series_parallel_legs/blance_leg.slx.original differ diff --git a/utils/Simulation-master/balance/series_parallel_legs/blance_leg.slx.r2022b b/utils/Simulation-master/balance/series_parallel_legs/blance_leg.slx.r2022b new file mode 100644 index 0000000..0f66126 Binary files /dev/null and b/utils/Simulation-master/balance/series_parallel_legs/blance_leg.slx.r2022b differ diff --git a/utils/Simulation-master/balance/series_parallel_legs/blance_leg.slx.r2023a b/utils/Simulation-master/balance/series_parallel_legs/blance_leg.slx.r2023a new file mode 100644 index 0000000..0f8003c Binary files /dev/null and b/utils/Simulation-master/balance/series_parallel_legs/blance_leg.slx.r2023a differ diff --git a/utils/Simulation-master/balance/series_parallel_legs/d_phi0.m b/utils/Simulation-master/balance/series_parallel_legs/d_phi0.m new file mode 100644 index 0000000..2657604 --- /dev/null +++ b/utils/Simulation-master/balance/series_parallel_legs/d_phi0.m @@ -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 + diff --git a/utils/Simulation-master/balance/series_parallel_legs/display_polynomial.m b/utils/Simulation-master/balance/series_parallel_legs/display_polynomial.m new file mode 100644 index 0000000..44467e6 --- /dev/null +++ b/utils/Simulation-master/balance/series_parallel_legs/display_polynomial.m @@ -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 \ No newline at end of file diff --git a/utils/Simulation-master/balance/series_parallel_legs/get_k.m b/utils/Simulation-master/balance/series_parallel_legs/get_k.m new file mode 100644 index 0000000..46e1806 --- /dev/null +++ b/utils/Simulation-master/balance/series_parallel_legs/get_k.m @@ -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 diff --git a/utils/Simulation-master/balance/series_parallel_legs/get_k_length.m b/utils/Simulation-master/balance/series_parallel_legs/get_k_length.m new file mode 100644 index 0000000..9a1d58c --- /dev/null +++ b/utils/Simulation-master/balance/series_parallel_legs/get_k_length.m @@ -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 \ No newline at end of file diff --git a/utils/Simulation-master/balance/series_parallel_legs/physical_calc.mlx b/utils/Simulation-master/balance/series_parallel_legs/physical_calc.mlx new file mode 100644 index 0000000..78bd3d3 Binary files /dev/null and b/utils/Simulation-master/balance/series_parallel_legs/physical_calc.mlx differ diff --git a/utils/Simulation-master/balance/series_parallel_legs/save_phy.slx b/utils/Simulation-master/balance/series_parallel_legs/save_phy.slx new file mode 100644 index 0000000..da20365 Binary files /dev/null and b/utils/Simulation-master/balance/series_parallel_legs/save_phy.slx differ diff --git a/utils/Simulation-master/dog_simple/dog.slx b/utils/Simulation-master/dog_simple/dog.slx new file mode 100644 index 0000000..0e5c4c8 Binary files /dev/null and b/utils/Simulation-master/dog_simple/dog.slx differ diff --git a/utils/Simulation-master/dog_simple/dog.slx.r2023a b/utils/Simulation-master/dog_simple/dog.slx.r2023a new file mode 100644 index 0000000..f1fb734 Binary files /dev/null and b/utils/Simulation-master/dog_simple/dog.slx.r2023a differ diff --git a/utils/k_calc/chassis_calc_heu.m b/utils/k_calc/chassis_calc_heu.m new file mode 100644 index 0000000..047b219 --- /dev/null +++ b/utils/k_calc/chassis_calc_heu.m @@ -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) \ No newline at end of file diff --git a/utils/matlab/LQR_calc.m b/utils/matlab/LQR_calc.m new file mode 100644 index 0000000..0a56cef --- /dev/null +++ b/utils/matlab/LQR_calc.m @@ -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 \ No newline at end of file diff --git a/utils/matlab/UseBodyVel.m b/utils/matlab/UseBodyVel.m new file mode 100644 index 0000000..0d7df2f --- /dev/null +++ b/utils/matlab/UseBodyVel.m @@ -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 + + diff --git a/utils/matlab/vmc.m b/utils/matlab/vmc.m new file mode 100644 index 0000000..b3f73da --- /dev/null +++ b/utils/matlab/vmc.m @@ -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); diff --git a/utils/matlab/wheel_leg.slx b/utils/matlab/wheel_leg.slx new file mode 100644 index 0000000..981433e Binary files /dev/null and b/utils/matlab/wheel_leg.slx differ diff --git a/utils/matlab/wheel_leg.slxc b/utils/matlab/wheel_leg.slxc new file mode 100644 index 0000000..854b3f3 Binary files /dev/null and b/utils/matlab/wheel_leg.slxc differ diff --git a/utils/mod_wheelleg_chassis.cpp b/utils/mod_wheelleg_chassis.cpp new file mode 100644 index 0000000..2958ad2 --- /dev/null +++ b/utils/mod_wheelleg_chassis.cpp @@ -0,0 +1,731 @@ +#include "mod_wheelleg_chassis.hpp" + +#include +#include + +using namespace Module; +Wheelleg::Wheelleg(Param ¶m) + : 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( + event_callback, this, this->param_.EVENT_MAP); + + auto chassis_thread = [](Wheelleg *chassis) + { + auto cmd_sub = + Message::Subscriber("cmd_chassis"); + auto eulr_sub = + Message::Subscriber("chassis_imu_eulr"); + auto gyro_sub = + Message::Subscriber("chassis_gyro"); + + auto yaw_sub = Message::Subscriber("chassis_yaw"); + + auto accl_sub = + Message::Subscriber("chassis_imu_accl_abs"); + // auto yaw_sub = Message::Subscriber("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 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 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 result3; + std::tuple 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; +} diff --git a/utils/香港大学轮腿电控及建模开源-3/Chassis_Task.c b/utils/香港大学轮腿电控及建模开源-3/Chassis_Task.c new file mode 100644 index 0000000..aead3e0 --- /dev/null +++ b/utils/香港大学轮腿电控及建模开源-3/Chassis_Task.c @@ -0,0 +1,1347 @@ +#include "Chassis_Task.h" +#include "bsp_buzzer.h" +#include "FreeRTOS.h" +#include "task.h" +#include "detect_task.h" +#include "remote_control.h" +#include "referee.h" +#include "can.h" +#include "bsp_can.h" +#include "arm_math.h" +#include "string.h" +#include "Kalman_Mix.h" +#include "INS_Task.h" +#include "math.h" +#include "VMC.h" +#include "mpc.h" +#include "lqr.h" +#include "user_lib.h" + + +#ifndef PI +#define PI 3.14159265358979f +#endif +#ifndef FOLLOW_DEADBAND +#define FOLLOW_DEADBAND 0.05f +#endif +#ifndef min +#define min(a,b) ((a)<(b)?(a):(b)) +#endif +#ifndef square +#define square(x) ((x)*(x)) +#endif + +#define ABS(x) (((x) > 0) ? (x) : (-(x))) +#define LimitMax(input, max) \ +{ \ + if ((input) > max) \ + { \ + input = max; \ + } \ + else if ((input) < -max) \ + { \ + input = -max; \ + } \ +} +#define LimitOutput(input, min, max ) \ +{ \ + if( input < min ) \ + input = min; \ + else if( input > max ) \ + input = max; \ +} + +#define SIGN(x) ((x)>0 ? 1 : ((x)<0? -1: 0)) + + +uint8_t use_mpc = 0, abnormal_debug = 0; +chassis_control_t chassis_control; +kalman2_state Body_Speed_KF; +// ------------------ PID info ------------------ +fp32 roll_PD[2] = {100.0, 20.0}; // 200, 45 +fp32 coordinate_PD[2] = { 10.0f, 0.5f }; //15.0f,1.0f +fp32 yaw_PD_test[2] = { 20.0f, 180.0f }; +fp32 stand_PD[2] = { 200.0f, 50.0f }; +fp32 jump_stand_PD[2] = { 10000.0f, 150.0f }; +fp32 suspend_stand_PD[2] = { 100.0f, 30.0f }; +// fp32 stand_PD[2]={150.0f, 20.0f};//1N/cm +fp32 suspend_foot_speed_p = 10.0f; +// fp32 High_Offset = 0.20; +fp32 High_Offset = 0.06; +fp32 Moving_High_Offset = 0.0f; +uint8_t last_is_update; +fp32 IDEAL_PREPARING_STAND_JUMPING_ANGLE = 0.6f; + +extern fp32 LQR[4][10]; + +fp32 suspend_LQR[2][6] = { + 20.0f,5.0f, 0.0f, 0.0f, 0.0f, 0.0f, + 0, 0, 0, 0, 0, 0 +}; +fp32 x_set[10]; +fp32 debug_2 = 1,upper_bound, stepp = 0.2; +fp32 debug_flag, last_debug_flag; +fp32 HIGH_HIGH = 0.35f; + +fp32 SIT_HIGH = 0.11f; +fp32 NORMAL_HIGH = 0.18f; //16 + +fp32 lower_dec_speed = 1.0f; +uint8_t mpc_cnt = 0, mpc_period = 9; +float alpha_dx = 0.95f; +fp32 rollP, rollD, roll_angle_deadband = 0.0f, roll_gyro_deadband = 0.0f, leg_dlength_deadband = 0.0f; +fp32 rotate_move_threshold = 45.0f, rotate_speed = 12.0f, fake_sign = 1.0f, rc_sign = 1.0f, rotate_move_scale = 1.0f,normal_move_scale = 2.0f, offset_k = 0.31f, fly_speed = 2.5f; +fp32 rc_angle, rc_angle_temp, X_speed, Y_speed, delta_theta, delta_theta_temp, rotate_move_offset, normalized_speed, temp_max_spd, acc_step = 1.3f, dec_step = 0.1f; +fp32 move_scale_list[11] = { 0.0, 1.0, 1.5, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0}; +fp32 cap_move_scale_list[11] = { 0.0, 1.5, 2.0, 2.5, 2.5, 3.0, 3.0, 3.0, 3.0, 3.0, 3.0}; +fp32 rotate_speed_list[11] = {0.0, 5.0, 5.3, 5.6, 6.0, 6.0, 7.0, 8.0, 9.0, 10.0, 12.0}; +fp32 rotate_move_scale_list[11] = {0.0, 1.0, 1.2, 1.4, 1.6, 1.8, 2.0, 2.2, 2.4, 2.6, 2.8}; +int speed_sign = 0, target_speed_sign = 0; +uint8_t robot_level = 1; +ext_robot_status_t *robot; +fp32 TK_x_p = -10.0f, TK_y_p = 10.0f, TK_y_d = 3.0f, reducing_p = 120.0f; +fp32 stablize_foot_speed_threshold = 1.2f, stablize_yaw_speed_threshold = 1.5f; +uint8_t no_follow_flag, follow_angle; + +enum Chassis_Mode chassis_mode; + + +// ------------------ function definition ------------------ +void Chassis_Init( chassis_control_t *init ); +void Chassis_Data_Update( chassis_control_t *fdb ); +void Chassis_Status_Detect( chassis_control_t *detect ); +void Chassis_Mode_Set( chassis_control_t *mode_set ); +void Chassis_Mode_Change_Control_Transit( chassis_control_t *chassis_mode_change ); +void Target_Value_Set( chassis_control_t *target_value_set ); +void Chassis_Torque_Calculation( chassis_control_t *bl_ctrl ); +void Chassis_Torque_Combine( chassis_control_t *bl_ctrl ); +void Motor_CMD_Send(chassis_control_t *CMD_Send); +void Joint_Motor_to_Init_Pos(void); +void Motor_Zero_CMD_Send(void); +void HT_Motor_zero_set(void); + +int16_t set1, set2; +uint16_t launching_time = 0; + +void Chassis_Task(void const * argument) +{ + vTaskDelay(CHASSIS_TASK_INIT_TIME); + Chassis_Init(&chassis_control); + vTaskDelay(100); + while(1) + { + Chassis_Data_Update(&chassis_control); + Chassis_Status_Detect(&chassis_control); + Chassis_Mode_Set(&chassis_control); + Chassis_Mode_Change_Control_Transit(&chassis_control); + Target_Value_Set(&chassis_control); + Chassis_Torque_Calculation(&chassis_control); + Chassis_Torque_Combine(&chassis_control); + Motor_CMD_Send(&chassis_control); + vTaskDelay(1); + } +} + + +void Chassis_Init( chassis_control_t *init ) +{ + // ------------------ Set HT Zero Point ------------------ + buzzer_on(75, 60); + vTaskDelay(100); + HT_Motor_zero_set(); + buzzer_off(); + + Motor_Zero_CMD_Send(); + + kalman_second_order_init(&Body_Speed_KF); + + // ------------------ Ptr Set ------------------ + init->joint_motor_1.motor_measure = get_HT_motor_measure_point(0); + init->joint_motor_2.motor_measure = get_HT_motor_measure_point(1); + init->joint_motor_3.motor_measure = get_HT_motor_measure_point(2); + init->joint_motor_4.motor_measure = get_HT_motor_measure_point(3); + // init->foot_motor_L.motor_measure = get_BMmotor_measure_point(0); + // init->foot_motor_R.motor_measure = get_BMmotor_measure_point(1); + init->chassis_rc_ctrl = get_Gimabl_control_point(); + init->chassis_posture_info.chassis_INS_angle_point = get_INS_angle_point(); + init->chassis_posture_info.chassis_INS_gyro_point = get_gyro_data_point(); + init->chassis_posture_info.chassis_INS_accel_point = get_acc_data_point(); + + /* + first_order_filter_init(&rc_filter, 0.002f, rc_filt_numb); + first_order_filter_init(&tl_filter, 0.002f, tl_numb); + first_order_filter_init(&tl_run_filter, 0.002f, tl_numb); + OLS_Init(&OLS_S0_L,2); + OLS_Init(&OLS_S0_R,2); + */ + + // ------------------ Mode Set ------------------ + init->mode.chassis_mode = init->mode.last_chassis_mode = DISABLE_CHASSIS; + init->mode.chassis_balancing_mode = init->mode.last_chassis_balancing_mode = NO_FORCE; + init->mode.sport_mode = init->mode.last_sport_mode = NORMAL_MOVING_MODE; + init->joint_motor_1.motor_mode = init->joint_motor_1.last_motor_mode = MOTOR_NO_FORCE; + init->joint_motor_2.motor_mode = init->joint_motor_2.last_motor_mode = MOTOR_NO_FORCE; + init->joint_motor_3.motor_mode = init->joint_motor_3.last_motor_mode = MOTOR_NO_FORCE; + init->joint_motor_4.motor_mode = init->joint_motor_4.last_motor_mode = MOTOR_NO_FORCE; + init->foot_motor_L.motor_mode = init->foot_motor_L.last_motor_mode = MOTOR_NO_FORCE; + init->foot_motor_R.motor_mode = init->foot_motor_R.last_motor_mode = MOTOR_NO_FORCE; + + // ------------------ Initialize HT Motor ------------------ + init->joint_motor_1.position_offset = init->joint_motor_1.motor_measure->ecd; + init->joint_motor_2.position_offset = init->joint_motor_2.motor_measure->ecd; + init->joint_motor_3.position_offset = init->joint_motor_3.motor_measure->ecd; + init->joint_motor_4.position_offset = init->joint_motor_4.motor_measure->ecd; + init->joint_motor_1.position = (init->joint_motor_1.motor_measure->ecd -init->joint_motor_1.position_offset) + LEG_OFFSET; + init->joint_motor_2.position = (init->joint_motor_2.motor_measure->ecd -init->joint_motor_2.position_offset) - LEG_OFFSET; + init->joint_motor_3.position = (init->joint_motor_3.motor_measure->ecd -init->joint_motor_3.position_offset) + LEG_OFFSET; + init->joint_motor_4.position = (init->joint_motor_4.motor_measure->ecd -init->joint_motor_4.position_offset) - LEG_OFFSET; + + init->flag_info.init_flag = 1; + Chassis_Data_Update(init); + init->flag_info.init_flag = 0; + + init->foot_motor_L.distance_offset = init->foot_motor_L.distance; + init->foot_motor_R.distance_offset = init->foot_motor_R.distance; + + Power_Init(init); + + // ------------------ MPC init ------------------ + MPC_Init(); +} + +void Chassis_Data_Update( chassis_control_t *fdb ) +{ + robot = get_robot_status_point(); + robot_level = robot->robot_level; + // robot_level = 1; + // -------------------- Update foot measure -------------------- + fdb->foot_motor_L.motor_measure.ecd = (uint16_t)motor_BM[0].ecd; + fdb->foot_motor_L.motor_measure.last_ecd = (uint16_t)motor_BM[0].last_ecd; + fdb->foot_motor_L.motor_measure.speed_rpm = (int16_t)motor_BM[0].speed_rpm; + fdb->foot_motor_L.torque_get = motor_BM[0].given_current*55.0f/32767.0f; + + fdb->foot_motor_R.motor_measure.ecd = (uint16_t)motor_BM[1].ecd; + fdb->foot_motor_R.motor_measure.last_ecd = (uint16_t)motor_BM[1].last_ecd; + fdb->foot_motor_R.motor_measure.speed_rpm = (int16_t)motor_BM[1].speed_rpm; + fdb->foot_motor_R.torque_get = motor_BM[1].given_current*55.0f/32767.0f; + + // ------------------ Update mode info ------------------ + fdb->mode.last_chassis_mode = fdb->mode.chassis_mode; + fdb->mode.last_chassis_balancing_mode = fdb->mode.chassis_balancing_mode; + fdb->mode.last_sport_mode = fdb->mode.sport_mode; + fdb->mode.last_jumping_mode = fdb->mode.jumping_mode; + fdb->mode.last_jumping_stage = fdb->mode.jumping_stage; + fdb->mode.last_chassis_high_mode = fdb->mode.chassis_high_mode; + fdb->flag_info.last_static_flag = fdb->flag_info.static_flag; + fdb->flag_info.last_moving_flag = fdb->flag_info.moving_flag; + last_debug_flag = debug_flag; + fdb->flag_info.last_overpower_warning_flag = fdb->flag_info.overpower_warning_flag; + fdb->flag_info.last_stablize_high_flag = fdb->flag_info.stablize_high_flag; + + fdb->flag_info.last_suspend_flag_L = fdb->flag_info.suspend_flag_L; + fdb->flag_info.last_suspend_flag_R = fdb->flag_info.suspend_flag_R; + + fdb->joint_motor_1.last_motor_mode = fdb->joint_motor_1.motor_mode; + fdb->joint_motor_2.last_motor_mode = fdb->joint_motor_2.motor_mode; + fdb->joint_motor_3.last_motor_mode = fdb->joint_motor_3.motor_mode; + fdb->joint_motor_4.last_motor_mode = fdb->joint_motor_4.motor_mode; + fdb->foot_motor_L.last_motor_mode = fdb->foot_motor_L.motor_mode; + fdb->foot_motor_R.last_motor_mode = fdb->foot_motor_R.motor_mode; + + // ------------------ Update IMU info ------------------ + fdb->chassis_posture_info.pitch_angle = *(fdb->chassis_posture_info.chassis_INS_angle_point + INS_PITCH_ADDRESS_OFFSET); + fdb->chassis_posture_info.roll_angle = *(fdb->chassis_posture_info.chassis_INS_angle_point + INS_ROLL_ADDRESS_OFFSET); + fdb->chassis_posture_info.yaw_angle = *(fdb->chassis_posture_info.chassis_INS_angle_point + INS_YAW_ADDRESS_OFFSET) + yaw_count * PI2; + fdb->chassis_posture_info.pitch_gyro = *(fdb->chassis_posture_info.chassis_INS_gyro_point + INS_PITCH_ADDRESS_OFFSET); + fdb->chassis_posture_info.roll_gyro = *(fdb->chassis_posture_info.chassis_INS_gyro_point + INS_ROLL_ADDRESS_OFFSET); + fdb->chassis_posture_info.yaw_gyro = *(fdb->chassis_posture_info.chassis_INS_gyro_point + INS_YAW_ADDRESS_OFFSET); + + // ------------------ Update HT Motor info ------------------ + fdb->joint_motor_1.position = (fdb->joint_motor_1.motor_measure->ecd - fdb->joint_motor_1.position_offset) + LEG_OFFSET; + fdb->joint_motor_2.position = (fdb->joint_motor_2.motor_measure->ecd - fdb->joint_motor_2.position_offset) - LEG_OFFSET; + fdb->joint_motor_3.position = (fdb->joint_motor_3.motor_measure->ecd - fdb->joint_motor_3.position_offset) + LEG_OFFSET; + fdb->joint_motor_4.position = (fdb->joint_motor_4.motor_measure->ecd - fdb->joint_motor_4.position_offset) - LEG_OFFSET; + + fdb->joint_motor_1.velocity = fdb->joint_motor_1.motor_measure->speed_rpm; + fdb->joint_motor_2.velocity = fdb->joint_motor_2.motor_measure->speed_rpm; + fdb->joint_motor_3.velocity = fdb->joint_motor_3.motor_measure->speed_rpm; + fdb->joint_motor_4.velocity = fdb->joint_motor_4.motor_measure->speed_rpm; + + fdb->joint_motor_1.torque_get = 0.95f * fdb->joint_motor_1.torque_get + 0.05f * fdb->joint_motor_1.motor_measure->real_torque; + fdb->joint_motor_2.torque_get = 0.95f * fdb->joint_motor_2.torque_get + 0.05f * fdb->joint_motor_2.motor_measure->real_torque; + fdb->joint_motor_3.torque_get = 0.95f * fdb->joint_motor_3.torque_get + 0.05f * fdb->joint_motor_3.motor_measure->real_torque; + fdb->joint_motor_4.torque_get = 0.95f * fdb->joint_motor_4.torque_get + 0.05f * fdb->joint_motor_4.motor_measure->real_torque; + + // ------------------ Update LK Motor info ------------------ + fdb->foot_motor_L.last_position = fdb->foot_motor_L.position; + fdb->foot_motor_R.last_position = fdb->foot_motor_R.position; + fdb->foot_motor_L.position = fdb->foot_motor_L.motor_measure.ecd * MOTOR_ECD_TO_RAD;// Angle (rad) + fdb->foot_motor_R.position = fdb->foot_motor_R.motor_measure.ecd * MOTOR_ECD_TO_RAD; + + if( fdb->flag_info.init_flag != 1 ) // Init_flag = 0: after Init fdb 如果不是在init里面调用就�?�圈 + { + if( (fdb->foot_motor_L.last_position - fdb->foot_motor_L.position ) > HALF_POSITION_RANGE ) + fdb->foot_motor_L.turns++; + else if( (fdb->foot_motor_L.last_position - fdb->foot_motor_L.position ) < -HALF_POSITION_RANGE ) + fdb->foot_motor_L.turns--; + if( ( fdb->foot_motor_R.last_position - fdb->foot_motor_R.position ) > HALF_POSITION_RANGE ) + fdb->foot_motor_R.turns--; + else if( ( fdb->foot_motor_R.last_position - fdb->foot_motor_R.position ) < -HALF_POSITION_RANGE ) + fdb->foot_motor_R.turns++; + } + fdb->foot_motor_L.distance = ( fdb->foot_motor_L.position/PI2 + fdb->foot_motor_L.turns ) * WHEEL_PERIMETER - fdb->foot_motor_L.distance_offset; + fdb->foot_motor_R.distance = ( -fdb->foot_motor_R.position/PI2 + fdb->foot_motor_R.turns ) * WHEEL_PERIMETER - fdb->foot_motor_R.distance_offset; + fdb->chassis_posture_info.foot_distance = ( fdb->foot_motor_L.distance + fdb->foot_motor_R.distance ) /2.0f; + + fdb->foot_motor_L.speed = fdb->foot_motor_L.motor_measure.speed_rpm * PI2 * WHEEL_RADIUS / 10.0f / 60.0f; // rpm -> m/s + fdb->foot_motor_R.speed = -fdb->foot_motor_R.motor_measure.speed_rpm * PI2 * WHEEL_RADIUS / 10.0f / 60.0f; + fdb->chassis_posture_info.foot_speed = ( fdb->foot_motor_L.speed + fdb->foot_motor_R.speed ) / 2.0f; + + // fdb->foot_motor_L.torque_get = fdb->foot_motor_L.motor_measure.given_current; + // fdb->foot_motor_R.torque_get = fdb->foot_motor_R.motor_measure.given_current; + + // ------------------ Kalman Filter ------------------ + + kalman_second_order_update(&Body_Speed_KF, fdb->chassis_posture_info.foot_distance, fdb->chassis_posture_info.foot_speed, temp_a); + fdb->chassis_posture_info.foot_distance_K = Body_Speed_KF.x[0]; + fdb->chassis_posture_info.foot_speed_K = Body_Speed_KF.x[1]; + + // ------------------ Five_Bars_to_Pendulum ------------------ + Forward_kinematic_solution(fdb,-fdb->joint_motor_2.position,-fdb->joint_motor_2.velocity, + -fdb->joint_motor_1.position,-fdb->joint_motor_1.velocity,1); + Forward_kinematic_solution(fdb,fdb->joint_motor_3.position,fdb->joint_motor_3.velocity, + fdb->joint_motor_4.position, fdb->joint_motor_4.velocity,0); + + // ------------------ Huanzhi information update ------------------ + fdb->chassis_posture_info.leg_angle_L -= PI_2; + fdb->chassis_posture_info.leg_angle_L -= fdb->chassis_posture_info.pitch_angle; + // fdb->chassis_posture_info.leg_gyro_L -= fdb->chassis_posture_info.pitch_gyro; + fp32 temp_v_L = ( fdb->chassis_posture_info.leg_angle_L - fdb->chassis_posture_info.last_leg_angle_L ) / TASK_RUN_TIME; + fdb->chassis_posture_info.leg_gyro_L = alpha_dx * temp_v_L + (1-alpha_dx) * fdb->chassis_posture_info.leg_gyro_L; + fdb->chassis_posture_info .last_leg_angle_L = fdb->chassis_posture_info .leg_angle_L; + + fdb->chassis_posture_info.leg_angle_R -= PI_2; + fdb->chassis_posture_info.leg_angle_R -= fdb->chassis_posture_info.pitch_angle; + // fdb->chassis_posture_info.leg_gyro_R -= fdb->chassis_posture_info.pitch_gyro; + fp32 temp_v_R = ( fdb->chassis_posture_info.leg_angle_R - fdb->chassis_posture_info.last_leg_angle_R ) / TASK_RUN_TIME; + fdb->chassis_posture_info.leg_gyro_R = alpha_dx * temp_v_R + (1-alpha_dx) * fdb->chassis_posture_info.leg_gyro_R; + fdb->chassis_posture_info.last_leg_angle_R = fdb->chassis_posture_info .leg_angle_R; + + temp_v_L = ( fdb->chassis_posture_info.leg_length_L - fdb->chassis_posture_info.last_leg_length_L ) / TASK_RUN_TIME; + fdb->chassis_posture_info.leg_dlength_L = alpha_dx * temp_v_L + (1-alpha_dx) * fdb->chassis_posture_info.leg_dlength_L; + fdb->chassis_posture_info.last_leg_length_L = fdb->chassis_posture_info.leg_length_L; + + temp_v_R = ( fdb->chassis_posture_info.leg_length_R - fdb->chassis_posture_info.last_leg_length_R ) / TASK_RUN_TIME; + fdb->chassis_posture_info.leg_dlength_R = alpha_dx * temp_v_R + (1-alpha_dx) * fdb->chassis_posture_info.leg_dlength_R; + fdb->chassis_posture_info.last_leg_length_R = fdb->chassis_posture_info.leg_length_R; + + // ---------- Rotate and Move Info update ------------ + rotate_move_offset = offset_k * rotate_speed_list[robot_level]; + rc_sign = 1.0f; + X_speed = fdb->chassis_rc_ctrl->X_speed; + Y_speed = fdb->chassis_rc_ctrl->Y_speed; + // Original rc angle + if (X_speed == 0){ + if (Y_speed > 0) + rc_angle_temp = PI/2; + else if (Y_speed < 0) + rc_angle_temp = -PI/2; + else + rc_angle_temp = 0; + } + else{ + Y_speed = fdb->chassis_rc_ctrl->Y_speed; + rc_angle_temp = atan_tl(Y_speed/X_speed); + } + // Normalized speed + if (abs(X_speed) < 0.1 ||abs(Y_speed) <0.1) + temp_max_spd = 1.0f; + else if (X_speedPI/2||rc_angle_temp<-PI/2){ + rc_sign *= -1.0f; + } + while (rc_angle_temp>PI/2) { + rc_angle_temp -= PI; + } + while (rc_angle_temp<-PI/2){ + rc_angle_temp += PI; + } + // if (X_speed < 0) rc_sign = -1.0f; + rc_angle = rc_angle_temp * 180.0f / PI; + if (fdb->chassis_rc_ctrl->fake_flag == 0) fake_sign = -1.0f; + else fake_sign = 1.0f; +} + +void Chassis_Status_Detect( chassis_control_t *detect ) +{ + // ------------------ Off Ground Detect ------------------ + + + if( detect->mode.chassis_balancing_mode == BALANCING_READY && detect->mode.sport_mode!=TK_MODE) + { + if( ABS(detect->chassis_posture_info.pitch_angle) >= DANGER_PITCH_ANGLE ) + detect->flag_info.abnormal_flag = 1; + else if( ABS(detect->chassis_posture_info.foot_speed_K) < MOVE_LOWER_BOUND && + ABS(detect->chassis_posture_info.pitch_angle) < 0.1f && + detect->flag_info.abnormal_flag ) + detect->flag_info.abnormal_flag = 0; + } + if (abnormal_debug){ + detect->flag_info.abnormal_flag = 0; + } + + Supportive_Force_Cal(detect, detect->joint_motor_1.position, detect->joint_motor_2.position, 1.0f ); + Supportive_Force_Cal(detect, detect->joint_motor_3.position, detect->joint_motor_4.position, 0.0f ); + + if( detect->mode.jumping_stage == CONSTACTING_LEGS ) + detect->flag_info.suspend_flag_L = detect->flag_info.suspend_flag_R = ON_GROUND; + else + { + if( detect->mode.sport_mode == JUMPING_MODE ) + { + if( detect->chassis_posture_info.supportive_force_R <= LOWER_SUPPORT_FORCE_FOR_JUMP && + detect->chassis_posture_info.leg_length_R > 0.13f ) + detect->flag_info.suspend_flag_R = OFF_GROUND; + else + detect->flag_info.suspend_flag_L = ON_GROUND; + if( detect->chassis_posture_info.supportive_force_L <= LOWER_SUPPORT_FORCE_FOR_JUMP && + detect->chassis_posture_info.leg_length_L > 0.13f ) + detect->flag_info.suspend_flag_L = OFF_GROUND; + else + detect->flag_info.suspend_flag_R = ON_GROUND; + } + else + { + if( detect->chassis_posture_info.supportive_force_R <= LOWER_SUPPORT_FORCE && + detect->chassis_posture_info.leg_length_R > 0.13f ) + detect->flag_info.suspend_flag_R = OFF_GROUND; + else + detect->flag_info.suspend_flag_L = ON_GROUND; + if( detect->chassis_posture_info.supportive_force_L <= LOWER_SUPPORT_FORCE && + detect->chassis_posture_info.leg_length_L > 0.13f ) + detect->flag_info.suspend_flag_L = OFF_GROUND; + else + detect->flag_info.suspend_flag_R = ON_GROUND; + } + } + + + if( detect->flag_info.abnormal_flag == 1 && + ( detect->flag_info.last_suspend_flag_L == ON_GROUND || detect->flag_info.last_suspend_flag_R == ON_GROUND ) && + ( detect->flag_info.suspend_flag_L == OFF_GROUND || detect->flag_info.suspend_flag_R == OFF_GROUND ) ) + detect->flag_info.Ignore_Off_Ground = 1; + else if( detect->flag_info.abnormal_flag != 1 ) + detect->flag_info.Ignore_Off_Ground = 0; + if( detect->flag_info.Ignore_Off_Ground ) + { + detect->flag_info.suspend_flag_R = ON_GROUND; + detect->flag_info.suspend_flag_L = ON_GROUND; + } + // detect->flag_info.suspend_flag_L = detect->flag_info.suspend_flag_R = ON_GROUND; + //Moving_High_Offset + + if( ABS(detect->chassis_posture_info.foot_speed_K) > stablize_foot_speed_threshold && + ABS(detect->chassis_posture_info.yaw_gyro ) > stablize_yaw_speed_threshold ) + detect->flag_info.stablize_high_flag = 1; + +} + +void Chassis_Mode_Set( chassis_control_t *mode_set ) +{ + // ----------------- Chassis Mode Update ----------------- + if( mode_set->chassis_rc_ctrl->mode_R == 0 || toe_is_error(GIMBAL_TOE)|| toe_is_error(BM1_TOE)||toe_is_error(BM2_TOE)) + mode_set->mode.chassis_mode = DISABLE_CHASSIS; + else + mode_set->mode.chassis_mode = ENABLE_CHASSIS; + + if( mode_set->mode.chassis_mode == ENABLE_CHASSIS ) + { + mode_set->joint_motor_1.motor_mode = MOTOR_FORCE; + mode_set->joint_motor_2.motor_mode = MOTOR_FORCE; + mode_set->joint_motor_3.motor_mode = MOTOR_FORCE; + mode_set->joint_motor_4.motor_mode = MOTOR_FORCE; + mode_set->foot_motor_L.motor_mode = MOTOR_FORCE; + mode_set->foot_motor_R.motor_mode = MOTOR_FORCE; + } + else + { + if( mode_set->mode.chassis_balancing_mode == JOINT_REDUCING ) + { + mode_set->joint_motor_1.motor_mode = MOTOR_FORCE; + mode_set->joint_motor_2.motor_mode = MOTOR_FORCE; + mode_set->joint_motor_3.motor_mode = MOTOR_FORCE; + mode_set->joint_motor_4.motor_mode = MOTOR_FORCE; + mode_set->foot_motor_L.motor_mode = MOTOR_NO_FORCE; + mode_set->foot_motor_R.motor_mode = MOTOR_NO_FORCE; + } + else + { + mode_set->joint_motor_1.motor_mode = MOTOR_NO_FORCE; + mode_set->joint_motor_2.motor_mode = MOTOR_NO_FORCE; + mode_set->joint_motor_3.motor_mode = MOTOR_NO_FORCE; + mode_set->joint_motor_4.motor_mode = MOTOR_NO_FORCE; + mode_set->foot_motor_L.motor_mode = MOTOR_NO_FORCE; + mode_set->foot_motor_R.motor_mode = MOTOR_NO_FORCE; + } + + } + + // ----------------- Sport Mode Update ----------------- + if( mode_set->mode.chassis_balancing_mode == BALANCING_READY ) + { + if( mode_set->chassis_rc_ctrl->tk_flag ) + mode_set->mode.sport_mode = TK_MODE; + else if( mode_set->flag_info.abnormal_flag ) + mode_set->mode.sport_mode = ABNORMAL_MOVING_MODE; + else if( mode_set->mode.sport_mode == JUMPING_MODE && mode_set->mode.jumping_stage != FINISHED ) + mode_set->mode.sport_mode = JUMPING_MODE; + else if( mode_set->chassis_rc_ctrl->jump_flag ) + mode_set->mode.sport_mode = JUMPING_MODE; + else if( mode_set->chassis_rc_ctrl->cap_flag ) + mode_set->mode.sport_mode = CAP_MODE; + else if( mode_set->chassis_rc_ctrl->fly_flag ) + mode_set->mode.sport_mode = FLY_MODE; + else + mode_set->mode.sport_mode = NORMAL_MOVING_MODE; + } + else + mode_set->mode.sport_mode = NONE; + + // ----------------- Rotation Flag -------------------- + static uint8_t last_rotation_flag = 0; + last_rotation_flag = mode_set->flag_info.rotation_flag; + mode_set->flag_info.rotation_flag = mode_set->chassis_rc_ctrl->mode_R == 4; //Rotate + if (!last_rotation_flag && mode_set->flag_info.rotation_flag){ + for (int i = 0; i < 11; ++i) + rotate_speed_list[i] = - rotate_speed_list[i]; + } + + // ----------------- No Follow Flag ---------------------- + if (toe_is_error(GIMBAL_TOE)) no_follow_flag = 1; + else no_follow_flag = mode_set->chassis_rc_ctrl->mode_R == 3; //No Follow + + // ---------------- Moving Flag ---------------------------- + if( mode_set->chassis_rc_ctrl->X_speed != 0.0f ) + { + mode_set->flag_info.controlling_flag = 1; + mode_set->flag_info.set_pos_after_moving = 1; + } + else + mode_set->flag_info.controlling_flag = 0; + + if( ABS(mode_set->chassis_posture_info.foot_speed_K) <= 0.05f && + !mode_set->flag_info.controlling_flag && + mode_set->flag_info.set_pos_after_moving ) // maybe bug + { + mode_set->flag_info.moving_flag = 0; + mode_set->flag_info.set_pos_after_moving = 0; + } + else + mode_set->flag_info.moving_flag = 1; + + +} + +uint8_t reduce_flag = 0; +fp32 reduce_high, high_offset = 0.2f; +fp32 debug_1 = 0.995; +void Chassis_Mode_Change_Control_Transit( chassis_control_t *chassis_mode_change ) +{ + if( chassis_mode_change->mode.chassis_mode == ENABLE_CHASSIS && chassis_mode_change->mode.last_chassis_mode == DISABLE_CHASSIS ) + chassis_mode_change->mode.chassis_balancing_mode = FOOT_LAUNCHING; + + if( chassis_mode_change->mode.chassis_balancing_mode == FOOT_LAUNCHING && + ABS(chassis_mode_change->chassis_posture_info.pitch_angle) < EXIT_PITCH_ANGLE ) + chassis_mode_change->mode.chassis_balancing_mode = JOINT_LAUNCHING; + else if( chassis_mode_change->mode.chassis_balancing_mode == JOINT_LAUNCHING && ( + (NORMAL_HIGH - chassis_mode_change->chassis_posture_info.leg_length_L) < 0.03f || + (NORMAL_HIGH - chassis_mode_change->chassis_posture_info.leg_length_R) < 0.03f) ) + chassis_mode_change->mode.chassis_balancing_mode = BALANCING_READY; + else if( chassis_mode_change->mode.chassis_balancing_mode == BALANCING_READY && + chassis_mode_change->mode.chassis_mode == DISABLE_CHASSIS ){ + if (!reduce_flag) { + reduce_high = chassis_mode_change->chassis_posture_info.ideal_high + high_offset; + reduce_flag = 1; + } + chassis_mode_change->mode.chassis_balancing_mode = JOINT_REDUCING; + } + else if( chassis_mode_change->mode.chassis_balancing_mode == JOINT_REDUCING && ( + ABS( chassis_mode_change->chassis_posture_info.leg_length_L - SIT_HIGH ) < 0.001f || + ABS( chassis_mode_change->chassis_posture_info.leg_length_R - SIT_HIGH ) < 0.001f ) ){ + chassis_mode_change->mode.chassis_balancing_mode = NO_FORCE; + reduce_flag = 0; + } + + if( chassis_mode_change->mode.sport_mode == JUMPING_MODE && chassis_mode_change->mode.last_sport_mode != JUMPING_MODE ) + { + chassis_mode_change->mode.jumping_mode = STANDING_JUMP; + } + else if( chassis_mode_change->mode.sport_mode != JUMPING_MODE ) + chassis_mode_change->mode.jumping_mode = NOT_DEFINE; + + + if( chassis_mode_change->mode.jumping_mode == MOVING_JUMP ) + { + if( chassis_mode_change->mode.jumping_stage == READY_TO_JUMP ) + chassis_mode_change->mode.jumping_stage = CONSTACTING_LEGS; + else if( chassis_mode_change->mode.jumping_stage == CONSTACTING_LEGS && + chassis_mode_change->chassis_posture_info.leg_length_L <= 0.13f ) + chassis_mode_change->mode.jumping_stage = EXTENDING_LEGS; + else if( chassis_mode_change->mode.jumping_stage == EXTENDING_LEGS && + chassis_mode_change->chassis_posture_info.leg_length_L >= 0.30f ) + chassis_mode_change->mode.jumping_stage = CONSTACTING_LEGS_2; + else if( chassis_mode_change->mode.jumping_stage == CONSTACTING_LEGS_2 && + chassis_mode_change->chassis_posture_info.leg_length_L <= 0.13f ) + chassis_mode_change->mode.jumping_stage = PREPARING_LANDING; + else if( chassis_mode_change->mode.jumping_stage == PREPARING_LANDING && + chassis_mode_change->flag_info.suspend_flag_R == ON_GROUND && + chassis_mode_change->flag_info.suspend_flag_L == ON_GROUND ) + chassis_mode_change->mode.jumping_stage = FINISHED; + else if( chassis_mode_change->mode.jumping_stage == FINISHED ) + chassis_mode_change->mode.jumping_stage = READY_TO_JUMP; + } + else if( chassis_mode_change->mode.jumping_mode == STANDING_JUMP ) + { + if( chassis_mode_change->mode.jumping_stage == READY_TO_JUMP ) + chassis_mode_change->mode.jumping_stage = PREPARING_STAND_JUMPING; + else if(chassis_mode_change->mode.jumping_stage == PREPARING_STAND_JUMPING && + ABS(chassis_mode_change->chassis_posture_info.leg_angle_L + IDEAL_PREPARING_STAND_JUMPING_ANGLE ) < stepp ) + chassis_mode_change->mode.jumping_stage = EXTENDING_LEGS; + else if( chassis_mode_change->mode.jumping_stage == EXTENDING_LEGS && + chassis_mode_change->chassis_posture_info.leg_length_L >= 0.30f ) + chassis_mode_change->mode.jumping_stage = CONSTACTING_LEGS_2; + else if( chassis_mode_change->mode.jumping_stage == CONSTACTING_LEGS_2 && + chassis_mode_change->chassis_posture_info.leg_length_L <= 0.13f ) + chassis_mode_change->mode.jumping_stage = PREPARING_LANDING; + else if( chassis_mode_change->mode.jumping_stage == PREPARING_LANDING && + chassis_mode_change->flag_info.suspend_flag_R == ON_GROUND && + chassis_mode_change->flag_info.suspend_flag_L == ON_GROUND ) + chassis_mode_change->mode.jumping_stage = FINISHED; + else if( chassis_mode_change->mode.jumping_stage == FINISHED ) + chassis_mode_change->mode.jumping_stage = READY_TO_JUMP; + } + else + chassis_mode_change->mode.jumping_stage = FINISHED; + + + if( chassis_mode_change->flag_info.stablize_high_flag == 1 && + Moving_High_Offset >= 0.2 && + chassis_mode_change->chassis_posture_info.yaw_gyro <= ( stablize_yaw_speed_threshold - 0.5f ) ) + chassis_mode_change->flag_info.stablize_high_flag = 0; +} + +fp32 HIGH_SWITCH = 36.0f; + +void Target_Value_Set( chassis_control_t *target_value_set ) +{ + // ------------------------------------ + if( target_value_set->flag_info.stablize_high_flag == 1 ) + if( Moving_High_Offset < 0.2 ) + Moving_High_Offset += 0.001f; + + if( target_value_set->flag_info.stablize_high_flag == 0 ) + Moving_High_Offset = 0.0f; + + // --------- X Speed Set ---------- + if( target_value_set->mode.sport_mode != NONE && + target_value_set->flag_info.suspend_flag_L == ON_GROUND && + target_value_set->flag_info.suspend_flag_R == ON_GROUND ) + { + if (!target_value_set->flag_info.rotation_flag) {// Normal move + if(toe_is_error(REFEREE_TOE)){ + target_value_set->chassis_posture_info.foot_speed_set = fake_sign * target_value_set->chassis_rc_ctrl->X_speed * normal_move_scale ; + + } else { + // target_speed_sign = (fake_sign * target_value_set->chassis_rc_ctrl->X_speed * move_scale_list[robot_level]) > 0 ? 1: -1; + target_speed_sign = SIGN(fake_sign * target_value_set->chassis_rc_ctrl->X_speed * move_scale_list[robot_level]); + target_value_set->chassis_posture_info.foot_speed_set = target_value_set->chassis_posture_info.foot_speed_K + target_speed_sign * acc_step; + + if (target_value_set->mode.sport_mode == FLY_MODE) + target_value_set->chassis_posture_info.foot_speed_set = target_speed_sign * min(ABS(target_value_set->chassis_rc_ctrl->X_speed * fly_speed),ABS(target_value_set->chassis_posture_info.foot_speed_set)); + else if (target_value_set->mode.sport_mode == CAP_MODE) + target_value_set->chassis_posture_info.foot_speed_set = target_speed_sign * min(ABS(target_value_set->chassis_rc_ctrl->X_speed * cap_move_scale_list[robot_level]),ABS(target_value_set->chassis_posture_info.foot_speed_set)); + else + target_value_set->chassis_posture_info.foot_speed_set = target_speed_sign * min(ABS(target_value_set->chassis_rc_ctrl->X_speed * move_scale_list[robot_level]),ABS(target_value_set->chassis_posture_info.foot_speed_set)); + + // if( target_value_set->chassis_posture_info.leg_length_L > 0.22f || target_value_set->chassis_posture_info.leg_length_R > 0.22f ) + // { + // target_value_set->chassis_posture_info.foot_speed_set /= 2.0f; + // debug_2++; + // } + } + + } + else // Rotate and move + { + delta_theta_temp = target_value_set->chassis_rc_ctrl->W_angle - rc_angle; + if (delta_theta_temp>PI/2) delta_theta_temp -= PI; + else if (delta_theta_temp<-PI/2) delta_theta_temp += PI; + delta_theta = abs(delta_theta_temp); + if (delta_thetachassis_posture_info.foot_speed_set + = ((rotate_move_threshold-delta_theta)/rotate_move_threshold) + * rc_sign * fake_sign * normalized_speed * rotate_move_scale_list[robot_level]; + else target_value_set->chassis_posture_info.foot_speed_set = 0; + } + + } + else + target_value_set->chassis_posture_info.foot_speed_set = 0; + + // --------- Distance Set --------- + if( target_value_set->mode.chassis_balancing_mode == NO_FORCE || target_value_set->mode.sport_mode == TK_MODE) + target_value_set->chassis_posture_info.foot_distance_set = target_value_set->chassis_posture_info.foot_distance_K; + else if( target_value_set->mode.sport_mode == ABNORMAL_MOVING_MODE ) + target_value_set->chassis_posture_info.foot_distance_set = target_value_set->chassis_posture_info.foot_distance_set; + else if( target_value_set->flag_info.suspend_flag_R == OFF_GROUND || + target_value_set->flag_info.suspend_flag_L == OFF_GROUND ) + target_value_set->chassis_posture_info.foot_distance_set = target_value_set->chassis_posture_info.foot_distance_K; + else if( target_value_set->flag_info.controlling_flag ) + target_value_set->chassis_posture_info.foot_distance_set = target_value_set->chassis_posture_info.foot_distance_K; + else if( !target_value_set->flag_info.moving_flag && + target_value_set->mode.chassis_balancing_mode == BALANCING_READY && + target_value_set->flag_info.last_moving_flag ) + target_value_set->chassis_posture_info.foot_distance_set = target_value_set->chassis_posture_info.foot_distance_K; + + + + // --------- Y Speed & Angle Set --------- + if( target_value_set->mode.sport_mode != NONE && + target_value_set->flag_info.suspend_flag_L == ON_GROUND && + target_value_set->flag_info.suspend_flag_R == ON_GROUND ) + { + if( target_value_set->flag_info.rotation_flag == 1 ) + { + target_value_set->chassis_posture_info.yaw_angle_sett = target_value_set->chassis_posture_info.yaw_angle; + target_value_set->chassis_posture_info.yaw_gyro_set = rotate_speed_list[robot_level]; + } + else + { + if (target_value_set->chassis_rc_ctrl->W_angle<-FOLLOW_DEADBAND) + target_value_set->chassis_posture_info.yaw_angle_sett = target_value_set->chassis_posture_info.yaw_angle + (target_value_set->chassis_rc_ctrl->W_angle+FOLLOW_DEADBAND) / 180.0f * PI; + else if (target_value_set->chassis_rc_ctrl->W_angle>FOLLOW_DEADBAND) + target_value_set->chassis_posture_info.yaw_angle_sett = target_value_set->chassis_posture_info.yaw_angle + (target_value_set->chassis_rc_ctrl->W_angle-FOLLOW_DEADBAND) / 180.0f * PI; + else + target_value_set->chassis_posture_info.yaw_angle_sett = target_value_set->chassis_posture_info.yaw_angle; + target_value_set->chassis_posture_info.yaw_gyro_set = 0.0f; + } + } + else + { + target_value_set->chassis_posture_info.yaw_angle_sett = target_value_set->chassis_posture_info.yaw_angle; + target_value_set->chassis_posture_info.yaw_gyro_set = target_value_set->chassis_posture_info.yaw_gyro; + } + + + + // --------- Side Angle Set --------- + // if( target_value_set->mode.sport_mode != NONE && + // target_value_set->mode.sport_mode != ABNORMAL_MOVING_MODE && + // target_value_set->flag_info.suspend_flag_R == ON_GROUND && + // target_value_set->flag_info.suspend_flag_L == ON_GROUND ) + // { + // if( target_value_set->mode.sport_mode == SIDE_MODE ) + // target_value_set->chassis_posture_info.roll_angle_set = (fp32)(5.0 / 180.0 * PI) * 50.0f; + // // else if( target_value_set->chassis_rc_ctrl->side_flag == -1 ) + // // target_value_set->chassis_posture_info.roll_angle_set = -(fp32)(5.0 / 180.0 * PI) * 50.0f; + // else + // target_value_set->chassis_posture_info.roll_angle_set = 0.0f; + // } + // else + target_value_set->chassis_posture_info.roll_angle_set = 0.0f; + + // ---------------- Leg Angle Set ---------- + if( target_value_set->mode.jumping_stage == PREPARING_STAND_JUMPING ) + { + target_value_set->chassis_posture_info.leg_angle_L_set = -IDEAL_PREPARING_STAND_JUMPING_ANGLE; + target_value_set->chassis_posture_info.leg_angle_R_set = -IDEAL_PREPARING_STAND_JUMPING_ANGLE; + } + else + { + target_value_set->chassis_posture_info.leg_angle_L_set = 0.0f; + target_value_set->chassis_posture_info.leg_angle_R_set = 0.0f; + } + + + // ----------------- Chassis High Mode Update ----------------- + if( target_value_set->mode.sport_mode == ABNORMAL_MOVING_MODE || target_value_set->mode.sport_mode == TK_MODE) + target_value_set->mode.chassis_high_mode = SIT_MODE; + else if( target_value_set->mode.chassis_balancing_mode == FOOT_LAUNCHING ) + target_value_set->mode.chassis_high_mode = SIT_MODE; + else if( target_value_set->mode.chassis_balancing_mode == JOINT_LAUNCHING ) + target_value_set->mode.chassis_high_mode = NORMAL_MODE; + else if( target_value_set->mode.chassis_balancing_mode == JOINT_REDUCING ) + target_value_set->mode.chassis_high_mode = CHANGING_HIGH; + else if( target_value_set->chassis_rc_ctrl->sit_flag ) + target_value_set->mode.chassis_high_mode = SIT_MODE; + else if( target_value_set->chassis_rc_ctrl->high_flag ) + target_value_set->mode.chassis_high_mode = HIGH_MODE; + else if( target_value_set->mode.jumping_stage == CONSTACTING_LEGS ) + target_value_set->mode.chassis_high_mode = SIT_MODE; + else if( target_value_set->mode.jumping_stage == EXTENDING_LEGS ) + target_value_set->mode.chassis_high_mode = HIGH_MODE; + else if( target_value_set->mode.jumping_stage == CONSTACTING_LEGS_2 ) + target_value_set->mode.chassis_high_mode = SIT_MODE; + else if( target_value_set->mode.jumping_stage == PREPARING_LANDING ) + target_value_set->mode.chassis_high_mode = NORMAL_MODE; + else + target_value_set->mode.chassis_high_mode = NORMAL_MODE; + + // --------- Leg Length Set --------- + if( target_value_set->mode.chassis_high_mode == SIT_MODE ) + target_value_set->chassis_posture_info.ideal_high = SIT_HIGH; + else if( target_value_set->mode.chassis_high_mode == NORMAL_MODE ) + target_value_set->chassis_posture_info.ideal_high = NORMAL_HIGH + High_Offset - Moving_High_Offset; + else if( target_value_set->mode.chassis_high_mode == HIGH_MODE ) + target_value_set->chassis_posture_info.ideal_high = HIGH_HIGH + High_Offset - Moving_High_Offset; + else if( target_value_set->mode.chassis_high_mode == CHANGING_HIGH ) + { + reduce_high = reduce_high * debug_1; + target_value_set->chassis_posture_info.ideal_high = min(reduce_high,target_value_set->chassis_posture_info.ideal_high); + // target_value_set->chassis_posture_info.ideal_high = + // debug_1 * target_value_set->chassis_posture_info.ideal_high; + } + + if( target_value_set->mode.sport_mode == ABNORMAL_MOVING_MODE || + ( target_value_set->flag_info.suspend_flag_L == 1 && target_value_set->flag_info.suspend_flag_R == 1 ) || + target_value_set->chassis_posture_info.ideal_high == SIT_HIGH || + target_value_set->mode.chassis_balancing_mode == JOINT_REDUCING ) + { + target_value_set->chassis_posture_info.leg_length_L_set = target_value_set->chassis_posture_info.ideal_high; + target_value_set->chassis_posture_info.leg_length_R_set = target_value_set->chassis_posture_info.ideal_high; + } + else + { + target_value_set->chassis_posture_info.foot_roll_angle = + target_value_set->chassis_posture_info.roll_angle + + atan(( target_value_set->chassis_posture_info.leg_length_L - target_value_set->chassis_posture_info.leg_length_R ) / 0.50f ); + + target_value_set->chassis_posture_info.leg_length_L_set = + target_value_set->chassis_posture_info .ideal_high + + 0.25f * arm_sin_f32( target_value_set->chassis_posture_info .foot_roll_angle ) / arm_cos_f32( target_value_set->chassis_posture_info .foot_roll_angle ); + target_value_set->chassis_posture_info.leg_length_R_set = + target_value_set->chassis_posture_info .ideal_high + - 0.25f * arm_sin_f32( target_value_set->chassis_posture_info .foot_roll_angle ) / arm_cos_f32( target_value_set->chassis_posture_info .foot_roll_angle ); + } + +} + +void Chassis_Torque_Calculation(chassis_control_t *bl_ctrl) +{ + + LQR_Data_Update(bl_ctrl); + // -----Roll Balance----- + if( bl_ctrl->flag_info.suspend_flag_R == 1 || bl_ctrl->flag_info.suspend_flag_L == 1 || + bl_ctrl->mode.chassis_high_mode == SIT_MODE ) + { + bl_ctrl->torque_info.joint_roll_torque_R = 0.0f; + bl_ctrl->torque_info.joint_roll_torque_L = 0.0f; + } + else + { + if (bl_ctrl->chassis_posture_info.roll_angle < -roll_angle_deadband) { + rollP = roll_PD[0] * (bl_ctrl->chassis_posture_info.roll_angle_set - (bl_ctrl->chassis_posture_info.roll_angle+roll_angle_deadband)); + } + else if (bl_ctrl->chassis_posture_info.roll_angle > roll_angle_deadband){ + rollP = roll_PD[0] * (bl_ctrl->chassis_posture_info.roll_angle_set - (bl_ctrl->chassis_posture_info.roll_angle-roll_angle_deadband)); + } + else { + rollP = 0.0f; + } + + if (bl_ctrl->chassis_posture_info.roll_gyro < -roll_gyro_deadband) { + rollD = roll_PD[1] * - (bl_ctrl->chassis_posture_info.roll_gyro + roll_gyro_deadband); + } + else if (bl_ctrl->chassis_posture_info.roll_gyro > roll_gyro_deadband){ + rollD = roll_PD[1] * - (bl_ctrl->chassis_posture_info.roll_gyro - roll_gyro_deadband); + } + else { + rollD = 0.0f; + } + bl_ctrl->torque_info.joint_roll_torque_R = rollP + rollD; + bl_ctrl->torque_info.joint_roll_torque_L = -bl_ctrl->torque_info.joint_roll_torque_R; + } + + + // -----During turns: prevent displacement of two legs----- + bl_ctrl->torque_info.joint_prevent_splits_torque_L = + coordinate_PD[0] * (bl_ctrl->chassis_posture_info.leg_angle_L - bl_ctrl->chassis_posture_info.leg_angle_R) + + coordinate_PD[1] * (bl_ctrl->chassis_posture_info.leg_gyro_L - bl_ctrl->chassis_posture_info.leg_gyro_R); + + bl_ctrl->torque_info.joint_prevent_splits_torque_R = -bl_ctrl->torque_info.joint_prevent_splits_torque_L; + + if( bl_ctrl->mode.jumping_stage == EXTENDING_LEGS || + bl_ctrl->mode.jumping_stage == CONSTACTING_LEGS_2 ) + { + bl_ctrl->torque_info.joint_stand_torque_L = + + jump_stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_L_set - bl_ctrl->chassis_posture_info.leg_length_L ) + + jump_stand_PD[1] * ( 0 - bl_ctrl->chassis_posture_info.leg_dlength_L ); + + bl_ctrl->torque_info.joint_stand_torque_R = + + jump_stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_R_set - bl_ctrl->chassis_posture_info.leg_length_R ) + + jump_stand_PD[1] * ( 0 - bl_ctrl->chassis_posture_info.leg_dlength_R ); + } + else if( bl_ctrl->mode.sport_mode == ABNORMAL_MOVING_MODE ) + { + bl_ctrl->torque_info.joint_stand_torque_L = + + suspend_stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_L_set - bl_ctrl->chassis_posture_info.leg_length_L ) + + suspend_stand_PD[1] * ( 0.0f - bl_ctrl->chassis_posture_info.leg_dlength_L ); + bl_ctrl->torque_info.joint_stand_torque_R = + + suspend_stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_R_set - bl_ctrl->chassis_posture_info.leg_length_R ) + + suspend_stand_PD[1] * ( 0.0f - bl_ctrl->chassis_posture_info.leg_dlength_R ); + } + else if( bl_ctrl->flag_info.suspend_flag_R == 1 || bl_ctrl->flag_info.suspend_flag_L == 1 ) + { + if( bl_ctrl->flag_info.suspend_flag_L == 1 ) + { + bl_ctrl->torque_info.joint_stand_torque_L = + + suspend_stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_L_set - bl_ctrl->chassis_posture_info.leg_length_L ) + + suspend_stand_PD[1] * ( 0.0f - bl_ctrl->chassis_posture_info.leg_dlength_L ); + } + else{ + bl_ctrl->torque_info.joint_stand_torque_L = + FEED_f + + stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_L_set - bl_ctrl->chassis_posture_info.leg_length_L ) + + stand_PD[1] * ( 0 - bl_ctrl->chassis_posture_info.leg_dlength_L ); + } + + if( bl_ctrl->flag_info.suspend_flag_R == 1 ) + { + bl_ctrl->torque_info.joint_stand_torque_R = + + suspend_stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_R_set - bl_ctrl->chassis_posture_info.leg_length_R ) + + suspend_stand_PD[1] * ( 0.0f - bl_ctrl->chassis_posture_info.leg_dlength_R ); + } else { + bl_ctrl->torque_info.joint_stand_torque_R = + FEED_f + + stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_R_set - bl_ctrl->chassis_posture_info.leg_length_R ) + + stand_PD[1] * ( 0 - bl_ctrl->chassis_posture_info.leg_dlength_R ); + } + } + else if( bl_ctrl->mode.chassis_balancing_mode == JOINT_REDUCING ) + { + + bl_ctrl->torque_info.joint_stand_torque_L = + + suspend_stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_L_set - bl_ctrl->chassis_posture_info.leg_length_L ) + + suspend_stand_PD[1] * ( 0.0f - bl_ctrl->chassis_posture_info.leg_dlength_L ); + + bl_ctrl->torque_info.joint_stand_torque_R = + + suspend_stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_R_set - bl_ctrl->chassis_posture_info.leg_length_R ) + + suspend_stand_PD[1] * ( 0.0f - bl_ctrl->chassis_posture_info.leg_dlength_R ); + } + else + { + bl_ctrl->torque_info.joint_stand_torque_L = FEED_f; + bl_ctrl->torque_info.joint_stand_torque_L += stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_L_set - bl_ctrl->chassis_posture_info.leg_length_L ); + if (bl_ctrl->chassis_posture_info.leg_dlength_L > leg_dlength_deadband){ + bl_ctrl->torque_info.joint_stand_torque_L += stand_PD[1] * ( 0 - (bl_ctrl->chassis_posture_info.leg_dlength_L-leg_dlength_deadband) ); + } + else if (bl_ctrl->chassis_posture_info.leg_dlength_L < -leg_dlength_deadband){ + bl_ctrl->torque_info.joint_stand_torque_L += stand_PD[1] * ( 0 - (bl_ctrl->chassis_posture_info.leg_dlength_L+leg_dlength_deadband)); + } + else { + bl_ctrl->torque_info.joint_stand_torque_L += 0.0f; + } + bl_ctrl->torque_info.joint_stand_torque_R = FEED_f; + bl_ctrl->torque_info.joint_stand_torque_R += stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_R_set - bl_ctrl->chassis_posture_info.leg_length_R ); + if (bl_ctrl->chassis_posture_info.leg_dlength_R > leg_dlength_deadband){ + bl_ctrl->torque_info.joint_stand_torque_R += stand_PD[1] * ( 0 - (bl_ctrl->chassis_posture_info.leg_dlength_R-leg_dlength_deadband) ); + } + else if (bl_ctrl->chassis_posture_info.leg_dlength_R < -leg_dlength_deadband){ + bl_ctrl->torque_info.joint_stand_torque_R += stand_PD[1] * ( 0 - (bl_ctrl->chassis_posture_info.leg_dlength_R+leg_dlength_deadband)); + } + else { + bl_ctrl->torque_info.joint_stand_torque_R += 0.0f; + } + + } + // --------------------- joint motor calucaltion --------------------- + + + // TODDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDO + if( bl_ctrl->mode.jumping_stage == CONSTACTING_LEGS_2 ) + { + bl_ctrl->torque_info.joint_balancing_torque_L = + -suspend_LQR[0][0] * ( 0 - bl_ctrl->chassis_posture_info.leg_angle_L ) + -suspend_LQR[0][1] * ( 0 - bl_ctrl->chassis_posture_info.leg_gyro_L ); + bl_ctrl->torque_info.joint_balancing_torque_R = + -suspend_LQR[0][0] * ( 0 - bl_ctrl->chassis_posture_info.leg_angle_R ) + -suspend_LQR[0][1] * ( 0 - bl_ctrl->chassis_posture_info.leg_gyro_R ); + + bl_ctrl->torque_info.joint_moving_torque_L = 0.0f; + bl_ctrl->torque_info.joint_moving_torque_R = 0.0f; + } + else + { + if( bl_ctrl->mode.sport_mode == ABNORMAL_MOVING_MODE || bl_ctrl->mode.sport_mode == TK_MODE ) + { + bl_ctrl->torque_info.joint_balancing_torque_L = + bl_ctrl->torque_info.joint_moving_torque_L = + bl_ctrl->torque_info.joint_balancing_torque_R = + bl_ctrl->torque_info.joint_moving_torque_R = 0; + } + else + { + if( bl_ctrl->mode.chassis_high_mode == SIT_MODE || + bl_ctrl->mode.chassis_balancing_mode == JOINT_REDUCING ) + { + bl_ctrl->torque_info.joint_balancing_torque_L = + bl_ctrl->torque_info.joint_moving_torque_L = + bl_ctrl->torque_info.joint_balancing_torque_R = + bl_ctrl->torque_info.joint_moving_torque_R = 0; + } + else + { + + bl_ctrl->torque_info.joint_balancing_torque_L = ( + -LQR[2][4] * ( bl_ctrl->chassis_posture_info.leg_angle_L_set - bl_ctrl->chassis_posture_info.leg_angle_L ) + -LQR[2][5] * ( 0.0f - bl_ctrl->chassis_posture_info.leg_gyro_L ) + -LQR[2][8] * ( bl_ctrl->chassis_posture_info.pitch_angle_set - bl_ctrl->chassis_posture_info.pitch_angle ) + -LQR[2][9] * ( bl_ctrl->chassis_posture_info.pitch_gyro_set - bl_ctrl->chassis_posture_info.pitch_gyro ) ); + bl_ctrl->torque_info.joint_moving_torque_L = ( + +LQR[2][0] * ( bl_ctrl->chassis_posture_info.foot_distance_set - bl_ctrl->chassis_posture_info.foot_distance_K + NORMAL_MODE_WEIGHT_DISTANCE_OFFSET) + +LQR[2][1] * ( bl_ctrl->chassis_posture_info.foot_speed_set - bl_ctrl->chassis_posture_info.foot_speed_K ) + +LQR[2][2] * ( bl_ctrl->chassis_posture_info.yaw_angle_sett - bl_ctrl->chassis_posture_info.yaw_angle ) + +LQR[2][3] * ( bl_ctrl->chassis_posture_info.yaw_gyro_set - bl_ctrl->chassis_posture_info.yaw_gyro ) + ); + + bl_ctrl->torque_info.joint_balancing_torque_R = ( + -LQR[3][6] * ( bl_ctrl->chassis_posture_info.leg_angle_R_set - bl_ctrl->chassis_posture_info.leg_angle_R ) + -LQR[3][7] * ( 0.0f - bl_ctrl->chassis_posture_info.leg_gyro_R ) + -LQR[3][8] * ( bl_ctrl->chassis_posture_info.pitch_angle_set - bl_ctrl->chassis_posture_info.pitch_angle ) + -LQR[3][9] * ( bl_ctrl->chassis_posture_info.pitch_gyro_set - bl_ctrl->chassis_posture_info.pitch_gyro ) ); + bl_ctrl->torque_info.joint_moving_torque_R = ( + +LQR[3][0] * ( bl_ctrl->chassis_posture_info.foot_distance_set - bl_ctrl->chassis_posture_info.foot_distance_K + NORMAL_MODE_WEIGHT_DISTANCE_OFFSET) + +LQR[3][1] * ( bl_ctrl->chassis_posture_info.foot_speed_set - bl_ctrl->chassis_posture_info.foot_speed_K ) + +LQR[3][2] * ( bl_ctrl->chassis_posture_info.yaw_angle_sett - bl_ctrl->chassis_posture_info.yaw_angle ) + +LQR[3][3] * ( bl_ctrl->chassis_posture_info.yaw_gyro_set - bl_ctrl->chassis_posture_info.yaw_gyro ) + ); + } + } + } + + // --------------------- Foot motor LQR --------------------- + if ( bl_ctrl->mode.sport_mode == TK_MODE ) { + bl_ctrl->torque_info.foot_balancing_torque_L = 0.0f; + bl_ctrl->torque_info.foot_balancing_torque_R = 0.0f; + bl_ctrl->torque_info.foot_moving_torque_L = (int) ( + -TK_x_p*( bl_ctrl->chassis_posture_info.foot_distance_set - bl_ctrl->chassis_posture_info.foot_speed_K ) + -TK_y_p*( bl_ctrl->chassis_posture_info.foot_speed_set - bl_ctrl->chassis_posture_info.yaw_angle ) + -TK_y_d*( 0.0f - bl_ctrl->chassis_posture_info.yaw_gyro ) + )* TORQ_K; + bl_ctrl->torque_info.foot_moving_torque_R = (int)-( + -TK_x_p*( bl_ctrl->chassis_posture_info.foot_distance_set - bl_ctrl->chassis_posture_info.foot_speed_K ) + +TK_y_p*( bl_ctrl->chassis_posture_info.foot_speed_set - bl_ctrl->chassis_posture_info.yaw_angle ) + +TK_y_d*( 0.0f - bl_ctrl->chassis_posture_info.yaw_gyro ) + )* TORQ_K; + } + else { + bl_ctrl->torque_info.foot_balancing_torque_L = (int) ( + -LQR[0][4]*( bl_ctrl->chassis_posture_info.leg_angle_L_set - bl_ctrl->chassis_posture_info.leg_angle_L) + -LQR[0][5]*( 0.0f - bl_ctrl->chassis_posture_info.leg_gyro_L) + +LQR[0][8]*( bl_ctrl->chassis_posture_info.pitch_angle_set - bl_ctrl->chassis_posture_info.pitch_angle) + +LQR[0][9]*( bl_ctrl->chassis_posture_info.pitch_gyro_set - bl_ctrl->chassis_posture_info.pitch_gyro) + )* TORQ_K; + + bl_ctrl->torque_info.foot_moving_torque_L = (int) ( + -LQR[0][0]*( bl_ctrl->chassis_posture_info.foot_distance_set - bl_ctrl->chassis_posture_info.foot_distance_K + NORMAL_MODE_WEIGHT_DISTANCE_OFFSET) + -LQR[0][1]*( bl_ctrl->chassis_posture_info.foot_speed_set - bl_ctrl->chassis_posture_info.foot_speed_K) + -LQR[0][2]*( bl_ctrl->chassis_posture_info.yaw_angle_sett - bl_ctrl->chassis_posture_info.yaw_angle) + -LQR[0][3]*( bl_ctrl->chassis_posture_info.yaw_gyro_set - bl_ctrl->chassis_posture_info.yaw_gyro ) + )* TORQ_K; + + bl_ctrl->torque_info.foot_balancing_torque_R = (int) -( + -LQR[1][6]*( bl_ctrl->chassis_posture_info.leg_angle_R_set - bl_ctrl->chassis_posture_info.leg_angle_R) + -LQR[1][7]*( 0.0f - bl_ctrl->chassis_posture_info.leg_gyro_R) + +LQR[1][8]*( bl_ctrl->chassis_posture_info.pitch_angle_set - bl_ctrl->chassis_posture_info.pitch_angle) + +LQR[1][9]*( bl_ctrl->chassis_posture_info.pitch_gyro_set - bl_ctrl->chassis_posture_info.pitch_gyro) + )* TORQ_K; + + bl_ctrl->torque_info.foot_moving_torque_R = (int) -( + -LQR[1][0]*( bl_ctrl->chassis_posture_info.foot_distance_set - bl_ctrl->chassis_posture_info.foot_distance_K + NORMAL_MODE_WEIGHT_DISTANCE_OFFSET) + -LQR[1][1]*( bl_ctrl->chassis_posture_info.foot_speed_set - bl_ctrl->chassis_posture_info.foot_speed_K) + +LQR[1][2]*( bl_ctrl->chassis_posture_info.yaw_angle_sett - bl_ctrl->chassis_posture_info.yaw_angle) + +LQR[1][3]*( bl_ctrl->chassis_posture_info.yaw_gyro_set - bl_ctrl->chassis_posture_info.yaw_gyro ) + )* TORQ_K; + } + + if( bl_ctrl->flag_info.suspend_flag_R == 1 ) + { + bl_ctrl->torque_info.joint_balancing_torque_R = + -suspend_LQR[0][0] * ( 0.0f - bl_ctrl->chassis_posture_info.leg_angle_R ) + -suspend_LQR[0][1] * ( 0 - bl_ctrl->chassis_posture_info.leg_gyro_R ); + + bl_ctrl->torque_info.foot_moving_torque_R = + -suspend_foot_speed_p * ( 0.0f - bl_ctrl->foot_motor_R.speed ); + + bl_ctrl->torque_info.joint_moving_torque_R = 0.0f; + bl_ctrl->torque_info.foot_balancing_torque_R = 0.0f; + } + if( bl_ctrl->flag_info.suspend_flag_L == 1 ) + { + bl_ctrl->torque_info.joint_balancing_torque_L = + -suspend_LQR[0][0] * ( 0.0f - bl_ctrl->chassis_posture_info.leg_angle_L ) + -suspend_LQR[0][1] * ( 0 - bl_ctrl->chassis_posture_info.leg_gyro_L ); + bl_ctrl->torque_info.foot_moving_torque_L = + +suspend_foot_speed_p * ( 0.0f - bl_ctrl->foot_motor_L.speed ); + + bl_ctrl->torque_info.joint_moving_torque_L = 0.0f; + bl_ctrl->torque_info.foot_balancing_torque_L = 0.0f; + } + + + // LimitMax( bl_ctrl->torque_info.foot_moving_torque_L, MAX_ACCL ); + // LimitMax( bl_ctrl->torque_info.foot_moving_torque_R, MAX_ACCL ); + LimitMax( bl_ctrl->torque_info.joint_moving_torque_L, MAX_ACCL_JOINT ); + LimitMax( bl_ctrl->torque_info.joint_moving_torque_R, MAX_ACCL_JOINT ); + +} + +void Chassis_Torque_Combine(chassis_control_t *bl_ctrl) +{ + bl_ctrl->mapping_info .J1_L = VMC_solve_J1(bl_ctrl->chassis_posture_info .leg_angle_L ,bl_ctrl->chassis_posture_info .leg_length_L ); + bl_ctrl->mapping_info .J2_L = VMC_solve_J2(bl_ctrl->chassis_posture_info .leg_angle_L ,bl_ctrl->chassis_posture_info .leg_length_L); + bl_ctrl->mapping_info .J3_L = VMC_solve_J3(bl_ctrl->chassis_posture_info .leg_angle_L ,bl_ctrl->chassis_posture_info .leg_length_L); + bl_ctrl->mapping_info .J4_L = VMC_solve_J4(bl_ctrl->chassis_posture_info .leg_angle_L ,bl_ctrl->chassis_posture_info .leg_length_L ); + bl_ctrl->mapping_info .J1_R = VMC_solve_J1(bl_ctrl->chassis_posture_info .leg_angle_R ,bl_ctrl->chassis_posture_info .leg_length_R ); + bl_ctrl->mapping_info .J2_R = VMC_solve_J2(bl_ctrl->chassis_posture_info .leg_angle_R ,bl_ctrl->chassis_posture_info .leg_length_R); + bl_ctrl->mapping_info .J3_R = VMC_solve_J3(bl_ctrl->chassis_posture_info .leg_angle_R ,bl_ctrl->chassis_posture_info .leg_length_R); + bl_ctrl->mapping_info .J4_R = VMC_solve_J4(bl_ctrl->chassis_posture_info .leg_angle_R ,bl_ctrl->chassis_posture_info .leg_length_R ); + + bl_ctrl->torque_info.foot_horizontal_torque_L = + bl_ctrl->torque_info.foot_balancing_torque_L + bl_ctrl->torque_info.foot_moving_torque_L; + bl_ctrl->torque_info.foot_horizontal_torque_R = + bl_ctrl->torque_info.foot_balancing_torque_R + bl_ctrl->torque_info.foot_moving_torque_R; + + bl_ctrl->foot_motor_L.torque_out = bl_ctrl->torque_info.foot_horizontal_torque_L; + bl_ctrl->foot_motor_R.torque_out = bl_ctrl->torque_info.foot_horizontal_torque_R; + + LimitMax(bl_ctrl->foot_motor_L.torque_out,16383); + LimitMax(bl_ctrl->foot_motor_R.torque_out,16383); + + bl_ctrl->torque_info.joint_horizontal_torque_L = + bl_ctrl->torque_info.joint_balancing_torque_L + bl_ctrl->torque_info.joint_moving_torque_L + bl_ctrl->torque_info.joint_prevent_splits_torque_L; + bl_ctrl->torque_info.joint_horizontal_torque_R = + bl_ctrl->torque_info.joint_balancing_torque_R + bl_ctrl->torque_info.joint_moving_torque_R + bl_ctrl->torque_info.joint_prevent_splits_torque_R; + + bl_ctrl->torque_info.joint_vertical_torque_L = + bl_ctrl->torque_info.joint_stand_torque_L + bl_ctrl->torque_info.joint_roll_torque_L; + bl_ctrl->torque_info.joint_vertical_torque_R = + bl_ctrl->torque_info.joint_stand_torque_R + bl_ctrl->torque_info.joint_roll_torque_R; + + bl_ctrl->torque_info.joint_horizontal_torque_temp1_L = + (bl_ctrl->torque_info.joint_horizontal_torque_L) * bl_ctrl->mapping_info .J3_L ; + bl_ctrl->torque_info.joint_horizontal_torque_temp2_L = + (bl_ctrl->torque_info.joint_horizontal_torque_L) * bl_ctrl->mapping_info .J4_L ; + bl_ctrl->torque_info.joint_horizontal_torque_temp1_R = + (bl_ctrl->torque_info.joint_horizontal_torque_R) * bl_ctrl->mapping_info .J3_R ; + bl_ctrl->torque_info.joint_horizontal_torque_temp2_R = + (bl_ctrl->torque_info.joint_horizontal_torque_R) * bl_ctrl->mapping_info .J4_R ; + + bl_ctrl->torque_info.joint_vertical_torque_temp1_L = + (bl_ctrl->torque_info.joint_vertical_torque_L) * bl_ctrl->mapping_info .J1_L; + bl_ctrl->torque_info.joint_vertical_torque_temp2_L = + (bl_ctrl->torque_info.joint_vertical_torque_L) * bl_ctrl->mapping_info .J2_L; + bl_ctrl->torque_info.joint_vertical_torque_temp1_R = + (bl_ctrl->torque_info.joint_vertical_torque_R) * bl_ctrl->mapping_info .J1_R; + bl_ctrl->torque_info.joint_vertical_torque_temp2_R = + (bl_ctrl->torque_info.joint_vertical_torque_R) * bl_ctrl->mapping_info .J2_R; + + /****************************************/ + + fp32 MAX_balance = 2000.0f; + + LimitMax(bl_ctrl->torque_info.joint_horizontal_torque_temp1_L,MAX_balance); + LimitMax(bl_ctrl->torque_info.joint_horizontal_torque_temp2_L,MAX_balance); + LimitMax(bl_ctrl->torque_info.joint_horizontal_torque_temp1_R,MAX_balance); + LimitMax(bl_ctrl->torque_info.joint_horizontal_torque_temp2_R,MAX_balance); + LimitMax(bl_ctrl->torque_info.joint_vertical_torque_temp1_L,15); + LimitMax(bl_ctrl->torque_info.joint_vertical_torque_temp2_L,15); + LimitMax(bl_ctrl->torque_info.joint_vertical_torque_temp1_R,15); + LimitMax(bl_ctrl->torque_info.joint_vertical_torque_temp2_R,15); + + // 分配到四�?关节电机 + bl_ctrl->joint_motor_1.torque_out = + bl_ctrl->torque_info.joint_horizontal_torque_temp1_L + bl_ctrl->torque_info.joint_vertical_torque_temp1_L; + bl_ctrl->joint_motor_2.torque_out = + bl_ctrl->torque_info.joint_horizontal_torque_temp2_L + bl_ctrl->torque_info.joint_vertical_torque_temp2_L; + bl_ctrl->joint_motor_3.torque_out = - bl_ctrl->torque_info.joint_horizontal_torque_temp1_R + bl_ctrl->torque_info.joint_vertical_torque_temp1_R; + bl_ctrl->joint_motor_4.torque_out = - bl_ctrl->torque_info.joint_horizontal_torque_temp2_R + bl_ctrl->torque_info.joint_vertical_torque_temp2_R; + + if( ABS(bl_ctrl->joint_motor_1.motor_measure->ecd) <= MOTOR_POS_UPPER_BOUND ) + { + bl_ctrl->joint_motor_1.max_torque = LIMITED_TORQUE; + bl_ctrl->joint_motor_1.min_torque = -1.0f * UNLIMITED_TORQUE; + } + else if( ABS(bl_ctrl->joint_motor_1.motor_measure->ecd) >= MOTOR_POS_LOWER_BOUND ) + { + bl_ctrl->joint_motor_1.max_torque = UNLIMITED_TORQUE; + bl_ctrl->joint_motor_1.min_torque = -1.0f * LIMITED_TORQUE; + } + else + { + bl_ctrl->joint_motor_1.max_torque = UNLIMITED_TORQUE; + bl_ctrl->joint_motor_1.min_torque = -1.0f * UNLIMITED_TORQUE; + } + + if( ABS(bl_ctrl->joint_motor_3.motor_measure->ecd) <= MOTOR_POS_UPPER_BOUND ) + { + bl_ctrl->joint_motor_3.max_torque = LIMITED_TORQUE; + bl_ctrl->joint_motor_3.min_torque = -1.0f * UNLIMITED_TORQUE; + } + else if( ABS(bl_ctrl->joint_motor_3.motor_measure->ecd) >= MOTOR_POS_LOWER_BOUND ) + { + bl_ctrl->joint_motor_3.max_torque = UNLIMITED_TORQUE; + bl_ctrl->joint_motor_3.min_torque = -1.0f * LIMITED_TORQUE; + } + else + { + bl_ctrl->joint_motor_3.max_torque = UNLIMITED_TORQUE; + bl_ctrl->joint_motor_3.min_torque = -1.0f * UNLIMITED_TORQUE; + } + + if( ABS(bl_ctrl->joint_motor_2.motor_measure->ecd) <= MOTOR_POS_UPPER_BOUND ) + { + bl_ctrl->joint_motor_2.max_torque = UNLIMITED_TORQUE; + bl_ctrl->joint_motor_2.min_torque = -1.0f * LIMITED_TORQUE; + } + else if( ABS(bl_ctrl->joint_motor_2.motor_measure->ecd) >= MOTOR_POS_LOWER_BOUND ) + { + bl_ctrl->joint_motor_2.max_torque = LIMITED_TORQUE; + bl_ctrl->joint_motor_2.min_torque = -1.0f * UNLIMITED_TORQUE; + } + else + { + bl_ctrl->joint_motor_2.max_torque = UNLIMITED_TORQUE; + bl_ctrl->joint_motor_2.min_torque = -1.0f * UNLIMITED_TORQUE; + } + + if( ABS(bl_ctrl->joint_motor_4.motor_measure->ecd) <= MOTOR_POS_UPPER_BOUND ) + { + bl_ctrl->joint_motor_4.max_torque = UNLIMITED_TORQUE; + bl_ctrl->joint_motor_4.min_torque = -1.0f * LIMITED_TORQUE; + } + else if( ABS(bl_ctrl->joint_motor_4.motor_measure->ecd) >= MOTOR_POS_LOWER_BOUND ) + { + bl_ctrl->joint_motor_4.max_torque = LIMITED_TORQUE; + bl_ctrl->joint_motor_4.min_torque = -1.0f * UNLIMITED_TORQUE; + } + else + { + bl_ctrl->joint_motor_4.max_torque = UNLIMITED_TORQUE; + bl_ctrl->joint_motor_4.min_torque = -1.0f * UNLIMITED_TORQUE; + } + + LimitOutput( bl_ctrl->joint_motor_1.torque_out, bl_ctrl->joint_motor_1.min_torque, bl_ctrl->joint_motor_1.max_torque); + LimitOutput( bl_ctrl->joint_motor_2.torque_out, bl_ctrl->joint_motor_2.min_torque, bl_ctrl->joint_motor_2.max_torque); + LimitOutput( bl_ctrl->joint_motor_3.torque_out, bl_ctrl->joint_motor_3.min_torque, bl_ctrl->joint_motor_3.max_torque); + LimitOutput( bl_ctrl->joint_motor_4.torque_out, bl_ctrl->joint_motor_4.min_torque, bl_ctrl->joint_motor_4.max_torque); +} + +void Motor_CMD_Send(chassis_control_t *CMD_Send) +{ + if( CMD_Send->joint_motor_1.motor_mode == MOTOR_FORCE ) + CAN_HT_CMD( 0x01, CMD_Send->joint_motor_1.torque_out ); + else + CAN_HT_CMD( 0x01, 0.0 ); + if( CMD_Send->joint_motor_2.motor_mode == MOTOR_FORCE ) + CAN_HT_CMD( 0x02, CMD_Send->joint_motor_2.torque_out ); + else + CAN_HT_CMD( 0x02, 0.0 ); + + + if( CMD_Send->foot_motor_R.motor_mode != MOTOR_FORCE ) + CMD_Send->foot_motor_R .torque_out = 0.0f; + if( CMD_Send->foot_motor_L.motor_mode != MOTOR_FORCE ) + CMD_Send->foot_motor_L .torque_out = 0.0f; + CAN_BM_CONTROL_CMD( CMD_Send->foot_motor_L.torque_out, CMD_Send->foot_motor_R.torque_out ); + + vTaskDelay(1); + + if( CMD_Send->joint_motor_3.motor_mode == MOTOR_FORCE ) + CAN_HT_CMD( 0x03, CMD_Send->joint_motor_3.torque_out ); + else + CAN_HT_CMD( 0x03, 0.0 ); + + + if( CMD_Send->joint_motor_4.motor_mode == MOTOR_FORCE ) + CAN_HT_CMD( 0x04, CMD_Send->joint_motor_4.torque_out ); + else + CAN_HT_CMD( 0x04, 0.0 ); +} + +void Joint_Motor_to_Init_Pos() +{ + int Init_Time = 0; + while( Init_Time < 300 ) + { + CAN_HT_CMD( 0x01, 1.0 ); + CAN_HT_CMD( 0x02, -1.0 ); + CAN_HT_CMD( 0x03, 1.0 ); + vTaskDelay(1); + CAN_HT_CMD( 0x04, -1.0 ); + vTaskDelay(1); + Init_Time++; + } +} + +void HT_Motor_zero_set(void) +{ + uint8_t tx_buff[8]; + for( int i = 0; i < 7; i++ ) + tx_buff[i] = 0xFF; + tx_buff[7] = 0xfc; + + // ENABLE_CHASSIS + CAN_CMD_HT_Enable( 0x01, tx_buff ); + vTaskDelay(50); + CAN_CMD_HT_Enable( 0x02, tx_buff ); + vTaskDelay(50); + CAN_CMD_HT_Enable( 0x03, tx_buff ); + vTaskDelay(50); + CAN_CMD_HT_Enable( 0x04, tx_buff ); + vTaskDelay(50); + // �?到限�? + Joint_Motor_to_Init_Pos(); + // Set zero init point + tx_buff[7] = 0xfe; + + CAN_CMD_HT_Enable( 0x01, tx_buff ); + vTaskDelay(50); + CAN_CMD_HT_Enable( 0x02, tx_buff ); + vTaskDelay(50); + CAN_CMD_HT_Enable( 0x03, tx_buff ); + vTaskDelay(50); + CAN_CMD_HT_Enable( 0x04, tx_buff ); + + vTaskDelay(50); + +} + +void Motor_Zero_CMD_Send(void) +{ + CAN_HT_CMD( 0x01, 0.0 ); + vTaskDelay(1); + CAN_HT_CMD( 0x02, 0.0 ); + vTaskDelay(1); + CAN_HT_CMD( 0x03, 0.0 ); + vTaskDelay(1); + CAN_HT_CMD( 0x04, 0.0 ); + vTaskDelay(1); + + // CAN_BM_ENABLE_CMD(); + // vTaskDelay(1); + + // CAN_BM_MODE_SET_CMD(); + // vTaskDelay(2); + + // CAN_BM_CURRENT_MODE_CMD(); + // vTaskDelay(1); + +} diff --git a/utils/香港大学轮腿电控及建模开源-3/Chassis_Task.h b/utils/香港大学轮腿电控及建模开源-3/Chassis_Task.h new file mode 100644 index 0000000..2d33123 --- /dev/null +++ b/utils/香港大学轮腿电控及建模开源-3/Chassis_Task.h @@ -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 diff --git a/utils/香港大学轮腿电控及建模开源-3/HerKules_VOCAL_SJ_LQR_v4_with_data.m b/utils/香港大学轮腿电控及建模开源-3/HerKules_VOCAL_SJ_LQR_v4_with_data.m new file mode 100644 index 0000000..e762696 --- /dev/null +++ b/utils/香港大学轮腿电控及建模开源-3/HerKules_VOCAL_SJ_LQR_v4_with_data.m @@ -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 + diff --git a/utils/香港大学轮腿电控及建模开源-3/some_functions.c b/utils/香港大学轮腿电控及建模开源-3/some_functions.c new file mode 100644 index 0000000..5dbbd5b --- /dev/null +++ b/utils/香港大学轮腿电控及建模开源-3/some_functions.c @@ -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; + } + +} \ No newline at end of file diff --git a/utils/香港大学轮腿电控及建模开源-3/香港大学轮腿平衡步兵机械&电控解决方案概括.pdf b/utils/香港大学轮腿电控及建模开源-3/香港大学轮腿平衡步兵机械&电控解决方案概括.pdf new file mode 100644 index 0000000..d9966cb Binary files /dev/null and b/utils/香港大学轮腿电控及建模开源-3/香港大学轮腿平衡步兵机械&电控解决方案概括.pdf differ