Sungwoo Kim
Original Version of STM Board
Diff: main.cpp
- Revision:
- 179:d5377766d7ea
- Parent:
- 178:1074553d2f6f
- Child:
- 180:02be1711ee0b
diff -r 1074553d2f6f -r d5377766d7ea main.cpp
--- a/main.cpp Wed Nov 25 06:19:12 2020 +0000
+++ b/main.cpp Mon Dec 14 05:27:22 2020 +0000
@@ -1,4 +1,4 @@
-//201125_1
+//201214-1
#include "mbed.h"
#include "FastPWM.h"
#include "INIT_HW.h"
@@ -11,6 +11,7 @@
#include "FlashWriter.h"
#include <string>
#include <iostream>
+#include <cmath>
using namespace std;
Timer t;
@@ -197,72 +198,72 @@
float input_NN[num_input] = { 0.0f };
const float h1[num_input][16] = {
- {-1.3752100467681885f,0.9850414395332336f,0.12903714179992676f,-0.23237967491149902f,-1.5664141178131104f,0.36565715074539185f,-0.17472904920578003f,-0.3780243396759033f,-0.5318458676338196f,-0.6931207776069641f,-0.24376395344734192f,-3.9280478954315186f,-0.259793758392334f,-0.5339483022689819f,-0.2571594715118408f,-0.4960939884185791f},
- {-1.184814453125f,0.8704654574394226f,-0.3927857577800751f,-0.40050771832466125f,-1.4921096563339233f,0.5157514810562134f,-0.4054011106491089f,-0.3723553717136383f,-0.264297217130661f,-0.4598728120326996f,-0.2805407643318176f,-2.543855905532837f,0.24001067876815796f,-0.9143947958946228f,-0.20967772603034973f,-0.46271881461143494f},
- {-0.9112814664840698f,0.6714562773704529f,0.36391180753707886f,-0.6117674708366394f,-1.1069424152374268f,0.9712548851966858f,0.2872133255004883f,-0.059537652879953384f,-0.28272250294685364f,-0.4464949667453766f,-0.3544096052646637f,-1.0609019994735718f,0.0019771659281104803f,-0.2590618431568146f,0.05590221285820007f,-1.0201798677444458f},
- {-0.2895396053791046f,-0.01403216365724802f,0.39742106199264526f,0.20069332420825958f,-0.6715657114982605f,0.5361601710319519f,-0.11584559082984924f,0.3479021489620209f,-0.01779201813042164f,-0.017457854002714157f,0.08943979442119598f,0.845181941986084f,-0.15994226932525635f,-0.14522784948349f,-0.2961459457874298f,-0.7345172166824341f},
- {1.1968307495117188f,-0.8280011415481567f,-0.0051451027393341064f,0.39627134799957275f,0.3181920051574707f,-0.05303953215479851f,0.1294034719467163f,0.16528914868831635f,0.07129872590303421f,0.7101694941520691f,0.34140077233314514f,1.623598575592041f,-0.06323867291212082f,0.27155500650405884f,-0.3865056037902832f,-0.30631402134895325f},
- {3.009206533432007f,-3.122997522354126f,0.21773302555084229f,0.7692314386367798f,2.249300003051758f,-2.275317907333374f,0.16514194011688232f,0.11058115214109421f,0.17132356762886047f,2.417614459991455f,-0.25413116812705994f,2.2743594646453857f,-0.25959381461143494f,0.8003392219543457f,-0.4132833778858185f,1.4499995708465576f},
- {-0.7179104089736938f,0.8526995182037354f,-0.2501986622810364f,-0.875395655632019f,-0.5985659956932068f,2.8744397163391113f,0.06014183163642883f,-0.8439451456069946f,-0.05617062747478485f,-0.640657901763916f,0.14294663071632385f,3.381211519241333f,-0.1638868898153305f,-0.2370987981557846f,-0.15618428587913513f,-0.7710326910018921f},
- {-0.5504567623138428f,0.25199615955352783f,-0.26619744300842285f,-0.08971554040908813f,-0.3043712079524994f,0.903326153755188f,-0.17925891280174255f,-0.6943695545196533f,-0.25526830554008484f,0.20558907091617584f,0.09723648428916931f,2.3618743419647217f,0.38060376048088074f,-0.1769549399614334f,0.04303058981895447f,-0.12222103029489517f},
- {0.06810358166694641f,0.17962348461151123f,0.10248100757598877f,-0.30372604727745056f,0.4354613721370697f,-0.7276828289031982f,-0.07246989011764526f,-0.6118704676628113f,-0.42743170261383057f,1.453460693359375f,-0.30540645122528076f,1.6834946870803833f,0.23858578503131866f,0.21455851197242737f,-0.30555272102355957f,0.8063939213752747f},
- {-0.0713198110461235f,-0.4495846927165985f,0.18462657928466797f,0.05500239133834839f,0.7719969153404236f,-1.2848507165908813f,-0.2990540862083435f,-0.22474081814289093f,-0.1015893891453743f,0.7663454413414001f,-0.016399379819631577f,-0.03539150580763817f,0.38201630115509033f,-0.09081173688173294f,0.3484981060028076f,0.7820040583610535f},
- {0.33533185720443726f,0.13371603190898895f,0.37182438373565674f,0.39484986662864685f,0.35771995782852173f,-1.3015228509902954f,-0.20339298248291016f,-0.14145439863204956f,-0.5059540271759033f,0.8015591502189636f,0.34025880694389343f,-0.6485148072242737f,-0.05657690018415451f,0.09265313297510147f,0.26688337326049805f,0.2284013032913208f},
- {-0.4380135238170624f,-0.15535405278205872f,-0.40320003032684326f,0.0452105812728405f,0.23505118489265442f,-0.8950125575065613f,0.1533789038658142f,0.1997886300086975f,-0.24218705296516418f,0.45601460337638855f,-0.42232224345207214f,-0.5756524205207825f,0.15488487482070923f,-0.013335001654922962f,0.10493969917297363f,0.23898905515670776f},
- {-0.02198643982410431f,0.009633726440370083f,0.19847965240478516f,0.0860355794429779f,0.336037278175354f,-0.277248352766037f,-0.2404318004846573f,-0.07731665670871735f,-0.2765587270259857f,-0.1708865910768509f,-0.11712735891342163f,-0.7335506677627563f,0.10241620987653732f,-0.06552530825138092f,-0.22268100082874298f,-0.13819573819637299f},
- {0.020149538293480873f,-0.03840681165456772f,0.4127817749977112f,-0.2523573637008667f,-0.21765978634357452f,-0.4540342092514038f,-0.32001304626464844f,-0.5396589040756226f,-0.21919101476669312f,-0.2550864815711975f,-0.20302052795886993f,-0.2918916642665863f,-0.3021690249443054f,-0.19007131457328796f,0.0479682981967926f,0.28083792328834534f},
- {0.19955825805664062f,0.13080951571464539f,0.20282304286956787f,-0.21126452088356018f,0.15339018404483795f,-0.06708966940641403f,0.04202890396118164f,0.06752092391252518f,0.25217023491859436f,-0.20238792896270752f,-0.2873592674732208f,-0.42253321409225464f,0.11435109376907349f,-0.2376958578824997f,0.0668090283870697f,0.04261681064963341f},
- {-0.03747318685054779f,-0.29164087772369385f,-0.40439701080322266f,-0.3783135414123535f,-0.40124887228012085f,-0.026558339595794678f,-0.15568238496780396f,-0.08766567707061768f,0.21709603071212769f,-0.35802650451660156f,-0.4817592203617096f,-0.3809756338596344f,-0.2601439654827118f,-0.1273980438709259f,-0.08245879411697388f,0.014104350470006466f},
- {0.16147102415561676f,-0.5628581643104553f,0.16706281900405884f,-0.1172998696565628f,-0.03297983109951019f,-0.9104568362236023f,-0.041414469480514526f,0.1465616226196289f,0.04248951002955437f,0.007101915311068296f,-0.3614332377910614f,-0.48312774300575256f,0.12316523492336273f,0.3939219117164612f,0.29427415132522583f,0.3038717210292816f},
+ {-0.6526780724525452f,0.7266743183135986f,0.12903714179992676f,-0.23237967491149902f,-0.9990780353546143f,0.7280523777008057f,-0.17472904920578003f,-0.3780243396759033f,-0.5318458676338196f,-0.6824988722801208f,-0.24376395344734192f,-1.2289026975631714f,-0.259793758392334f,-0.5339483022689819f,-0.2571594715118408f,-0.9135630130767822f},
+{-0.6027737259864807f,0.5964298844337463f,-0.3927857577800751f,-0.40050771832466125f,-0.8369812965393066f,0.3892684876918793f,-0.4054011106491089f,-0.3723553717136383f,-0.264297217130661f,-0.22327673435211182f,-0.2805407643318176f,-0.9518417716026306f,0.24001067876815796f,-0.9143947958946228f,-0.20967772603034973f,-0.5016147494316101f},
+{-0.5003423094749451f,0.3953498303890228f,0.36391180753707886f,-0.6117674708366394f,-0.35621193051338196f,0.361548513174057f,0.2872133255004883f,-0.059537652879953384f,-0.28272250294685364f,-0.01068309135735035f,-0.3544096052646637f,-0.7833338975906372f,0.0019771659281104803f,-0.2590618431568146f,0.05590221285820007f,-0.6216552257537842f},
+{-0.1946873813867569f,-0.1142597571015358f,0.39742106199264526f,0.20069332420825958f,-0.016939876601099968f,-0.28546977043151855f,-0.11584559082984924f,0.3479021489620209f,-0.01779201813042164f,0.38596200942993164f,0.08943979442119598f,-0.122506283223629f,-0.15994226932525635f,-0.14522784948349f,-0.2961459457874298f,-0.11170782893896103f},
+{0.6847711205482483f,-0.3565831780433655f,-0.0051451027393341064f,0.39627134799957275f,0.45064908266067505f,-0.4076618254184723f,0.1294034719467163f,0.16528914868831635f,0.07129872590303421f,0.6038545370101929f,0.34140077233314514f,-0.026348024606704712f,-0.06323867291212082f,0.27155500650405884f,-0.3865056037902832f,-0.07509569823741913f},
+{1.483199954032898f,-1.5428612232208252f,0.21773302555084229f,0.7692314386367798f,1.2946882247924805f,-1.288189172744751f,0.16514194011688232f,0.11058115214109421f,0.17132356762886047f,1.1954634189605713f,-0.25413116812705994f,0.6743490695953369f,-0.25959381461143494f,0.8003392219543457f,-0.4132833778858185f,0.46136754751205444f},
+{-0.8217329382896423f,0.7089616060256958f,-0.2501986622810364f,-0.875395655632019f,-1.1229442358016968f,0.9968200922012329f,0.06014183163642883f,-0.8439451456069946f,-0.05617062747478485f,-0.4700510501861572f,0.14294663071632385f,0.6414217352867126f,-0.1638868898153305f,-0.2370987981557846f,-0.15618428587913513f,-0.625933825969696f},
+{-0.463580846786499f,0.05003904551267624f,-0.26619744300842285f,-0.08971554040908813f,-0.7169193625450134f,0.23099951446056366f,-0.17925891280174255f,-0.6943695545196533f,-0.25526830554008484f,-0.034314900636672974f,0.09723648428916931f,0.6139658093452454f,0.38060376048088074f,-0.1769549399614334f,0.04303058981895447f,-0.12070699036121368f},
+{0.1414080113172531f,0.1877378225326538f,0.10248100757598877f,-0.30372604727745056f,-0.12201636284589767f,0.022746695205569267f,-0.07246989011764526f,-0.6118704676628113f,-0.42743170261383057f,0.6341602206230164f,-0.30540645122528076f,1.4260834455490112f,0.23858578503131866f,0.21455851197242737f,-0.30555272102355957f,0.25425052642822266f},
+{0.10944874584674835f,-0.5504550337791443f,0.18462657928466797f,0.05500239133834839f,0.38295459747314453f,-0.040897730737924576f,-0.2990540862083435f,-0.22474081814289093f,-0.1015893891453743f,0.014746497385203838f,-0.016399379819631577f,0.7377010583877563f,0.38201630115509033f,-0.09081173688173294f,0.3484981060028076f,0.18850760161876678f},
+{0.6069241166114807f,-0.18160517513751984f,0.37182438373565674f,0.39484986662864685f,0.2415466606616974f,-0.2599449157714844f,-0.20339298248291016f,-0.14145439863204956f,-0.5059540271759033f,0.4776276648044586f,0.34025880694389343f,0.7560402154922485f,-0.05657690018415451f,0.09265313297510147f,0.26688337326049805f,-0.17793112993240356f},
+{-0.23725034296512604f,-0.5791488885879517f,-0.40320003032684326f,0.0452105812728405f,0.27909013628959656f,-0.27708446979522705f,0.1533789038658142f,0.1997886300086975f,-0.24218705296516418f,0.5504290461540222f,-0.42232224345207214f,0.8678635358810425f,0.15488487482070923f,-0.013335001654922962f,0.10493969917297363f,0.015422948636114597f},
+{-0.0060953362844884396f,-0.2789563536643982f,0.19847965240478516f,0.0860355794429779f,0.38908523321151733f,-0.01648942194879055f,-0.2404318004846573f,-0.07731665670871735f,-0.2765587270259857f,0.02809620089828968f,-0.11712735891342163f,0.11512216180562973f,0.10241620987653732f,-0.06552530825138092f,-0.22268100082874298f,-0.31739288568496704f},
+{-0.17114806175231934f,-0.10933959484100342f,0.4127817749977112f,-0.2523573637008667f,-0.24868325889110565f,-0.3818814158439636f,-0.32001304626464844f,-0.5396589040756226f,-0.21919101476669312f,-0.10611488670110703f,-0.20302052795886993f,-0.15492098033428192f,-0.3021690249443054f,-0.19007131457328796f,0.0479682981967926f,0.15465888381004333f},
+{0.04039499908685684f,0.07923353463411331f,0.20282304286956787f,-0.21126452088356018f,0.2369239777326584f,-0.20800116658210754f,0.04202890396118164f,0.06752092391252518f,0.25217023491859436f,0.06576687842607498f,-0.2873592674732208f,-0.705759584903717f,0.11435109376907349f,-0.2376958578824997f,0.0668090283870697f,0.16948974132537842f},
+{-0.1650223582983017f,-0.30675071477890015f,-0.40439701080322266f,-0.3783135414123535f,-0.27307847142219543f,-0.09857283532619476f,-0.15568238496780396f,-0.08766567707061768f,0.21709603071212769f,-0.02477334812283516f,-0.4817592203617096f,-0.5585556030273438f,-0.2601439654827118f,-0.1273980438709259f,-0.08245879411697388f,0.2804436981678009f},
+{-0.0361105240881443f,-0.32941773533821106f,0.16706281900405884f,-0.1172998696565628f,0.01294353324919939f,-0.5389335751533508f,-0.041414469480514526f,0.1465616226196289f,0.04248951002955437f,0.21840494871139526f,-0.3614332377910614f,-0.21242284774780273f,0.12316523492336273f,0.3939219117164612f,0.29427415132522583f,0.3293007016181946f},
};
const float h2[16][16] = {
- {0.33418041467666626f,2.9079723358154297f,-0.06966331601142883f,-0.009902028366923332f,2.812035322189331f,4.195849418640137f,-0.038234543055295944f,0.0904630720615387f,-0.43008196353912354f,-0.1415480375289917f,0.10707370936870575f,-0.20333009958267212f,-0.031739287078380585f,-3.289416551589966f,-1.172767996788025f,-0.6248859763145447f},
- {0.11070519685745239f,-2.3348469734191895f,0.057057321071624756f,-0.10134492069482803f,-5.252432346343994f,-1.3715343475341797f,-0.8426902890205383f,-0.2990124225616455f,-0.33757925033569336f,0.2895788848400116f,-7.361403465270996f,-0.858361005783081f,0.0845860168337822f,2.2169580459594727f,-3.0816471576690674f,-1.0190033912658691f},
- {-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.1391567885875702f,0.03293241187930107f,-0.2894435524940491f,0.25554025173187256f,0.08946844935417175f,0.1340968757867813f,-0.39087218046188354f,0.4122363030910492f,0.26211628317832947f,0.2953031361103058f,0.7025435566902161f,0.05798906087875366f,-0.5013425946235657f,-0.030901746824383736f,-0.06675297021865845f,-0.0446302704513073f},
- {-2.245600938796997f,1.6008015871047974f,-0.25313520431518555f,0.1469619870185852f,1.6072996854782104f,-1.3637473583221436f,-0.5957030653953552f,-0.08570799231529236f,-0.024399548768997192f,-0.39465832710266113f,-6.811410427093506f,-0.7977238893508911f,-0.022602174431085587f,-1.6454986333847046f,0.5441725850105286f,-0.7979243397712708f},
- {0.5210384130477905f,-0.11346126347780228f,-0.3519742488861084f,0.29306867718696594f,-0.3572634160518646f,1.75344979763031f,-0.744201123714447f,-0.019414573907852173f,-0.23047015070915222f,0.3579089343547821f,-2.398893117904663f,-0.48980283737182617f,-0.45957082509994507f,0.5487804412841797f,-0.7850930690765381f,-0.7663227319717407f},
- {-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},
- {-0.007747650612145662f,0.3667842745780945f,0.33083590865135193f,-0.38213321566581726f,-0.14358049631118774f,-0.2546055316925049f,-0.09043094515800476f,-0.3003333508968353f,0.2600560486316681f,-0.37898191809654236f,0.49078798294067383f,-0.3058888614177704f,-0.31673234701156616f,0.44664251804351807f,-0.9339061975479126f,-0.1061311885714531f},
- {-0.17694538831710815f,0.1767565757036209f,-0.11379697918891907f,-0.07116051763296127f,0.041274964809417725f,-0.517110288143158f,-0.10284432768821716f,0.0019084513187408447f,0.06103590130805969f,-0.38046833872795105f,-0.3456110656261444f,-0.24793067574501038f,-0.4166972041130066f,-0.30618393421173096f,-0.33565962314605713f,-0.48154377937316895f},
- {0.5043365359306335f,0.1850307136774063f,-0.13502129912376404f,-0.25706031918525696f,-0.8451530337333679f,-0.9415315389633179f,-0.2777014970779419f,-0.3632148504257202f,-0.13619378209114075f,0.16938945651054382f,1.5622152090072632f,-0.19312366843223572f,0.1250869333744049f,-0.1843482404947281f,1.0270578861236572f,-0.5840676426887512f},
- {-0.25693249702453613f,-0.010147050023078918f,0.0457797646522522f,-0.3549601435661316f,-0.03321319818496704f,-0.4391511380672455f,0.17873415350914001f,-0.20421427488327026f,-0.050184011459350586f,0.12480869889259338f,-0.22192376852035522f,0.3545852601528168f,-0.3647043704986572f,0.4194667637348175f,-0.3910166025161743f,-0.2837793529033661f},
- {2.3816981315612793f,0.4564768075942993f,0.3954955041408539f,0.11254638433456421f,1.1973353624343872f,1.804193377494812f,-0.8583166599273682f,-0.11795541644096375f,0.3487861454486847f,-0.32520344853401184f,-1.3897980451583862f,-0.5921090841293335f,-0.14213289320468903f,-0.26851770281791687f,1.597784161567688f,-0.5877130031585693f},
- {-0.267646849155426f,-0.20429572463035583f,-0.15798500180244446f,0.3999568819999695f,-0.37393757700920105f,0.35770177841186523f,0.11292675137519836f,-0.2947862446308136f,-0.3764709532260895f,0.2424570620059967f,-0.10294663906097412f,-0.2837170362472534f,0.41839322447776794f,-0.02792874164879322f,-0.13706544041633606f,0.11999254673719406f},
- {0.2282707840204239f,-0.13254989683628082f,-0.2018718123435974f,-0.07019486278295517f,0.07545611262321472f,-0.33204764127731323f,-0.4035329520702362f,-0.21416273713111877f,-0.24509364366531372f,0.19522181153297424f,0.47974857687950134f,-0.8069085478782654f,-0.40297332406044006f,0.23708327114582062f,0.5259239673614502f,-0.3548051416873932f},
- {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.5818981528282166f,0.014110059477388859f,0.12081471085548401f,-0.2973254919052124f,0.5392167568206787f,0.3773144781589508f,-0.39351940155029297f,0.31089308857917786f,-0.3893685042858124f,-0.02222958207130432f,-6.493835926055908f,-0.12242847681045532f,-0.2689415514469147f,-0.2518419325351715f,-0.1694900244474411f,-0.652117908000946f},
+ {-1.233986258506775f,1.6802256107330322f,-0.06966331601142883f,-0.005499060265719891f,-0.21907491981983185f,2.1583659648895264f,-0.038234543055295944f,0.0904630720615387f,-0.43008196353912354f,-0.1415480375289917f,-0.3416220247745514f,-0.20333009958267212f,-0.031739287078380585f,-1.315596342086792f,-0.19554688036441803f,-0.5387665629386902f},
+{0.38715651631355286f,-0.9310109615325928f,0.057057321071624756f,-0.06171989068388939f,-0.35503754019737244f,-0.85040283203125f,-0.8426902890205383f,-0.2990124225616455f,-0.33757925033569336f,0.2895788848400116f,-2.0213239192962646f,-0.858361005783081f,0.0845860168337822f,1.210322618484497f,-2.3236031532287598f,-0.8304648995399475f},
+{-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.1391567885875702f,0.03293241187930107f,-0.2894435524940491f,0.25554025173187256f,0.08946844935417175f,0.1340968757867813f,-0.39087218046188354f,0.4122363030910492f,0.26211628317832947f,0.2953031361103058f,0.7025435566902161f,0.05798906087875366f,-0.5013425946235657f,-0.030901746824383736f,-0.06675297021865845f,-0.0446302704513073f},
+{-1.1762845516204834f,0.8597515821456909f,-0.25313520431518555f,0.19948269426822662f,0.036378175020217896f,1.304591417312622f,-0.5957030653953552f,-0.08570799231529236f,-0.024399548768997192f,-0.39465832710266113f,-3.7821712493896484f,-0.7977238893508911f,-0.022602174431085587f,-1.1103366613388062f,-0.7675935626029968f,-0.537284791469574f},
+{0.6562917828559875f,-0.22383058071136475f,-0.3519742488861084f,0.3241978883743286f,-0.3015052080154419f,-0.2545274496078491f,-0.744201123714447f,-0.019414573907852173f,-0.23047015070915222f,0.3579089343547821f,-2.186229944229126f,-0.48980283737182617f,-0.45957082509994507f,0.5824981927871704f,-0.22895494103431702f,-0.7663227319717407f},
+{-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},
+{-0.007747650612145662f,0.3667842745780945f,0.33083590865135193f,-0.38213321566581726f,-0.14358049631118774f,-0.2546055316925049f,-0.09043094515800476f,-0.3003333508968353f,0.2600560486316681f,-0.37898191809654236f,0.49078798294067383f,-0.3058888614177704f,-0.31673234701156616f,0.44664251804351807f,-0.9339061975479126f,-0.1061311885714531f},
+{-0.17694538831710815f,0.1767565757036209f,-0.11379697918891907f,-0.07116051763296127f,0.041274964809417725f,-0.517110288143158f,-0.10284432768821716f,0.0019084513187408447f,0.06103590130805969f,-0.38046833872795105f,-0.3456110656261444f,-0.24793067574501038f,-0.4166972041130066f,-0.30618393421173096f,-0.33565962314605713f,-0.48154377937316895f},
+{0.17107118666172028f,0.5849232077598572f,-0.13502129912376404f,-0.25706031918525696f,0.12987366318702698f,-0.15911895036697388f,-0.2777014970779419f,-0.3632148504257202f,-0.13619378209114075f,0.16938945651054382f,0.8441702723503113f,-0.19312366843223572f,0.1250869333744049f,-0.7533880472183228f,0.4421834349632263f,-0.4836709797382355f},
+{-0.25693249702453613f,-0.010147050023078918f,0.0457797646522522f,-0.3549601435661316f,-0.03321319818496704f,-0.4391511380672455f,0.17873415350914001f,-0.20421427488327026f,-0.050184011459350586f,0.12480869889259338f,-0.22192376852035522f,0.3545852601528168f,-0.3647043704986572f,0.4194667637348175f,-0.3910166025161743f,-0.2837793529033661f},
+{0.6847327947616577f,-0.020395293831825256f,0.3954955041408539f,0.1992696225643158f,0.0033026933670043945f,0.22936652600765228f,-0.8583166599273682f,-0.11795541644096375f,0.3487861454486847f,-0.32520344853401184f,-3.0311942100524902f,-0.5921090841293335f,-0.14213289320468903f,-0.5792467594146729f,0.9252750873565674f,-0.22471986711025238f},
+{-0.267646849155426f,-0.20429572463035583f,-0.15798500180244446f,0.3999568819999695f,-0.37393757700920105f,0.35770177841186523f,0.11292675137519836f,-0.2947862446308136f,-0.3764709532260895f,0.2424570620059967f,-0.10294663906097412f,-0.2837170362472534f,0.41839322447776794f,-0.02792874164879322f,-0.13706544041633606f,0.11999254673719406f},
+{0.2282707840204239f,-0.13254989683628082f,-0.2018718123435974f,-0.07019486278295517f,0.07545611262321472f,-0.33204764127731323f,-0.4035329520702362f,-0.21416273713111877f,-0.24509364366531372f,0.19522181153297424f,0.47974857687950134f,-0.8069085478782654f,-0.40297332406044006f,0.23708327114582062f,0.5259239673614502f,-0.3548051416873932f},
+{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.9196669459342957f,0.6476971507072449f,0.12081471085548401f,-0.2587197721004486f,0.29976895451545715f,-0.28450649976730347f,-0.39351940155029297f,0.31089308857917786f,-0.3893685042858124f,-0.02222958207130432f,-4.534346580505371f,-0.12242847681045532f,-0.2689415514469147f,0.007701616268604994f,-0.33597034215927124f,-0.652117908000946f},
};
const float h3[16][16] = {
- {-0.36079341173171997f,-1.5671656131744385f,-0.870830237865448f,-0.4786093831062317f,-0.5575059056282043f,-0.773746132850647f,0.29977259039878845f,0.1743643879890442f,-0.5892294645309448f,-0.19605471193790436f,-0.27379146218299866f,0.06893575936555862f,2.2012624740600586f,-2.7650468349456787f,-1.8527313470840454f,1.5724681615829468f},
- {0.047732532024383545f,-0.23476845026016235f,-2.8836917877197266f,-0.42602431774139404f,-2.397148370742798f,1.254254937171936f,-0.3198729455471039f,-0.25123724341392517f,0.013695899397134781f,-0.5729482173919678f,0.09775999933481216f,-0.5587971806526184f,-0.9381403923034668f,-1.532671570777893f,-0.6944977045059204f,1.4258947372436523f},
- {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,-0.16969707608222961f,0.3056027889251709f,-0.380797415971756f,-0.17742466926574707f,0.10660405457019806f,0.20021501183509827f,0.07002416253089905f,-0.25412267446517944f,-0.365601509809494f,0.13192829489707947f,0.41021624207496643f,-0.13397035002708435f,0.0815882533788681f,0.15073642134666443f,0.10944337397813797f},
- {-0.39607733488082886f,-0.05481579899787903f,0.1976260244846344f,0.022423356771469116f,0.16892847418785095f,-0.6999471187591553f,0.16012099385261536f,0.21129152178764343f,-0.08640444278717041f,-0.11053556203842163f,-0.2634495496749878f,-0.31317979097366333f,-0.1530032455921173f,-0.24679358303546906f,0.22959044575691223f,-3.0342295169830322f},
- {-0.37449589371681213f,-0.10077743977308273f,-4.51036262512207f,-0.04963836818933487f,-0.8192825317382812f,-0.6951172947883606f,-0.19780586659908295f,-0.09049295634031296f,-0.9114404916763306f,-0.8623005151748657f,-0.4059421718120575f,-0.16295623779296875f,-5.017462730407715f,-1.1079373359680176f,0.34683844447135925f,0.6427209377288818f},
- {0.4110594093799591f,0.2715781033039093f,-0.38537508249282837f,0.37246426939964294f,-0.05490662157535553f,-0.009114405140280724f,-0.08670487999916077f,-0.25336313247680664f,-0.030661463737487793f,-0.06259563565254211f,-0.1344406008720398f,0.35313835740089417f,-0.13377737998962402f,0.25604528188705444f,0.3126353323459625f,-0.1528691202402115f},
- {-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.22762465476989746f,1.5232031345367432f,-0.2233445942401886f,1.524779200553894f,2.6997625827789307f,-0.08742031455039978f,0.05785742402076721f,-0.1277361363172531f,-0.37371426820755005f,-0.03133596479892731f,-0.30447322130203247f,-1.9101834297180176f,0.6954238414764404f,0.46117544174194336f,1.1762653589248657f},
- {-0.408692330121994f,0.07260357588529587f,-0.02147701010107994f,0.0922636091709137f,-0.1529182642698288f,-0.05657944083213806f,0.03285527229309082f,0.38763079047203064f,-0.20705322921276093f,-0.25883403420448303f,0.12809070944786072f,0.03996849060058594f,-0.6609845757484436f,-0.1087266057729721f,-0.10636871308088303f,-0.0133456289768219f},
- {-0.2991822361946106f,0.3794580399990082f,-0.08715943992137909f,-0.05932474136352539f,0.11478022485971451f,0.3007120192050934f,-0.11253207921981812f,0.34576353430747986f,0.04814547300338745f,-0.35770976543426514f,-0.044228196144104004f,-0.36229726672172546f,0.05380958318710327f,-0.13672849535942078f,0.35829514265060425f,-0.10585878044366837f},
- {-0.2675279378890991f,-0.1429223269224167f,-0.9267327189445496f,-0.1256239265203476f,0.49980443716049194f,-0.843601405620575f,-0.33532586693763733f,-0.31893211603164673f,-1.478386402130127f,-1.1926257610321045f,-0.5260127782821655f,-0.8384293913841248f,0.9772797226905823f,0.1813446283340454f,0.17612296342849731f,1.590701699256897f},
- {-0.015470266342163086f,-0.4976375699043274f,2.497199058532715f,0.14489558339118958f,0.11334121972322464f,0.3805958926677704f,0.02394937537610531f,-0.16103173792362213f,-0.677362859249115f,-0.8498573303222656f,-0.026575788855552673f,-0.427055299282074f,0.17177484929561615f,1.67837655544281f,0.08538639545440674f,-1.3552721738815308f},
- {0.1863725483417511f,0.13157431781291962f,-0.047772910445928574f,-0.49708226323127747f,0.7275430560112f,-0.145163431763649f,0.04860696196556091f,0.17769548296928406f,-0.050254471600055695f,0.19876523315906525f,-0.5504136085510254f,0.19592127203941345f,0.23928996920585632f,-0.12190091609954834f,-0.1250786930322647f,0.041962411254644394f},
+ {-0.36079341173171997f,-1.5671656131744385f,-1.394708514213562f,-0.4786093831062317f,0.8965405225753784f,-1.7463947534561157f,0.3332441747188568f,0.3036012351512909f,-0.5677260756492615f,0.04891335964202881f,-0.11870327591896057f,0.24536024034023285f,0.3745346665382385f,-2.4593026638031006f,-1.8527313470840454f,0.2478635460138321f},
+{0.047732532024383545f,-0.23476845026016235f,-1.3676623106002808f,-0.42602431774139404f,-0.48098695278167725f,0.4428248107433319f,-0.3198729455471039f,-0.09521423280239105f,0.2190944105386734f,-0.33393579721450806f,0.2562189996242523f,-0.3128277659416199f,1.0166772603988647f,2.1899778842926025f,-0.6944977045059204f,-0.7924290299415588f},
+{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,-0.16969707608222961f,0.3056027889251709f,-0.380797415971756f,-0.22033976018428802f,0.10660405457019806f,0.20021501183509827f,0.07002416253089905f,-0.25412267446517944f,-0.365601509809494f,0.13192829489707947f,0.41021624207496643f,-0.04961511492729187f,0.0815882533788681f,0.15073642134666443f,0.004613089840859175f},
+{-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,-0.10077743977308273f,-2.5148682594299316f,-0.04963836818933487f,0.6845123767852783f,-0.18825586140155792f,-0.18732719123363495f,0.06602504104375839f,-0.43416672945022583f,-0.19430825114250183f,-0.2474861443042755f,0.034658562391996384f,-1.109938621520996f,-0.23138557374477386f,0.34683844447135925f,-1.0771756172180176f},
+{0.4110594093799591f,0.2715781033039093f,-0.38537508249282837f,0.37246426939964294f,-0.05490662157535553f,-0.009114405140280724f,-0.08670487999916077f,-0.25336313247680664f,-0.030661463737487793f,-0.06259563565254211f,-0.1344406008720398f,0.35313835740089417f,-0.13377737998962402f,0.25604528188705444f,0.3126353323459625f,-0.1528691202402115f},
+{-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.22762465476989746f,0.9617792367935181f,-0.2233445942401886f,0.7956365942955017f,2.2177605628967285f,-0.08742031455039978f,0.05785742402076721f,-0.1277361363172531f,-0.37371426820755005f,-0.03133596479892731f,-0.30447322130203247f,-2.2757232189178467f,2.249086618423462f,0.46117544174194336f,0.18256771564483643f},
+{-0.408692330121994f,0.07260357588529587f,-0.02147701010107994f,0.0922636091709137f,-0.1529182642698288f,-0.05657944083213806f,0.03285527229309082f,0.38763079047203064f,-0.20705322921276093f,-0.25883403420448303f,0.12809070944786072f,0.03996849060058594f,-0.6609845757484436f,-0.1087266057729721f,-0.10636871308088303f,-0.0133456289768219f},
+{-0.2991822361946106f,0.3794580399990082f,-0.08715943992137909f,-0.05932474136352539f,0.11478022485971451f,0.3007120192050934f,-0.11253207921981812f,0.34576353430747986f,0.04814547300338745f,-0.35770976543426514f,-0.044228196144104004f,-0.36229726672172546f,0.05380958318710327f,-0.13672849535942078f,0.35829514265060425f,-0.10585878044366837f},
+{-0.2675279378890991f,-0.1429223269224167f,0.2487889677286148f,-0.1256239265203476f,0.010968400165438652f,-0.14684614539146423f,-0.32875844836235046f,-0.31893211603164673f,-0.5752826929092407f,-0.5557873845100403f,-0.5260127782821655f,-0.5674514770507812f,0.6408833265304565f,-0.28706681728363037f,0.264085978269577f,0.7492877840995789f},
+{-0.015470266342163086f,-0.4976375699043274f,0.6594434380531311f,0.14489558339118958f,-3.8797414302825928f,0.5358673930168152f,0.05196094512939453f,-0.008358269929885864f,-0.6339079737663269f,-0.38441595435142517f,0.13179203867912292f,-0.2711312472820282f,0.45540013909339905f,-1.2167638540267944f,0.08538639545440674f,-0.11235777288675308f},
+{0.1863725483417511f,0.13157431781291962f,-0.047772910445928574f,-0.49708226323127747f,0.6382043957710266f,-0.04539773613214493f,0.04860696196556091f,0.17769548296928406f,-0.050254471600055695f,0.19876523315906525f,-0.5504136085510254f,0.19592127203941345f,0.23928996920585632f,-0.0024931563530117273f,-0.1250786930322647f,0.041962411254644394f},
};
-const float hout[16] = { 0.45773375034332275f,0.5733839869499207f,-0.5152473449707031f,-0.035915032029151917f,-0.306951105594635f,0.30648893117904663f,-0.1005084440112114f,-0.08898112922906876f,-0.2034129500389099f,-0.05844772607088089f,-0.07081260532140732f,0.08596939593553543f,0.5426119565963745f,0.5678461194038391f,1.0715678930282593f,-0.25178417563438416f };
-
-const float b1[16] = { 0.5956194400787354f,1.4903173446655273f,-1.7145336866378784f,0.2382747083902359f,1.812330722808838f,1.1310549974441528f,-0.058932315558195114f,1.0475828647613525f,0.5750831365585327f,-1.1691874265670776f,0.564017653465271f,0.8430382013320923f,-0.3627738058567047f,-0.8757394552230835f,-1.087764859199524f,1.8534502983093262f };
-
-const float b2[16] = { -0.19254711270332336f,-1.2538373470306396f,-1.4564176797866821f,-0.6360846757888794f,-2.2159981727600098f,-0.5284036993980408f,0.07729385793209076f,-0.30969977378845215f,-1.20063054561615f,-1.912178635597229f,0.16195619106292725f,0.17741809785366058f,-0.3301374316215515f,1.8553483486175537f,-0.1654871553182602f,0.5861462950706482f };
-
-const float b3[16] = { -1.963319182395935f,-0.5293647646903992f,-0.10495924949645996f,-0.23769626021385193f,1.5550658702850342f,1.2147407531738281f,-0.47398853302001953f,-0.9554895758628845f,-0.067354217171669f,-0.006115084979683161f,-0.45877549052238464f,-0.290895938873291f,-1.0047727823257446f,1.1215981245040894f,-1.407442569732666f,-1.6007274389266968f };
-
-const float bout[1] = { -0.10839174687862396f };
+const float hout[16] = { 0.45773375034332275f,0.5733839869499207f,-0.6704624891281128f,-0.035915032029151917f,-0.26347172260284424f,0.4087086021900177f,0.0030125975608825684f,0.05245202034711838f,-0.0588601790368557f,-0.11953806132078171f,0.08675530552864075f,-0.15528400242328644f,0.2920013666152954f,0.373188316822052f,1.212536096572876f,-0.7900809645652771f };
+
+const float b1[16] = { 0.5264097452163696f,1.0703750848770142f,-1.7145336866378784f,0.2382747083902359f,1.0430195331573486f,0.8539859652519226f,-0.058932315558195114f,1.0475828647613525f,0.5750831365585327f,-0.7904888987541199f,0.564017653465271f,0.7548366189002991f,-0.3627738058567047f,-0.8757394552230835f,-1.087764859199524f,1.5956077575683594f };
+
+const float b2[16] = { 0.0007784571498632431f,-0.9460138082504272f,-1.4564176797866821f,-0.5824369788169861f,-0.6939148902893066f,0.038586944341659546f,0.07729385793209076f,-0.30969977378845215f,-1.20063054561615f,-1.912178635597229f,-0.15237417817115784f,0.17741809785366058f,-0.3301374316215515f,2.0171046257019043f,-0.16467103362083435f,0.6815829873085022f };
+
+const float b3[16] = { -1.963319182395935f,-0.5293647646903992f,0.3431461751461029f,-0.23769626021385193f,0.11889180541038513f,0.7799785137176514f,-0.45353031158447266f,-0.8127179145812988f,0.36436596512794495f,0.37977612018585205f,-0.3018096387386322f,-0.10593394935131073f,-0.13431759178638458f,0.4824405014514923f,-1.3276978731155396f,-0.026577739045023918f };
+
+const float bout[1] = { -0.17510902881622314f };
/////////////////////////////////////////////////////////////////////////////////////////////RL
float input_RL[num_input_RL] = { 0.0f };
@@ -302,47 +303,11 @@
float VALVE_POS_RAW_NN = 0.0f;
float DDV_JOINT_POS_FF(float REF_JOINT_VEL);
-
-float Critic_Network(float *arr)
-{
- float output1[num_hidden_unit1] = { 0.0f };
- float output2[num_hidden_unit2] = { 0.0f };
- float output = 0.0f;
- for (int index2 = 0; index2 < num_hidden_unit1; index2++) {
- for (int index1 = 0; index1 < num_input_RL; index1++) {
- output1[index2] = output1[index2] + hc1[index1][index2] * arr[index1];
- }
- //ReLU
- output1[index2] = output1[index2] + bc1[index2];
- hx_c_sum[index2] = output1[index2];
- if (output1[index2] < 0) {
- output1[index2] = 0;
- }
- //tanh
- //output1[index2] = tanh(output1[index2] + bc1[index2]);
- }
- for (int index2 = 0; index2 < num_hidden_unit2; index2++) {
- for (int index1 = 0; index1 < num_hidden_unit1; index1++) {
- output2[index2] = output2[index2] + hc2[index1][index2] * output1[index1];
- }
- //ReLU
- output2[index2] = output2[index2] + bc2[index2];
- hxh_c_sum[index2] = output2[index2];
- if (output2[index2] < 0) {
- output2[index2] = 0;
- }
- //tanh
- //output2[index2] = tanh(output2[index2] + bc2[index2]);
- }
- for (int index2 = 0; index2 < 1; index2++) {
- for (int index1 = 0; index1 < num_hidden_unit2; index1++) {
- output = output + hc3[index1] * output2[index1];
- }
- output = output + bc3;
- hxhh_c_sum = output;
- }
- return output;
-}
+/////////////////////////////////////////////RL tuning
+float Gradient_Limit = 0.5f;
+float gradient_rate_actor = 0.001f;
+float gradient_rate_critic = 0.001f;
+//////////////////////////////////////////////////////////////////////////////
float Critic_Network_Temp(float *arr)
{
@@ -424,6 +389,8 @@
//Softplus
mean = log(1.0f+exp(mean_before_SP));
deviation = log(1.0f+exp(deviation_before_SP));
+ logging2 = mean;
+ logging4 = deviation;
}
@@ -488,7 +455,6 @@
void update_Critic_Networks(float (*arr)[num_input_RL])
{
- float gradient_rate = 0.1f;
float G_hc1[num_input_RL][num_hidden_unit1] = {0.0f};
float G_bc1[num_hidden_unit1] = {0.0f};
for (int index2 = 0; index2 < num_hidden_unit1; index2++) {
@@ -505,7 +471,9 @@
G_hc1[index1][index2] = G_hc1[index1][index2] + 2.0f*(return_G[n]-V[n])*(-d_V_d_hc1);
}
G_hc1[index1][index2] = G_hc1[index1][index2] / batch_size;
- //hc1_temp[index1][index2] = hc1_temp[index1][index2] - gradient_rate * G_hc1[index1][index2];
+ if(G_hc1[index1][index2] > Gradient_Limit) G_hc1[index1][index2] = Gradient_Limit;
+ else if (G_hc1[index1][index2] < -Gradient_Limit) G_hc1[index1][index2] = -Gradient_Limit;
+ //hc1_temp[index1][index2] = hc1_temp[index1][index2] - gradient_rate_critic * G_hc1[index1][index2];
}
for (int n=0; n<batch_size; n++) {
float d_V_d_bc1 = 0.0f;
@@ -519,7 +487,9 @@
G_bc1[index2] = G_bc1[index2] + 2.0f*(return_G[n]-V[n])*(-d_V_d_bc1);
}
G_bc1[index2] = G_bc1[index2] / batch_size;
- //bc1_temp[index2] = bc1_temp[index2] - gradient_rate * G_bc1[index2];
+ if(G_bc1[index2] > Gradient_Limit) G_bc1[index2] = Gradient_Limit;
+ else if (G_bc1[index2] < -Gradient_Limit) G_bc1[index2] = -Gradient_Limit;
+ //bc1_temp[index2] = bc1_temp[index2] - gradient_rate_critic * G_bc1[index2];
}
@@ -537,7 +507,9 @@
G_hc2[index1][index2] = G_hc2[index1][index2] + 2.0f*(return_G[n]-V[n])*(-d_V_d_hc2);
}
G_hc2[index1][index2] = G_hc2[index1][index2] / batch_size;
- //hc2_temp[index1][index2] = hc2_temp[index1][index2] - gradient_rate * G_hc2[index1][index2];
+ if(G_hc2[index1][index2] > Gradient_Limit) G_hc2[index1][index2] = Gradient_Limit;
+ else if (G_hc2[index1][index2] < -Gradient_Limit) G_hc2[index1][index2] = -Gradient_Limit;
+ //hc2_temp[index1][index2] = hc2_temp[index1][index2] - gradient_rate_critic * G_hc2[index1][index2];
}
for (int n=0; n<batch_size; n++) {
float d_V_d_bc2 = 0.0f;
@@ -547,7 +519,9 @@
G_bc2[index2] = G_bc2[index2] + 2.0f*(return_G[n]-V[n])*(-d_V_d_bc2);
}
G_bc2[index2] = G_bc2[index2] / batch_size;
- //bc2_temp[index2] = bc2_temp[index2] - gradient_rate * G_bc2[index2];
+ if(G_bc2[index2] > Gradient_Limit) G_bc2[index2] = Gradient_Limit;
+ else if (G_bc2[index2] < -Gradient_Limit) G_bc2[index2] = -Gradient_Limit;
+ //bc2_temp[index2] = bc2_temp[index2] - gradient_rate_critic * G_bc2[index2];
}
float G_hc3[num_hidden_unit2]= {0.0f};
@@ -562,7 +536,9 @@
G_hc3[index1] = G_hc3[index1] + 2.0f*(return_G[n]-V[n])*(-d_V_d_hc3);
}
G_hc3[index1] = G_hc3[index1] / batch_size;
- //hc3_temp[index1] = hc3_temp[index1] - gradient_rate * G_hc3[index1];
+ if(G_hc3[index1] > Gradient_Limit) G_hc3[index1] = Gradient_Limit;
+ else if (G_hc3[index1] < -Gradient_Limit) G_hc3[index1] = -Gradient_Limit;
+ //hc3_temp[index1] = hc3_temp[index1] - gradient_rate_critic * G_hc3[index1];
}
for (int n=0; n<batch_size; n++) {
float d_V_d_bc3 = 0.0f;
@@ -570,34 +546,36 @@
G_bc3 = G_bc3 + 2.0f*(return_G[n]-V[n])*(-d_V_d_bc3);
}
G_bc3 = G_bc3 / batch_size;
- //bc3_temp = bc3_temp - gradient_rate * G_bc3;
+ if(G_bc3 > Gradient_Limit) G_bc3 = Gradient_Limit;
+ else if (G_bc3 < -Gradient_Limit) G_bc3 = -Gradient_Limit;
+ //bc3_temp = bc3_temp - gradient_rate_critic * G_bc3;
}
// Simultaneous Update
for (int index2 = 0; index2 < num_hidden_unit1; index2++) {
for (int index1 = 0; index1 < num_input_RL; index1++) {
- hc1_temp[index1][index2] = hc1_temp[index1][index2] - gradient_rate * G_hc1[index1][index2];
+ hc1_temp[index1][index2] = hc1_temp[index1][index2] - gradient_rate_critic * G_hc1[index1][index2];
}
- bc1_temp[index2] = bc1_temp[index2] - gradient_rate * G_bc1[index2];
+ bc1_temp[index2] = bc1_temp[index2] - gradient_rate_critic * G_bc1[index2];
}
for (int index2 = 0; index2 < num_hidden_unit2; index2++) {
for (int index1 = 0; index1 < num_hidden_unit1; index1++) {
- hc2_temp[index1][index2] = hc2_temp[index1][index2] - gradient_rate * G_hc2[index1][index2];
+ hc2_temp[index1][index2] = hc2_temp[index1][index2] - gradient_rate_critic * G_hc2[index1][index2];
}
- bc2_temp[index2] = bc2_temp[index2] - gradient_rate * G_bc2[index2];
+ bc2_temp[index2] = bc2_temp[index2] - gradient_rate_critic * G_bc2[index2];
}
for (int index2 = 0; index2 < 1; index2++) {
for (int index1 = 0; index1 < num_hidden_unit2; index1++) {
- hc3_temp[index1] = hc3_temp[index1] - gradient_rate * G_hc3[index1];
+ hc3_temp[index1] = hc3_temp[index1] - gradient_rate_critic * G_hc3[index1];
}
- bc3_temp = bc3_temp - gradient_rate * G_bc3;
+ bc3_temp = bc3_temp - gradient_rate_critic * G_bc3;
}
}
///////////////////////////Softplus//////////////////////////////////
void update_Actor_Networks(float (*arr)[num_input_RL])
{
- float gradient_rate = 0.1f;
+
float G_ha1[num_input_RL][num_hidden_unit1] = {0.0f};
float G_ba1[num_hidden_unit1] = {0.0f};
@@ -627,7 +605,9 @@
}
}
G_ha1[index1][index2] = -G_ha1[index1][index2] / batch_size;
- //ha1_temp[index1][index2] = ha1_temp[index1][index2] - gradient_rate * G_ha1[index1][index2];
+ if(G_ha1[index1][index2] > Gradient_Limit) G_ha1[index1][index2] = Gradient_Limit;
+ else if (G_ha1[index1][index2] < -Gradient_Limit) G_ha1[index1][index2] = -Gradient_Limit;
+ //ha1_temp[index1][index2] = ha1_temp[index1][index2] - gradient_rate_actor * G_ha1[index1][index2];
}
for (int n=0; n<batch_size; n++) {
@@ -653,7 +633,9 @@
}
}
G_ba1[index2] = -G_ba1[index2] / batch_size;
- //ba1_temp[index2] = ba1_temp[index2] - gradient_rate * G_ba1[index2];
+ if(G_ba1[index2] > Gradient_Limit) G_ba1[index2] = Gradient_Limit;
+ else if (G_ba1[index2] < -Gradient_Limit) G_ba1[index2] = -Gradient_Limit;
+ //ba1_temp[index2] = ba1_temp[index2] - gradient_rate_actor * G_ba1[index2];
}
float G_ha2[num_hidden_unit1][num_hidden_unit2] = {0.0f};
@@ -683,7 +665,9 @@
}
}
G_ha2[index1][index2] = -G_ha2[index1][index2] / batch_size;
- //ha2_temp[index1][index2] = ha2_temp[index1][index2] - gradient_rate * G_ha2[index1][index2];
+ if(G_ha2[index1][index2] > Gradient_Limit) G_ha2[index1][index2] = Gradient_Limit;
+ else if (G_ha2[index1][index2] < -Gradient_Limit) G_ha2[index1][index2] = -Gradient_Limit;
+ //ha2_temp[index1][index2] = ha2_temp[index1][index2] - gradient_rate_actor * G_ha2[index1][index2];
}
for (int n=0; n<batch_size; n++) {
@@ -706,7 +690,9 @@
}
}
G_ba2[index2] = -G_ba2[index2] / batch_size;
- //ba2_temp[index2] = ba2_temp[index2] - gradient_rate * G_ba2[index2];
+ if(G_ba2[index2] > Gradient_Limit) G_ba2[index2] = Gradient_Limit;
+ else if (G_ba2[index2] < -Gradient_Limit) G_ba2[index2] = -Gradient_Limit;
+ //ba2_temp[index2] = ba2_temp[index2] - gradient_rate_actor * G_ba2[index2];
}
float G_ha3[num_hidden_unit2][2] = {0.0f};
@@ -735,7 +721,9 @@
}
}
G_ha3[index1][index2] = -G_ha3[index1][index2] / batch_size;
- //ha3_temp[index1][index2] = ha3_temp[index1][index2] - gradient_rate * G_ha3[index1][index2];
+ if(G_ha3[index1][index2] > Gradient_Limit) G_ha3[index1][index2] = Gradient_Limit;
+ else if (G_ha3[index1][index2] < -Gradient_Limit) G_ha3[index1][index2] = -Gradient_Limit;
+ //ha3_temp[index1][index2] = ha3_temp[index1][index2] - gradient_rate_actor * G_ha3[index1][index2];
}
for (int n=0; n<batch_size; n++) {
@@ -757,29 +745,30 @@
}
}
G_ba3[index2] = -G_ba3[index2] / batch_size;
- //ba3_temp[index2] = ba3_temp[index2] - gradient_rate * G_ba3[index2];
+ if(G_ba3[index2] > Gradient_Limit) G_ba3[index2] = Gradient_Limit;
+ else if (G_ba3[index2] < -Gradient_Limit) G_ba3[index2] = -Gradient_Limit;
+ //ba3_temp[index2] = ba3_temp[index2] - gradient_rate_actor * G_ba3[index2];
}
// Simultaneous Update
for (int index2 = 0; index2 < num_hidden_unit1; index2++) {
for (int index1 = 0; index1 < num_input_RL; index1++) {
- ha1_temp[index1][index2] = ha1_temp[index1][index2] - gradient_rate * G_ha1[index1][index2];
+ ha1_temp[index1][index2] = ha1_temp[index1][index2] - gradient_rate_actor * G_ha1[index1][index2];
}
- ba1_temp[index2] = ba1_temp[index2] - gradient_rate * G_ba1[index2];
+ ba1_temp[index2] = ba1_temp[index2] - gradient_rate_actor * G_ba1[index2];
}
for (int index2 = 0; index2 < num_hidden_unit2; index2++) {
for (int index1 = 0; index1 < num_hidden_unit1; index1++) {
- ha2_temp[index1][index2] = ha2_temp[index1][index2] - gradient_rate * G_ha2[index1][index2];
+ ha2_temp[index1][index2] = ha2_temp[index1][index2] - gradient_rate_actor * G_ha2[index1][index2];
}
- ba2_temp[index2] = ba2_temp[index2] - gradient_rate * G_ba2[index2];
+ ba2_temp[index2] = ba2_temp[index2] - gradient_rate_actor * G_ba2[index2];
}
for (int index2 = 0; index2 < 2; index2++) {
for (int index1 = 0; index1 < num_hidden_unit2; index1++) {
- ha3_temp[index1][index2] = ha3_temp[index1][index2] - gradient_rate * G_ha3[index1][index2];
+ ha3_temp[index1][index2] = ha3_temp[index1][index2] - gradient_rate_actor * G_ha3[index1][index2];
}
- ba3_temp[index2] = ba3_temp[index2] - gradient_rate * G_ba3[index2];
+ ba3_temp[index2] = ba3_temp[index2] - gradient_rate_actor * G_ba3[index2];
}
-
}
float rand_normal(double mean, double stddev)
@@ -809,6 +798,31 @@
}
}
+float mean_adv(float x[], int size)
+{
+ float add = 0.0f;
+ float result;
+
+ for (int i=0; i<size; i++)
+ {
+ add += x[i];
+ }
+ result = (float) add/size;
+ return result;
+}
+float deviation_adv(float x[], int size)
+{
+ float sigma = 0.0f;
+ float resultDeb = 0.0f;
+
+ for (int k=0; k<size; k++)
+ {
+ sigma = pow((float)x[k]-mean_adv(x,size), (float)2.0f)/(size-1);
+ resultDeb += sqrt(sigma);
+ }
+ return resultDeb;
+}
+
void Overwirte_Critic_Networks()
{
@@ -935,36 +949,36 @@
for (int index2 = 0; index2 < num_hidden_unit1; index2++) {
for (int index1 = 0; index1 < num_input_RL; index1++) {
- hc1_temp[index1][index2] = (float) (rand()%100) * 0.01f ;
+ hc1_temp[index1][index2] = (float) (rand()%100) * 0.007f ;
}
- bc1_temp[index2] = (float) (rand()%100) * 0.01f;
+ bc1_temp[index2] = (float) (rand()%100) * 0.007f;
}
for (int index2 = 0; index2 < num_hidden_unit2; index2++) {
for (int index1 = 0; index1 < num_hidden_unit1; index1++) {
- hc2_temp[index1][index2] = (float) (rand()%100) * 0.01f;
+ hc2_temp[index1][index2] = (float) (rand()%100) * 0.007f;
}
- bc2[index2] = (float) (rand()%100) * 0.01f;
- hc3[index2] = (float) (rand()%100) * 0.01f;
+ bc2_temp[index2] = (float) (rand()%100) * 0.007f;
+ hc3_temp[index2] = (float) (rand()%100) * 0.007f;
}
- bc3 = (float) (rand()%100) * 0.01f;
+ bc3_temp = (float) (rand()%100) * 0.007f;
for (int index2 = 0; index2 < num_hidden_unit1; index2++) {
for (int index1 = 0; index1 < num_input_RL; index1++) {
- ha1[index1][index2] = (float) (rand()%100) * 0.01f;
+ ha1_temp[index1][index2] = (float) (rand()%100) * 0.007f;
}
- ba1[index2] = (float) (rand()%100) * 0.01f;
+ ba1_temp[index2] = (float) (rand()%100) * 0.007f;
}
for (int index2 = 0; index2 < num_hidden_unit2; index2++) {
for (int index1 = 0; index1 < num_hidden_unit1; index1++) {
- ha2[index1][index2] = (float) (rand()%100) * 0.01f;
+ ha2_temp[index1][index2] = (float) (rand()%100) * 0.007f;
}
- ba2[index2] = (float) (rand()%100) * 0.01f;
+ ba2_temp[index2] = (float) (rand()%100) * 0.007f;
}
for (int index2 = 0; index2 < 2; index2++) {
for (int index1 = 0; index1 < num_hidden_unit2; index1++) {
- ha3[index1][index2] = (float) (rand()%100) * 0.01f;
+ ha3_temp[index1][index2] = (float) (rand()%100) * 0.007f;
}
- ba3[index2] = (float) (rand()%100) * 0.01f;
+ ba3_temp[index2] = (float) (rand()%100) * 0.007f;
}
Overwirte_Critic_Networks();
@@ -1023,8 +1037,8 @@
input_NN[ind] = torq.sen / 10000.0f + 0.5f;
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+0.5f;
- input_NN[ind] = (f_future[time_interval*i+time_interval])/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+0.5f;
ind = ind + 1;
}
@@ -1117,7 +1131,7 @@
pi[n] = exp(-(action_array[n]-mean_array[n])*(action_array[n]-mean_array[n])/(2.0f*deviation_array[n]*deviation_array[n]))/(sqrt(2.0f*PI)*deviation_array[n]);
Actor_Network_Old(temp_array);
pi_old[n] = exp(-(action_array[n]-mean_old)*(action_array[n]-mean_old)/(2.0f*deviation_old*deviation_old))/(sqrt(2.0f*PI)*deviation_old);
- r[n] = exp(-0.25f * state_array[n][1] * state_array[n][1]);
+ r[n] = exp(-0.25f * 5.0f * state_array[n][1] * state_array[n][1]);
if(n == batch_size-1) return_G[n] = 0.0f;
else return_G[n] = gamma * return_G[n+1] + r[n];
if(n == batch_size-1) td_target[n] = r[n];
@@ -1125,7 +1139,15 @@
delta[n] = td_target[n] - V[n];
if(n == batch_size-1) advantage[n] = 0.0f;
else advantage[n] = gamma * lmbda * advantage[n+1] + delta[n];
+// return_G[n] = advantage[n] + V[n];
ratio[n] = pi[n]/pi_old[n];
+ }
+ float mean_advantage = 0.0f;
+ float dev_advantage = 0.0f;
+ mean_advantage = mean_adv(advantage, batch_size);
+ dev_advantage = deviation_adv(advantage, batch_size);
+ for (int n=batch_size-1; n>=0; n--) {
+ //advantage[n] = (advantage[n]-mean_advantage)/dev_advantage;
surr1[n] = ratio[n] * advantage[n];
if (ratio[n] > 1.0f + epsilon) {
surr2[n] = (1.0f + epsilon)*advantage[n];
@@ -2567,10 +2589,10 @@
deviation_array[RL_timer] = deviation;
action_array[RL_timer] = rand_normal(mean_array[RL_timer], deviation_array[RL_timer]);
- virt_pos = virt_pos + (action_array[RL_timer] - 3.0f) * 1000.0f * 0.0002f;
- if (virt_pos > 70.0f ) {
+ virt_pos = virt_pos + (action_array[RL_timer] - 5.0f) * 1000.0f * 0.0002f;
+ if (virt_pos > 70 ) {
virt_pos = 70.0f;
- } else if(virt_pos < -70.0f) {
+ }else if(virt_pos < -70) {
virt_pos = -70.0f;
}
@@ -2588,7 +2610,7 @@
Update_Case = 1;
Update_Done_Flag = 0;
logging1 = virt_pos;
-
+
if(batch >= num_batch) {
batch = 0;
RL_timer = 0;
@@ -2600,7 +2622,6 @@
}
else {
-
pos.err = pos.sen/(float)(ENC_PULSE_PER_POSITION) - virt_pos; //[mm]
float temp_array[3] = {0.0f};
temp_array[0] = virt_pos/70.0f; //-1.0~1.0
@@ -2609,12 +2630,12 @@
Actor_Network(temp_array);
action = rand_normal(mean, deviation);
//logging1 = action;
- logging2 = mean;
- logging4 = deviation;
- virt_pos = virt_pos + (action-3.0f) * 1000.0f * 0.0002f;
- if (virt_pos > 70.0f) {
+ //logging2 = mean;
+ //logging4 = deviation;
+ virt_pos = virt_pos + (action-5.0f) * 1000.0f * 0.0002f;
+ if (virt_pos > 70) {
virt_pos = 70.0f;
- } else if(virt_pos < -70.0f) {
+ }else if(virt_pos < -70) {
virt_pos = -70.0f;
}
@@ -2780,14 +2801,7 @@
}
}
if (flag_data_request[1] == HIGH) {
- //valve position
- double t_value = 0.0f;
- if(valve_pos.ref>=(float) VALVE_CENTER) {
- t_value = 10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
- } else {
- t_value = -10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
- }
- CAN_TX_TORQUE((int16_t) (t_value)); //1300
+ CAN_TX_TORQUE((int16_t) (return_G[0]*100.0f)); //1300
}
@@ -2816,15 +2830,13 @@
if (flag_data_request[3] == HIGH) {
//PWM
- //CAN_TX_PWM((int16_t) value); //1500
- CAN_TX_PWM((int16_t) gamma_hat); //1500
+ CAN_TX_PWM((int16_t) (V[0]*100.0f)); //1500
}
- //for (i = 0; i < 10000; i++) {
-// ;
-// }
+
if (flag_data_request[4] == HIGH) {
//valve position
- CAN_TX_VALVE_POSITION((int16_t) pos.sen/(float)(ENC_PULSE_PER_POSITION), (int16_t) virt_pos, (int16_t) 0, (int16_t) 0); //1600
+ CAN_TX_VALVE_POSITION((int16_t) pos.sen/(float)(ENC_PULSE_PER_POSITION), (int16_t) virt_pos, (int16_t) (logging2*1000.0f), (int16_t) (logging4*1000.0f)); //1600
+ //CAN_TX_VALVE_POSITION((int16_t) action_array[20], (int16_t) virt_pos, (int16_t) Update_Case*1000, (int16_t) (logging4*1000.0f)); //1600
}
// Others : Reference position, Reference FT, PWM, Current (ID:1300)