![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Original Version of STM Board
Diff: main.cpp
- Revision:
- 209:ebc69d6ee6f1
- Parent:
- 208:408f9f15c486
- Child:
- 210:efc3a92cc6be
--- a/main.cpp Tue Dec 22 04:55:59 2020 +0000 +++ b/main.cpp Wed Dec 23 00:39:47 2020 +0000 @@ -1,4 +1,4 @@ -//201222_1 +//201223_1 #include "mbed.h" #include "FastPWM.h" #include "INIT_HW.h" @@ -199,72 +199,72 @@ float input_NN[num_input] = { 0.0f }; const float h1[num_input][16] = { - {-1.531315803527832f,1.2212283611297607f,0.12903714179992676f,0.033053863793611526f,-1.9360125064849854f,1.1332967281341553f,-0.17472904920578003f,1.3420113325119019f,-1.7463396787643433f,-0.5584802031517029f,-0.23686346411705017f,-0.5168386697769165f,-0.26205670833587646f,-0.4597594439983368f,-0.2571594715118408f,-0.39345231652259827f}, -{-0.9951635599136353f,0.6018988490104675f,-0.3927857577800751f,-0.20766094326972961f,-1.3627490997314453f,0.46893689036369324f,-0.4054011106491089f,0.4332157373428345f,-1.3325988054275513f,0.20551945269107819f,-0.27367573976516724f,-0.2924371659755707f,0.2375551164150238f,-0.7413603663444519f,-0.20967772603034973f,-0.06330179423093796f}, -{-1.0328309535980225f,0.7385027408599854f,0.36391180753707886f,-0.4938513934612274f,-0.930567741394043f,0.6501141786575317f,0.2872133255004883f,0.6771348118782043f,-1.2069311141967773f,-0.23835322260856628f,-0.34757405519485474f,-0.21662749350070953f,-0.0006922125467099249f,-0.08951175212860107f,0.05590221285820007f,-0.25968170166015625f}, -{-0.6548565030097961f,0.546489417552948f,0.39742106199264526f,0.23345571756362915f,-0.29420894384384155f,0.2337246835231781f,-0.11584559082984924f,0.9318102598190308f,-0.0883297249674797f,-0.6616360545158386f,0.09624255448579788f,0.3126813471317291f,-0.16282400488853455f,-0.017444096505641937f,-0.2961459457874298f,0.11506715416908264f}, -{0.9441654086112976f,-0.10412901639938354f,-0.0051451027393341064f,0.26189932227134705f,1.0599961280822754f,-0.13313990831375122f,0.1294034719467163f,-0.30942144989967346f,1.4052238464355469f,-0.5124068260192871f,0.34818795323371887f,0.18164508044719696f,-0.06639711558818817f,0.37773770093917847f,-0.3865056037902832f,-0.12323042005300522f}, -{3.354203939437866f,-3.036414384841919f,0.21773302555084229f,0.3865581452846527f,3.330247640609741f,-2.271965503692627f,0.16514194011688232f,-3.0577595233917236f,2.928663492202759f,1.7984576225280762f,-0.24734951555728912f,0.5408426523208618f,-0.2629496455192566f,0.958703875541687f,-0.4132833778858185f,-0.027743525803089142f}, -{-0.5307519435882568f,-0.06301706284284592f,-0.2501986622810364f,-0.6607210636138916f,-0.8369278311729431f,6.047488212585449f,0.06014183163642883f,-0.7298669815063477f,1.9458750486373901f,-5.044135570526123f,0.150299072265625f,0.15599359571933746f,-0.1737729012966156f,0.3276987075805664f,-0.15618428587913513f,-0.6801899075508118f}, -{-0.7901907563209534f,0.8988741636276245f,-0.26619744300842285f,0.025165753439068794f,-0.9365068674087524f,1.2295559644699097f,-0.17925891280174255f,0.7470439076423645f,-0.416498064994812f,-0.8755210638046265f,0.10463108122348785f,-0.5030022263526917f,0.3709419369697571f,-0.0012537387665361166f,0.04303058981895447f,-0.0186854749917984f}, -{0.6941404342651367f,-1.2099668979644775f,0.10248100757598877f,-0.4085768461227417f,-0.12387870997190475f,-1.1924992799758911f,-0.07246989011764526f,-1.665202021598816f,-1.7817565202713013f,2.3899729251861572f,-0.2979821562767029f,-0.18418405950069427f,0.2288188338279724f,0.5329566597938538f,-0.30555272102355957f,0.16609734296798706f}, -{-0.0660996288061142f,-1.2423714399337769f,0.18462657928466797f,-0.1813543140888214f,-0.2947060763835907f,-1.5129367113113403f,-0.2990540862083435f,-0.6701622605323792f,-1.3908182382583618f,1.1899564266204834f,-0.008929966948926449f,-1.0560450553894043f,0.3720368444919586f,0.07663324475288391f,0.3484981060028076f,-0.012029101140797138f}, -{0.28911903500556946f,-0.4749276638031006f,0.37182438373565674f,0.1734911948442459f,-0.4653550684452057f,-1.7457287311553955f,-0.20339298248291016f,-0.26837489008903503f,-1.042394757270813f,1.0918513536453247f,0.3477185070514679f,-0.7686545252799988f,-0.06686156243085861f,0.17347773909568787f,0.26688337326049805f,-0.3890383839607239f}, -{-0.28575125336647034f,-0.6693267226219177f,-0.40320003032684326f,-0.08735401183366776f,0.007286196108907461f,-1.8315372467041016f,0.1533789038658142f,0.022309979423880577f,-0.09538807719945908f,1.0808922052383423f,-0.41482847929000854f,-0.11989966779947281f,0.14430087804794312f,0.10769995301961899f,0.10493969917297363f,-0.21339687705039978f}, -{-0.10142430663108826f,-0.15534204244613647f,0.19847965240478516f,0.07757232338190079f,0.2527172267436981f,-1.3158543109893799f,-0.2404318004846573f,-0.01686050556600094f,0.036769624799489975f,0.4244236648082733f,-0.10963847488164902f,-0.3532784879207611f,0.09164450317621231f,0.0926702693104744f,-0.22268100082874298f,-0.5550960302352905f}, -{-0.23490804433822632f,0.05692242085933685f,0.4127817749977112f,-0.1364835798740387f,-0.17221158742904663f,-1.303916096687317f,-0.32001304626464844f,-0.298613041639328f,0.009885499253869057f,0.18572311103343964f,-0.1955135613679886f,-0.24695760011672974f,-0.31312841176986694f,-0.030824929475784302f,0.0479682981967926f,-0.07853478193283081f}, -{0.0065980833023786545f,0.021907925605773926f,0.20282304286956787f,0.04053569957613945f,0.3040331304073334f,-0.883127748966217f,0.04202890396118164f,0.18883246183395386f,0.227486252784729f,0.4307411313056946f,-0.27989521622657776f,-0.530148983001709f,0.10311337560415268f,-0.05951927602291107f,0.0668090283870697f,0.023457825183868408f}, -{-0.1333623081445694f,-0.27137404680252075f,-0.40439701080322266f,-0.10286334156990051f,-0.0770445168018341f,-0.4851205348968506f,-0.15568238496780396f,0.0012438585981726646f,0.2879120707511902f,0.1734604686498642f,-0.4743227958679199f,-0.2902921438217163f,-0.27164894342422485f,-0.008808882907032967f,-0.08245879411697388f,0.11585502326488495f}, -{-0.2022102028131485f,-0.27882087230682373f,0.16706281900405884f,0.07617825269699097f,-0.07703878730535507f,-0.6978834867477417f,-0.041414469480514526f,0.021906333044171333f,0.3641758859157562f,0.34097692370414734f,-0.35401779413223267f,-0.0123513825237751f,0.11138944327831268f,0.3358703553676605f,0.29427415132522583f,-0.004698142409324646f}, + {-1.6000940799713135f,0.4413917660713196f,0.12903714179992676f,-0.07903441786766052f,-1.8977726697921753f,1.8713637590408325f,-0.17472904920578003f,0.4568611681461334f,-0.43561747670173645f,-0.04615814611315727f,-0.23684236407279968f,-1.5037455558776855f,-0.2676747143268585f,-0.6216555237770081f,-0.2571594715118408f,-0.4336307942867279f}, +{-0.8367072343826294f,0.09323202818632126f,-0.3927857577800751f,-0.2727188766002655f,-1.0405902862548828f,0.8998438119888306f,-0.4054011106491089f,0.44874632358551025f,-0.29669591784477234f,0.21184058487415314f,-0.2736198306083679f,-0.8792054653167725f,0.23191256821155548f,-0.8017642498016357f,-0.20967772603034973f,-0.9694083333015442f}, +{-0.8188105821609497f,0.49548420310020447f,0.36391180753707886f,-0.5226353406906128f,-0.6585426330566406f,0.8799821138381958f,0.2872133255004883f,0.012798400595784187f,-0.41921380162239075f,0.2234516143798828f,-0.3474748432636261f,-0.5087661147117615f,-0.00637056166306138f,-0.18617482483386993f,0.05590221285820007f,-0.8025757670402527f}, +{-0.6304765939712524f,0.6922854781150818f,0.39742106199264526f,0.24299956858158112f,-0.42302995920181274f,0.14712309837341309f,-0.11584559082984924f,-0.5310759544372559f,-0.27385860681533813f,0.3663768470287323f,0.09638378769159317f,0.29256293177604675f,-0.16855108737945557f,-0.18148168921470642f,-0.2961459457874298f,0.8457931876182556f}, +{0.7473434209823608f,0.48988762497901917f,-0.0051451027393341064f,0.3333152234554291f,0.5788547396659851f,-0.6460171937942505f,0.1294034719467163f,-0.9815593957901001f,-0.3184085786342621f,0.19254086911678314f,0.34836387634277344f,0.49276548624038696f,-0.07216952741146088f,0.27343443036079407f,-0.3865056037902832f,1.4092313051223755f}, +{3.2342982292175293f,-2.2555158138275146f,0.21773302555084229f,0.5652960538864136f,3.146817207336426f,-3.1406891345977783f,0.16514194011688232f,0.6139293909072876f,-0.4235992729663849f,0.21656234562397003f,-0.24712057411670685f,1.274764895439148f,-0.2688344120979309f,1.1950470209121704f,-0.4132833778858185f,-0.059984441846609116f}, +{-0.6526948809623718f,0.7561483383178711f,-0.2501986622810364f,-0.7059148550033569f,-0.7783506512641907f,0.33273711800575256f,0.06014183163642883f,-4.296197891235352f,0.15086668729782104f,-0.12566223740577698f,0.1482049971818924f,0.7200766205787659f,-0.17589396238327026f,-0.14634668827056885f,-0.15618428587913513f,4.646012783050537f}, +{-0.7106614112854004f,-0.292780339717865f,-0.26619744300842285f,0.1277981847524643f,-0.9086422920227051f,0.2727634906768799f,-0.17925891280174255f,-2.5595145225524902f,-0.2821522653102875f,-0.020468801259994507f,0.1025993749499321f,0.01748223975300789f,0.36828503012657166f,0.025631556287407875f,0.04303058981895447f,1.733962059020996f}, +{0.9920234084129333f,-1.627883791923523f,0.10248100757598877f,-0.08735460788011551f,0.6591731309890747f,-0.48275211453437805f,-0.07246989011764526f,0.49826139211654663f,-0.6757456660270691f,0.2720978260040283f,-0.29995009303092957f,0.6254252791404724f,0.22595258057117462f,1.0093032121658325f,-0.30555272102355957f,-3.7689568996429443f}, +{0.14623138308525085f,-1.3082497119903564f,0.18462657928466797f,0.06593218445777893f,0.38641560077667236f,0.006398537661880255f,-0.2990540862083435f,0.5669264197349548f,-0.25464093685150146f,-0.5164272785186768f,-0.010852855630218983f,-0.6049553751945496f,0.36904722452163696f,0.27233806252479553f,0.3484981060028076f,-2.143152952194214f}, +{0.26631736755371094f,-0.37726837396621704f,0.37182438373565674f,0.2574600577354431f,-0.09127864241600037f,-0.01836557500064373f,-0.20339298248291016f,0.23786002397537231f,-0.49957215785980225f,-0.008945263922214508f,0.3458111584186554f,-0.824551522731781f,-0.06989830732345581f,0.1074192151427269f,0.26688337326049805f,-0.8953605890274048f}, +{-0.23877182602882385f,-0.7098087072372437f,-0.40320003032684326f,-0.1078604906797409f,0.28780120611190796f,-0.42413681745529175f,0.1533789038658142f,0.40164631605148315f,-0.1681622713804245f,0.16031795740127563f,-0.4167521595954895f,-0.43077829480171204f,0.1412402093410492f,0.07915178686380386f,0.10493969917297363f,-0.07492798566818237f}, +{-0.03229803964495659f,-0.42042604088783264f,0.19847965240478516f,-0.0030563066247850657f,0.36559855937957764f,-0.1463041454553604f,-0.2404318004846573f,0.20067740976810455f,-0.1734953671693802f,-0.2558341920375824f,-0.1115814819931984f,-0.7943522334098816f,0.08852691203355789f,0.1106438934803009f,-0.22268100082874298f,-0.1786385029554367f}, +{-0.11297377943992615f,-0.43093106150627136f,0.4127817749977112f,-0.24888765811920166f,-0.21446679532527924f,-0.5084688663482666f,-0.32001304626464844f,0.04496301710605621f,-0.12302793562412262f,-0.3023318648338318f,-0.19743306934833527f,-0.7400186061859131f,-0.3162824809551239f,0.03880959749221802f,0.0479682981967926f,-0.08672763407230377f}, +{-0.02818325348198414f,-0.06082405894994736f,0.20282304286956787f,-0.12476225942373276f,0.10320129990577698f,-0.1304563730955124f,0.04202890396118164f,0.21194368600845337f,0.32223421335220337f,-0.0071917143650352955f,-0.28180214762687683f,-1.0684038400650024f,0.09998573362827301f,-0.12126590311527252f,0.0668090283870697f,-0.012993170879781246f}, +{-0.005457713268697262f,-0.446575790643692f,-0.40439701080322266f,-0.2776923179626465f,-0.21002432703971863f,-0.2032385915517807f,-0.15568238496780396f,-0.06614420562982559f,0.19860975444316864f,-0.10510893166065216f,-0.4762294590473175f,-0.7791360020637512f,-0.27474963665008545f,-0.020535530522465706f,-0.08245879411697388f,0.022347593680024147f}, +{-0.11706838756799698f,-0.6494715213775635f,0.16706281900405884f,-0.1073165163397789f,-0.16269126534461975f,-0.41736364364624023f,-0.041414469480514526f,0.182882621884346f,-0.049576375633478165f,-0.01950126886367798f,-0.355907678604126f,-0.59775710105896f,0.10836437344551086f,0.30783337354660034f,0.29427415132522583f,0.02123108133673668f}, }; const float h2[16][16] = { - {-2.799281358718872f,0.16712452471256256f,-0.06966331601142883f,1.4672540426254272f,-0.21907491981983185f,1.5811668634414673f,-0.053468767553567886f,0.0904630720615387f,-0.43008196353912354f,-0.1415480375289917f,-0.12140687555074692f,-2.6124908924102783f,-0.0548873208463192f,-4.641530513763428f,-0.8119884133338928f,-2.656848669052124f}, -{0.29130902886390686f,-0.44502395391464233f,0.057057321071624756f,-0.4548104703426361f,-0.35503754019737244f,-2.724472999572754f,-0.7551739811897278f,-0.2990124225616455f,-0.33757925033569336f,0.2895788848400116f,-1.4328194856643677f,-1.4679527282714844f,0.2138679027557373f,-1.4188612699508667f,-0.4723600447177887f,-1.7820600271224976f}, + {-3.323141574859619f,2.0404324531555176f,-0.06966331601142883f,1.7146148681640625f,-0.21907491981983185f,0.3013889193534851f,-0.20208165049552917f,0.0904630720615387f,-0.43008196353912354f,-0.1415480375289917f,1.703560709953308f,-1.0790513753890991f,0.12968102097511292f,-4.26299524307251f,-0.03616251423954964f,1.0661470890045166f}, +{0.12045703083276749f,0.15307354927062988f,0.057057321071624756f,3.0073626041412354f,-0.35503754019737244f,-1.694530725479126f,-0.34237051010131836f,-0.2990124225616455f,-0.33757925033569336f,0.2895788848400116f,-1.596266746520996f,-0.9465551376342773f,0.33698758482933044f,1.5713340044021606f,0.2106013149023056f,0.14802111685276031f}, {-0.22745239734649658f,0.003037691116333008f,-0.061119019985198975f,0.35696902871131897f,0.05568113923072815f,0.011741191148757935f,-0.20225946605205536f,-0.08465918898582458f,0.3489862382411957f,0.0687277615070343f,0.31964078545570374f,0.3004753887653351f,0.36063823103904724f,-0.42892736196517944f,0.08652284741401672f,0.027493387460708618f}, -{0.04097104072570801f,-0.3449876606464386f,-0.2894435524940491f,0.29498887062072754f,0.08946844935417175f,0.24791251122951508f,-0.3380562663078308f,0.4122363030910492f,0.26211628317832947f,0.2953031361103058f,0.24438774585723877f,0.33562910556793213f,-0.5284942984580994f,-0.1639743596315384f,0.2636021077632904f,0.10835816711187363f}, -{-1.542779564857483f,0.5664149522781372f,-0.25313520431518555f,1.1262837648391724f,0.036378175020217896f,0.7033790349960327f,-0.5617905259132385f,-0.08570799231529236f,-0.024399548768997192f,-0.39465832710266113f,-0.3923259973526001f,-2.222499132156372f,-0.11098752915859222f,-3.194650650024414f,-0.8124714493751526f,-4.139412879943848f}, -{0.22803305089473724f,0.16566713154315948f,-0.3519742488861084f,-0.3744398057460785f,-0.3015052080154419f,0.008913038298487663f,-0.6454094052314758f,-0.019414573907852173f,-0.23047015070915222f,0.3579089343547821f,0.0011918706586584449f,0.1011725664138794f,-0.33286166191101074f,0.27339494228363037f,0.10018529742956161f,-2.3298583030700684f}, +{-0.0038494274485856295f,-0.27630358934402466f,-0.2894435524940491f,0.4252559542655945f,0.08946844935417175f,0.27233457565307617f,-0.3439287841320038f,0.4122363030910492f,0.26211628317832947f,0.2953031361103058f,0.2075938880443573f,0.2481931746006012f,-0.404754102230072f,-0.39094415307044983f,0.12583167850971222f,0.12181642651557922f}, +{-0.8256309032440186f,1.8575127124786377f,-0.25313520431518555f,0.03169454261660576f,0.036378175020217896f,0.14327585697174072f,-0.7250795960426331f,-0.08570799231529236f,-0.024399548768997192f,-0.39465832710266113f,-0.36996984481811523f,-1.1260812282562256f,0.10077962279319763f,-1.170457363128662f,-0.640777051448822f,0.3631480038166046f}, +{1.3197550773620605f,-1.6532154083251953f,-0.3519742488861084f,-0.4759509563446045f,-0.3015052080154419f,-0.37989047169685364f,-1.0408955812454224f,-0.019414573907852173f,-0.23047015070915222f,0.3579089343547821f,-0.4645023047924042f,-0.7267597317695618f,-0.20959392189979553f,-0.8655065298080444f,-0.023840362206101418f,-0.2557324171066284f}, {-0.1828227937221527f,-0.02554568648338318f,-0.3260969817638397f,0.08422836661338806f,-0.38453540205955505f,-0.25432005524635315f,0.285016268491745f,0.12387624382972717f,-0.0982072651386261f,0.13111665844917297f,-0.03692615032196045f,-0.32796353101730347f,-0.21546880900859833f,0.049302369356155396f,-0.27088475227355957f,-0.4124959409236908f}, -{2.628514289855957f,-0.2776978015899658f,0.33083590865135193f,0.8097639679908752f,-0.14358049631118774f,-3.2291982173919678f,-0.2805100381374359f,-0.3003333508968353f,0.2600560486316681f,-0.37898191809654236f,-0.19844326376914978f,-4.452375888824463f,-0.45537886023521423f,-2.5229427814483643f,0.28838008642196655f,0.604862630367279f}, -{-1.270782232284546f,0.6037241220474243f,-0.11379697918891907f,3.171149253845215f,0.041274964809417725f,0.7873656153678894f,-0.34019649028778076f,0.0019084513187408447f,0.06103590130805969f,-0.38046833872795105f,-3.5276150703430176f,1.4744601249694824f,-0.49409180879592896f,-0.061534080654382706f,-1.2860485315322876f,-3.6122732162475586f}, -{-0.3107764720916748f,0.3795167803764343f,-0.13502129912376404f,-6.376976013183594f,0.12987366318702698f,0.16281241178512573f,-0.22298330068588257f,-0.3632148504257202f,-0.13619378209114075f,0.16938945651054382f,-0.35801035165786743f,-0.2300788313150406f,0.1270095258951187f,0.2384432852268219f,-0.4074200689792633f,-0.6243495941162109f}, -{-0.2787967026233673f,-0.001280608936212957f,0.0457797646522522f,-0.3553522527217865f,-0.03321319818496704f,-0.4252603352069855f,0.17873415350914001f,-0.20421427488327026f,-0.050184011459350586f,0.12480869889259338f,-0.21523112058639526f,0.3545852601528168f,-0.3647043704986572f,0.411028116941452f,-0.3983408212661743f,-0.2770851254463196f}, -{0.3964467942714691f,0.35733214020729065f,0.3954955041408539f,0.3909227252006531f,0.0033026933670043945f,0.11897452175617218f,-0.4904947280883789f,-0.11795541644096375f,0.3487861454486847f,-0.32520344853401184f,-0.16489219665527344f,-0.33246710896492004f,-0.04149547591805458f,-0.6514362692832947f,0.22790876030921936f,-0.1729116290807724f}, -{-0.2808303236961365f,-0.21577134728431702f,-0.15798500180244446f,0.3731594383716583f,-0.37393757700920105f,0.36395198106765747f,0.11292675137519836f,-0.2947862446308136f,-0.3764709532260895f,0.2424570620059967f,-0.11440901458263397f,-0.2837170362472534f,0.41839322447776794f,-0.011972751468420029f,-0.12294908612966537f,0.10422277450561523f}, -{-0.5422749519348145f,0.41037851572036743f,-0.2018718123435974f,0.09328148514032364f,0.07545611262321472f,0.2041396200656891f,-0.4444405138492584f,-0.21416273713111877f,-0.24509364366531372f,0.19522181153297424f,0.06249627470970154f,-0.6804867386817932f,-0.29067087173461914f,-0.441444456577301f,-0.3149453103542328f,-0.46726182103157043f}, +{-0.07586349546909332f,-0.36089158058166504f,0.33083590865135193f,-0.2909453213214874f,-0.14358049631118774f,1.0950231552124023f,-0.025492865592241287f,-0.3003333508968353f,0.2600560486316681f,-0.37898191809654236f,0.6435545086860657f,-0.5071706771850586f,-0.31673234701156616f,0.11079536378383636f,-11.455972671508789f,0.5229361057281494f}, +{0.004702992737293243f,-0.1820937544107437f,-0.11379697918891907f,0.14139454066753387f,0.041274964809417725f,-0.14186520874500275f,-0.10284432768821716f,0.0019084513187408447f,0.06103590130805969f,-0.38046833872795105f,0.3429935574531555f,-0.34765779972076416f,-0.37024784088134766f,0.1993687003850937f,-0.2297692447900772f,-0.23237699270248413f}, +{-0.34216949343681335f,0.23496045172214508f,-0.13502129912376404f,0.08847349882125854f,0.12987366318702698f,0.14208538830280304f,0.2711336314678192f,-0.3632148504257202f,-0.13619378209114075f,0.16938945651054382f,0.2009814977645874f,0.44322916865348816f,0.3811538517475128f,-0.24470730125904083f,0.04349624365568161f,-0.24874821305274963f}, +{-0.28599539399147034f,0.0020868044812232256f,0.0457797646522522f,-0.3547317385673523f,-0.03321319818496704f,-0.42137426137924194f,0.17873415350914001f,-0.20421427488327026f,-0.050184011459350586f,0.12480869889259338f,-0.2116698920726776f,0.3545852601528168f,-0.3647043704986572f,0.40816983580589294f,-0.4018280804157257f,-0.27432969212532043f}, +{0.1522299200296402f,-0.45329317450523376f,0.3954955041408539f,-0.48470574617385864f,0.0033026933670043945f,0.572025716304779f,-0.31212282180786133f,-0.11795541644096375f,0.3487861454486847f,-0.32520344853401184f,-3.3134469985961914f,-0.530846357345581f,0.0820283591747284f,-2.0381298065185547f,1.0461171865463257f,0.4835590124130249f}, +{-0.2670588493347168f,-0.21156641840934753f,-0.15798500180244446f,0.38131895661354065f,-0.37393757700920105f,0.3658103048801422f,0.11292675137519836f,-0.2947862446308136f,-0.3764709532260895f,0.2424570620059967f,-0.10874747484922409f,-0.2837170362472534f,0.41839322447776794f,-0.016431119292974472f,-0.1284826099872589f,0.10878776758909225f}, +{-0.08318141847848892f,0.9871321320533752f,-0.2018718123435974f,-1.6108821630477905f,0.07545611262321472f,0.18611471354961395f,-0.43937158584594727f,-0.21416273713111877f,-0.24509364366531372f,0.19522181153297424f,-0.010126939043402672f,-0.5335819721221924f,-0.16718891263008118f,-1.5700724124908447f,0.43778514862060547f,-0.32085344195365906f}, {0.4138670265674591f,0.1604653298854828f,0.056746453046798706f,0.036025404930114746f,0.3228367865085602f,-0.07083973288536072f,0.018455177545547485f,0.0059362053871154785f,0.40515169501304626f,0.014240056276321411f,-0.07738298177719116f,0.1407785713672638f,-0.13024571537971497f,-0.29546058177948f,-0.11976784467697144f,-0.35825538635253906f}, -{-0.20526202023029327f,0.25847819447517395f,0.12081471085548401f,-0.2155798226594925f,0.29976895451545715f,-0.04457804560661316f,-0.12792883813381195f,0.31089308857917786f,-0.3893685042858124f,-0.02222958207130432f,0.26056283712387085f,-0.17489492893218994f,-0.32397574186325073f,0.021401749923825264f,0.303682804107666f,-0.07271138578653336f}, +{-0.7150428295135498f,1.2618980407714844f,0.12081471085548401f,0.5926679968833923f,0.29976895451545715f,-1.5232646465301514f,-0.25536221265792847f,0.31089308857917786f,-0.3893685042858124f,-0.02222958207130432f,-1.6488481760025024f,-0.8796235918998718f,-0.19991017878055573f,1.0744037628173828f,0.49341896176338196f,-2.339517593383789f}, }; const float h3[16][16] = { - {-0.36079341173171997f,-0.6086024641990662f,2.179344654083252f,-0.5495796799659729f,-0.139645516872406f,-0.42361360788345337f,0.3332441747188568f,0.3036012351512909f,-0.5051015019416809f,2.4959444999694824f,-0.1488727182149887f,1.460726261138916f,0.32145416736602783f,-0.73075270652771f,0.10370074957609177f,0.023498691618442535f}, -{0.047732532024383545f,0.26677238941192627f,-0.09477218240499496f,-0.2789490222930908f,-0.23424319922924042f,0.029373519122600555f,-0.3198729455471039f,0.06247803568840027f,0.29579290747642517f,-0.1780138611793518f,0.4144817292690277f,-0.3128277659416199f,-0.32693424820899963f,0.39209941029548645f,-0.4388388395309448f,-0.5931907892227173f}, + {-0.36079341173171997f,-4.516791343688965f,0.9862267971038818f,-0.4074249267578125f,-1.7838587760925293f,1.4453092813491821f,0.3332441747188568f,0.36921602487564087f,-0.3300304710865021f,-0.19002185761928558f,-0.40770405530929565f,0.4001283347606659f,2.8152735233306885f,-1.6125714778900146f,4.618896961212158f,0.12741845846176147f}, +{0.047732532024383545f,-0.34105879068374634f,0.20525789260864258f,-0.2253294587135315f,0.3607877194881439f,0.141396164894104f,-0.3198729455471039f,-0.08264346420764923f,0.07495583593845367f,-0.02900463528931141f,0.32022908329963684f,-0.5149437785148621f,-1.6438157558441162f,-1.3210893869400024f,-2.072963237762451f,-0.2941063344478607f}, {0.07903262972831726f,0.2790505588054657f,-0.07798504829406738f,0.04248586297035217f,-0.1963958442211151f,-0.19260792434215546f,-0.4038352966308594f,0.015906542539596558f,0.15353140234947205f,0.030178606510162354f,0.2488909661769867f,0.13805970549583435f,-0.0816211998462677f,-0.20733052492141724f,-0.3036302626132965f,0.054825395345687866f}, -{-0.30922991037368774f,-1.0693680047988892f,-0.9863292574882507f,-0.5392837524414062f,-1.6881351470947266f,-0.9114060997962952f,0.20021501183509827f,0.07002416253089905f,-0.5644193887710571f,0.34156420826911926f,0.04209338501095772f,1.9385147094726562f,-2.372239828109741f,-0.04623681306838989f,0.16066716611385345f,-2.871879816055298f}, +{-0.30922991037368774f,-0.0848562940955162f,-0.2771536707878113f,-0.39299437403678894f,-0.501600980758667f,-0.07938367873430252f,0.20021501183509827f,-0.019295204430818558f,-0.5755168795585632f,-0.46145495772361755f,0.06848174333572388f,0.11526546627283096f,-0.9005468487739563f,-0.6515923142433167f,-0.5762438774108887f,-0.04038632661104202f}, {-0.39607733488082886f,-0.05481579899787903f,0.1976260244846344f,0.022423356771469116f,0.16892847418785095f,-0.27518749237060547f,0.16012099385261536f,0.3626593053340912f,-0.08640444278717041f,-0.11053556203842163f,-0.10529157519340515f,-0.31317979097366333f,-0.1530032455921173f,-0.1336749792098999f,0.22959044575691223f,0.19986507296562195f}, -{-0.37449589371681213f,-2.4325759410858154f,-0.9931590557098389f,-0.044506754726171494f,-0.3293914198875427f,0.49045220017433167f,-0.18732719123363495f,0.22384825348854065f,-0.8132121562957764f,-0.7270507216453552f,-0.12444746494293213f,-3.9139862060546875f,-0.33186227083206177f,0.6893238425254822f,1.089476466178894f,-0.2878223657608032f}, -{0.4110594093799591f,0.06593003123998642f,-0.10276640951633453f,0.37246426939964294f,0.24670250713825226f,-0.24950674176216125f,-0.08670487999916077f,-0.25336313247680664f,-0.030661463737487793f,-0.06259563565254211f,-0.1344406008720398f,0.35313835740089417f,0.3990647792816162f,-0.06628382951021194f,0.04818518087267876f,-0.1465446799993515f}, +{-0.37449589371681213f,-0.9064159989356995f,-0.6610195636749268f,0.0874672383069992f,-0.5877532362937927f,0.3902459144592285f,-0.18732719123363495f,0.19508449733257294f,-0.6848570704460144f,-0.31486886739730835f,-0.0473649837076664f,-0.07467646896839142f,-0.5057598948478699f,1.9025719165802002f,0.09490102529525757f,-0.36226871609687805f}, +{0.4110594093799591f,0.2790789306163788f,-0.31044068932533264f,0.37246426939964294f,0.21461978554725647f,-0.05120496079325676f,-0.08670487999916077f,-0.25336313247680664f,-0.030661463737487793f,-0.06259563565254211f,-0.1344406008720398f,0.35313835740089417f,0.19873254001140594f,0.11977154016494751f,0.3239758014678955f,-0.3391006588935852f}, {-0.40892091393470764f,0.043769627809524536f,-0.3867315948009491f,0.25968697667121887f,0.3424709737300873f,-0.051169753074645996f,-0.23312048614025116f,-0.390264093875885f,0.28059282898902893f,-0.1559126079082489f,-0.14134526252746582f,-0.0003446042537689209f,-0.2742875814437866f,-0.36560842394828796f,0.07994696497917175f,0.005298197269439697f}, {0.10697010159492493f,-0.12228584289550781f,-0.37870171666145325f,0.21184906363487244f,-0.37222859263420105f,-0.17138728499412537f,-0.1382003128528595f,0.3493293821811676f,-0.360889196395874f,-0.3875247836112976f,0.42142823338508606f,-0.3482915461063385f,-0.3289247751235962f,-0.2186824083328247f,0.09620395302772522f,-0.06898030638694763f}, {0.2847062647342682f,0.018552124500274658f,0.11435768008232117f,0.36562982201576233f,-0.047046810388565063f,0.30447837710380554f,0.2430230677127838f,0.2909286320209503f,-0.2802048921585083f,0.18043199181556702f,0.41849127411842346f,-0.287167489528656f,0.24394884705543518f,-0.14084559679031372f,-0.10168051719665527f,0.010465055704116821f}, -{0.15459725260734558f,0.3019735515117645f,-0.09361782670021057f,-0.31664907932281494f,-0.57173752784729f,0.012515033595263958f,-0.08742031455039978f,0.05785742402076721f,-0.027446074411273003f,-0.2233911156654358f,0.1268840730190277f,-0.30447322130203247f,0.2713693380355835f,-0.5980722308158875f,0.432270348072052f,0.3365720510482788f}, -{-0.408692330121994f,-1.0179634094238281f,0.9107185006141663f,0.0922636091709137f,-0.218761146068573f,-0.7093446850776672f,0.03285527229309082f,0.38763079047203064f,-0.20705322921276093f,0.016747837886214256f,0.04190991446375847f,-0.6101645827293396f,-0.7766126394271851f,-0.607116162776947f,1.0755422115325928f,0.1505926102399826f}, -{-0.2991822361946106f,0.2430352121591568f,0.07094341516494751f,-0.05932474136352539f,0.22252558171749115f,0.09018097072839737f,-0.11253207921981812f,0.34576353430747986f,0.04814547300338745f,-0.35770976543426514f,-0.044228196144104004f,-0.36229726672172546f,0.15250977873802185f,-0.2781256139278412f,0.21565958857536316f,-0.032620642334222794f}, -{-0.2675279378890991f,2.295318365097046f,0.6247335076332092f,-0.09322933852672577f,-0.1486014425754547f,0.2098596841096878f,-0.32875844836235046f,-0.31893211603164673f,-0.8386218547821045f,-3.31750750541687f,-0.6547098159790039f,-0.7874006628990173f,-0.16551893949508667f,-0.09824063628911972f,1.3998380899429321f,0.12306978553533554f}, -{-0.015470266342163086f,0.2835947275161743f,0.23104417324066162f,0.20350171625614166f,0.1442382037639618f,-0.4490983188152313f,0.05196094512939453f,-0.008358269929885864f,-0.4102620780467987f,-0.3847755491733551f,0.13179203867912292f,-0.11511552333831787f,0.58179771900177f,-1.130089282989502f,1.005839467048645f,0.20838046073913574f}, -{0.1863725483417511f,-0.3666064739227295f,-1.1749751567840576f,-0.6089721322059631f,0.098382867872715f,0.8263962864875793f,0.04860696196556091f,0.17769548296928406f,-0.017368249595165253f,-1.8929333686828613f,-0.3929237127304077f,-9.379828453063965f,0.45098361372947693f,-1.4723246097564697f,-1.9610977172851562f,-0.027309749275445938f}, +{0.15459725260734558f,0.2614503502845764f,-0.8414085507392883f,-0.252727746963501f,-0.6894345283508301f,1.0636138916015625f,-0.08742031455039978f,0.06376123428344727f,-0.1714128702878952f,-0.47240930795669556f,0.1268840730190277f,-0.44391879439353943f,0.12466016411781311f,-0.9005870819091797f,-0.9095998406410217f,0.30346015095710754f}, +{-0.408692330121994f,-0.10330183058977127f,-0.03088713251054287f,-0.04910988733172417f,-0.14289908111095428f,-0.08836136758327484f,0.03285527229309082f,0.38763079047203064f,-0.20705322921276093f,-0.26367273926734924f,0.12809070944786072f,0.03996849060058594f,-0.6194682121276855f,-0.3420376479625702f,-0.15302342176437378f,0.334671288728714f}, +{-0.2991822361946106f,0.3794580399990082f,-0.07897943258285522f,-0.05932474136352539f,0.08582660555839539f,0.24227938055992126f,-0.11253207921981812f,0.34576353430747986f,0.04814547300338745f,-0.35770976543426514f,-0.044228196144104004f,-0.36229726672172546f,0.015840977430343628f,-0.13475483655929565f,0.36124154925346375f,-0.16869547963142395f}, +{-0.2675279378890991f,-0.2572242021560669f,-0.008989264257252216f,-0.14364181458950043f,2.3832051753997803f,0.2777503728866577f,-0.32875844836235046f,-0.2555496394634247f,-0.6392947435379028f,-0.7831358909606934f,-0.5172973275184631f,-0.6818525195121765f,0.8480085134506226f,-0.1394871473312378f,-1.3122553825378418f,0.1775851845741272f}, +{-0.015470266342163086f,0.5476723909378052f,0.4976131021976471f,0.19412125647068024f,-1.3260929584503174f,-1.7757221460342407f,0.05196094512939453f,0.09510938823223114f,-0.42801159620285034f,-0.7967047095298767f,0.13606210052967072f,-0.11511552333831787f,-0.7240217924118042f,-0.9549937844276428f,-0.9997981190681458f,-0.45693734288215637f}, +{0.1863725483417511f,0.9262488484382629f,-0.6855837106704712f,-0.577112078666687f,-0.642444372177124f,-0.24064891040325165f,0.04860696196556091f,0.14380735158920288f,-0.2648492157459259f,-0.029172860085964203f,-0.3929237127304077f,-0.10258594900369644f,1.1357184648513794f,0.016820937395095825f,1.862500786781311f,-0.1434863805770874f}, }; -const float hout[16] = { 0.45773375034332275f,0.2291797697544098f,-0.3173103332519531f,-0.04260948672890663f,-0.02334880642592907f,0.4189964234828949f,0.0030125975608825684f,0.15066689252853394f,-0.2544001638889313f,0.1269427388906479f,0.15506184101104736f,-0.25900861620903015f,-0.398495078086853f,0.18022006750106812f,0.21481767296791077f,-0.42234814167022705f }; - -const float b1[16] = { 0.7194389700889587f,1.7438278198242188f,-1.7145336866378784f,0.31947922706604004f,1.4380247592926025f,1.8047207593917847f,-0.058932315558195114f,1.4295762777328491f,1.0237047672271729f,-0.6756473779678345f,0.5715821385383606f,1.474446177482605f,-0.37206971645355225f,-0.9106371402740479f,-1.087764859199524f,0.8449752330780029f }; - -const float b2[16] = { 0.35840335488319397f,-0.8230704069137573f,-1.4564176797866821f,-0.08832063525915146f,-0.6939148902893066f,0.45191285014152527f,-0.030644390732049942f,-0.30969977378845215f,-1.20063054561615f,-1.912178635597229f,-0.1477353721857071f,0.8669278025627136f,-0.33790263533592224f,1.75452721118927f,-0.16518579423427582f,1.1603200435638428f }; - -const float b3[16] = { -1.963319182395935f,-0.6351005434989929f,0.45000067353248596f,-0.2284822016954422f,-0.28700459003448486f,1.1167820692062378f,-0.45353031158447266f,-0.6562485098838806f,-0.03606545552611351f,0.0329035222530365f,-0.18756742775440216f,-1.2942627668380737f,-1.1204206943511963f,0.4729367792606354f,-1.0672776699066162f,-0.5879763960838318f }; - -const float bout[1] = { -0.4794664680957794f }; +const float hout[16] = { 0.45773375034332275f,0.22148412466049194f,-0.22813020646572113f,-0.08917035162448883f,-0.21528248488903046f,0.29861685633659363f,0.0030125975608825684f,0.1412937343120575f,0.04779195412993431f,0.05501026287674904f,0.13024552166461945f,-0.04984400048851967f,-0.304860919713974f,-0.14634203910827637f,0.23745152354240417f,-0.16097530722618103f }; + +const float b1[16] = { 0.3074471652507782f,2.836015224456787f,-1.7145336866378784f,0.2683033347129822f,0.6529231071472168f,0.9833991527557373f,-0.058932315558195114f,2.29449462890625f,0.9298062920570374f,-0.9408509135246277f,0.5694966912269592f,1.5112576484680176f,-0.3744742274284363f,-1.0548338890075684f,-1.087764859199524f,0.6528083682060242f }; + +const float b2[16] = { 0.6096800565719604f,-0.6226189136505127f,-1.4564176797866821f,-0.5512577891349792f,-0.6939148902893066f,0.9173181653022766f,-0.0477285161614418f,-0.30969977378845215f,-1.200630784034729f,-1.912178635597229f,0.6023832559585571f,-0.04530715569853783f,-0.07918518036603928f,0.8115787506103516f,0.43986114859580994f,1.1172503232955933f }; + +const float b3[16] = { -1.963319182395935f,0.43098294734954834f,1.14909827709198f,-0.260453462600708f,-0.3398900330066681f,0.9021360874176025f,-0.45353031158447266f,-0.7342051267623901f,0.2843529284000397f,0.16309885680675507f,-0.24944843351840973f,-0.24138091504573822f,-0.16837841272354126f,-0.15490709245204926f,-0.771912157535553f,-0.42885565757751465f }; + +const float bout[1] = { -0.08879395574331284f }; /////////////////////////////////////////////////////////////////////////////////////////////RL float input_RL[num_input_RL] = { 0.0f }; @@ -1041,8 +1041,9 @@ ind = ind + 1; for(int i=0; i<numfuture_f; i++) { // input_NN[ind] = (f_future[time_interval*i+time_interval] - torq.sen)/10000.0f * 8.0f + 0.5f; - input_NN[ind] = (f_future[time_interval*i+time_interval] - torq.sen)/10000.0f + 0.5f; +// input_NN[ind] = (f_future[time_interval*i+time_interval] - torq.sen)/10000.0f + 0.5f; // input_NN[ind] = (f_future[time_interval*i+time_interval])/10000.0f*8.0f+0.5f; + input_NN[ind] = (f_future[time_interval*i+time_interval])/10000.0f + 0.5f; ind = ind + 1; } @@ -2378,7 +2379,6 @@ // torque feedback torq.err = torq_ref - torq.sen; //[N] torq.err_sum += torq.err/(float) TMR_FREQ_5k; //[N] - torq_dot.sen = (torq.sen-totq_sen_past)*(float) TMR_FREQ_5k; if (((OPERATING_MODE&0b110)>>1) == 0 || ((OPERATING_MODE&0b110)>>1) == 1) { @@ -2442,7 +2442,7 @@ float VALVE_POS_RAW_FORCE_FF = 0.0f; float VALVE_POS_RAW = 0.0f; - VALVE_POS_RAW_FORCE_FB = alpha_trans*(((float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum + (float) D_GAIN_JOINT_TORQUE * (torq.ref_diff - torq_dot.sen) / (float) TMR_FREQ_5k) * 0.01f + DDV_JOINT_POS_FF(vel.sen))+ (1.0f-alpha_trans) * (P_GAIN_JOINT_POSITION * 0.01f * pos.err + DDV_JOINT_POS_FF(vel.ref)); + VALVE_POS_RAW_FORCE_FB = alpha_trans*(((float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum + (float) D_GAIN_JOINT_TORQUE * (torq.ref_diff - torq_dot.sen)) * 0.01f + DDV_JOINT_POS_FF(vel.sen))+ (1.0f-alpha_trans) * (P_GAIN_JOINT_POSITION * 0.01f * pos.err + DDV_JOINT_POS_FF(vel.ref)); VALVE_POS_RAW_FORCE_FF = P_GAIN_JOINT_TORQUE_FF * torq_ref * 0.001f + D_GAIN_JOINT_TORQUE_FF * (torq_ref - torq_ref_past) * 0.0001f; @@ -2478,7 +2478,6 @@ } torq_ref_past = torq_ref; - totq_sen_past = torq.sen; break; @@ -2835,8 +2834,8 @@ if (flag_data_request[3] == HIGH) { //PWM - //CAN_TX_PWM((int16_t) (V[0]*100.0f)); //1500 - CAN_TX_PWM((int16_t) (f_future[1])); //1500 + CAN_TX_PWM((int16_t) (torq.ref)); //1500 +// CAN_TX_PWM((int16_t) (f_future[1])); //1500 } if (flag_data_request[4] == HIGH) {