Sungwoo Kim
for learning
Diff: main.cpp
- Revision:
- 252:179a8c8791dc
- Parent:
- 251:4b0e0b1ff297
- Child:
- 253:0246366e0dcb
--- a/main.cpp Fri Jan 08 12:39:16 2021 +0000
+++ b/main.cpp Tue Feb 16 07:12:23 2021 +0000
@@ -1,4 +1,4 @@
-//210108_5 500Hz num_input 15 210105 data
+//After RaL
#include "mbed.h"
#include "FastPWM.h"
#include "INIT_HW.h"
@@ -200,58 +200,58 @@
const float h1[num_input][16] = {
{-0.034636590629816055f,-0.08510791510343552f,-0.3801865577697754f,0.03776714578270912f,0.2840934693813324f,-0.1901460736989975f,0.12296205759048462f,0.2284609079360962f,0.14127975702285767f,-0.1274729073047638f,-0.6219204664230347f,-0.3406560719013214f,0.26510030031204224f,0.04862070083618164f,-0.10759569704532623f,0.15142720937728882f},
-{-0.14657218754291534f,-0.34830769896507263f,-0.044384635984897614f,0.09097779542207718f,0.29299843311309814f,0.20048391819000244f,-0.3669555187225342f,0.577765166759491f,0.26391828060150146f,-0.6116664409637451f,-0.48967164754867554f,0.43264442682266235f,-0.2875143885612488f,0.20838868618011475f,0.4987501800060272f,-0.12150795757770538f},
-{-0.36621227860450745f,-0.5775251388549805f,0.25328558683395386f,-0.4689198136329651f,0.48624423146247864f,0.1806947886943817f,-0.02977663278579712f,0.46608680486679077f,-0.3409912884235382f,0.09726854413747787f,-0.5142548680305481f,0.39275670051574707f,-0.05826932191848755f,-0.1345062553882599f,0.35789212584495544f,-0.3863421678543091f},
-{-0.4689311385154724f,-0.5520263910293579f,0.11156318336725235f,-0.4460775554180145f,0.477514386177063f,-0.06286724656820297f,-0.20234209299087524f,-0.023230334743857384f,-0.39478203654289246f,0.22159633040428162f,-0.4476903975009918f,0.38044995069503784f,-0.17981481552124023f,0.2829986810684204f,0.041659221053123474f,0.010006793774664402f},
-{0.07658018171787262f,-0.13551065325737f,-0.04930756241083145f,-0.18194161355495453f,-0.30434608459472656f,-0.3774596154689789f,0.030568987131118774f,0.45075833797454834f,-0.34993308782577515f,0.00814745482057333f,0.271629273891449f,0.2641938328742981f,0.3928338885307312f,0.3520948886871338f,0.2681606411933899f,0.04452916979789734f},
-{0.1398475617170334f,-0.30324965715408325f,0.23745456337928772f,0.3544830083847046f,0.28628432750701904f,0.1924915313720703f,0.031466126441955566f,-0.05242495983839035f,-0.003589123487472534f,0.019043268635869026f,0.15009407699108124f,-0.33466243743896484f,-0.30160367488861084f,-0.2585424780845642f,-0.03835311904549599f,-0.620620608329773f},
-{0.7865412831306458f,0.34918898344039917f,-0.1276814490556717f,-0.7611914873123169f,0.07091358304023743f,-0.3924251198768616f,-0.030166536569595337f,0.13422253727912903f,0.3674931526184082f,-0.10135196149349213f,-0.21664942800998688f,0.23833078145980835f,-0.27123963832855225f,-0.08386284112930298f,-0.01957280933856964f,0.8007774353027344f},
-{0.6253918409347534f,0.3427291512489319f,0.2446969449520111f,-0.5424080491065979f,-0.4171082675457001f,-0.1339646875858307f,0.08264404535293579f,0.20667369663715363f,-0.16528040170669556f,0.10038857161998749f,0.2882964611053467f,0.19816702604293823f,0.37595224380493164f,-0.2227029651403427f,-0.20252738893032074f,0.4010050594806671f},
-{-0.10320301353931427f,-0.0669734925031662f,-0.3952587842941284f,0.014424772933125496f,0.06123732402920723f,0.12584073841571808f,-0.15519720315933228f,-0.12384038418531418f,0.20291584730148315f,-0.013697299174964428f,0.34133532643318176f,-0.1940154880285263f,-0.2515442967414856f,-0.23373761773109436f,-0.3340805768966675f,-0.005585795734077692f},
-{0.21723592281341553f,-0.3871166408061981f,-0.6769354343414307f,0.013060376048088074f,-0.3935699462890625f,-0.13699966669082642f,0.20312511920928955f,-0.24272094666957855f,0.28880298137664795f,0.012722134590148926f,0.6101362109184265f,0.1407831907272339f,-0.4085739254951477f,0.014354228973388672f,0.2012835443019867f,-0.41867175698280334f},
-{-0.348349392414093f,0.10514415800571442f,-0.29263970255851746f,0.14298589527606964f,-0.4824093282222748f,-0.16298454999923706f,-0.3280268609523773f,-0.14835195243358612f,-0.3790266811847687f,0.12452945858240128f,0.09102929383516312f,-0.017614006996154785f,-0.18276402354240417f,0.21967029571533203f,0.18263429403305054f,0.12917377054691315f},
-{-0.16717736423015594f,0.3654094338417053f,0.22285446524620056f,-0.2079508751630783f,0.15254132449626923f,-0.428370863199234f,0.12378334999084473f,0.042758338153362274f,-0.23888027667999268f,0.015105051919817924f,-0.03244645148515701f,0.07999944686889648f,-0.19709044694900513f,0.3904615044593811f,0.3818567395210266f,-0.1811094582080841f},
-{0.44972071051597595f,-0.028732342645525932f,-0.12203003466129303f,-0.16853941977024078f,-0.002657646546140313f,-0.27321964502334595f,-0.39909395575523376f,-0.22719086706638336f,-0.04522088170051575f,0.39755183458328247f,0.26120442152023315f,0.2014879584312439f,0.2502625584602356f,-0.049918800592422485f,-0.2698128819465637f,-0.39346325397491455f},
-{0.18112419545650482f,-0.0779980793595314f,-0.45121440291404724f,-0.5473600029945374f,-0.5366666913032532f,0.18330438435077667f,0.1997523307800293f,-0.09354571998119354f,-0.12511596083641052f,0.48358815908432007f,-0.172188401222229f,0.41015303134918213f,-0.3052411675453186f,-0.1892547309398651f,0.2224881649017334f,-0.10170229524374008f},
-{0.10940403491258621f,-0.23863136768341064f,-0.6232994794845581f,0.16091983020305634f,-0.304459810256958f,0.2962200939655304f,0.18491721153259277f,0.01318122074007988f,0.18640142679214478f,0.13724061846733093f,0.24989944696426392f,0.35947704315185547f,-0.332058310508728f,0.40907591581344604f,-0.28181812167167664f,-0.16143277287483215f},
+ {-0.14657218754291534f,-0.34830769896507263f,-0.044384635984897614f,0.09097779542207718f,0.29299843311309814f,0.20048391819000244f,-0.3669555187225342f,0.577765166759491f,0.26391828060150146f,-0.6116664409637451f,-0.48967164754867554f,0.43264442682266235f,-0.2875143885612488f,0.20838868618011475f,0.4987501800060272f,-0.12150795757770538f},
+ {-0.36621227860450745f,-0.5775251388549805f,0.25328558683395386f,-0.4689198136329651f,0.48624423146247864f,0.1806947886943817f,-0.02977663278579712f,0.46608680486679077f,-0.3409912884235382f,0.09726854413747787f,-0.5142548680305481f,0.39275670051574707f,-0.05826932191848755f,-0.1345062553882599f,0.35789212584495544f,-0.3863421678543091f},
+ {-0.4689311385154724f,-0.5520263910293579f,0.11156318336725235f,-0.4460775554180145f,0.477514386177063f,-0.06286724656820297f,-0.20234209299087524f,-0.023230334743857384f,-0.39478203654289246f,0.22159633040428162f,-0.4476903975009918f,0.38044995069503784f,-0.17981481552124023f,0.2829986810684204f,0.041659221053123474f,0.010006793774664402f},
+ {0.07658018171787262f,-0.13551065325737f,-0.04930756241083145f,-0.18194161355495453f,-0.30434608459472656f,-0.3774596154689789f,0.030568987131118774f,0.45075833797454834f,-0.34993308782577515f,0.00814745482057333f,0.271629273891449f,0.2641938328742981f,0.3928338885307312f,0.3520948886871338f,0.2681606411933899f,0.04452916979789734f},
+ {0.1398475617170334f,-0.30324965715408325f,0.23745456337928772f,0.3544830083847046f,0.28628432750701904f,0.1924915313720703f,0.031466126441955566f,-0.05242495983839035f,-0.003589123487472534f,0.019043268635869026f,0.15009407699108124f,-0.33466243743896484f,-0.30160367488861084f,-0.2585424780845642f,-0.03835311904549599f,-0.620620608329773f},
+ {0.7865412831306458f,0.34918898344039917f,-0.1276814490556717f,-0.7611914873123169f,0.07091358304023743f,-0.3924251198768616f,-0.030166536569595337f,0.13422253727912903f,0.3674931526184082f,-0.10135196149349213f,-0.21664942800998688f,0.23833078145980835f,-0.27123963832855225f,-0.08386284112930298f,-0.01957280933856964f,0.8007774353027344f},
+ {0.6253918409347534f,0.3427291512489319f,0.2446969449520111f,-0.5424080491065979f,-0.4171082675457001f,-0.1339646875858307f,0.08264404535293579f,0.20667369663715363f,-0.16528040170669556f,0.10038857161998749f,0.2882964611053467f,0.19816702604293823f,0.37595224380493164f,-0.2227029651403427f,-0.20252738893032074f,0.4010050594806671f},
+ {-0.10320301353931427f,-0.0669734925031662f,-0.3952587842941284f,0.014424772933125496f,0.06123732402920723f,0.12584073841571808f,-0.15519720315933228f,-0.12384038418531418f,0.20291584730148315f,-0.013697299174964428f,0.34133532643318176f,-0.1940154880285263f,-0.2515442967414856f,-0.23373761773109436f,-0.3340805768966675f,-0.005585795734077692f},
+ {0.21723592281341553f,-0.3871166408061981f,-0.6769354343414307f,0.013060376048088074f,-0.3935699462890625f,-0.13699966669082642f,0.20312511920928955f,-0.24272094666957855f,0.28880298137664795f,0.012722134590148926f,0.6101362109184265f,0.1407831907272339f,-0.4085739254951477f,0.014354228973388672f,0.2012835443019867f,-0.41867175698280334f},
+ {-0.348349392414093f,0.10514415800571442f,-0.29263970255851746f,0.14298589527606964f,-0.4824093282222748f,-0.16298454999923706f,-0.3280268609523773f,-0.14835195243358612f,-0.3790266811847687f,0.12452945858240128f,0.09102929383516312f,-0.017614006996154785f,-0.18276402354240417f,0.21967029571533203f,0.18263429403305054f,0.12917377054691315f},
+ {-0.16717736423015594f,0.3654094338417053f,0.22285446524620056f,-0.2079508751630783f,0.15254132449626923f,-0.428370863199234f,0.12378334999084473f,0.042758338153362274f,-0.23888027667999268f,0.015105051919817924f,-0.03244645148515701f,0.07999944686889648f,-0.19709044694900513f,0.3904615044593811f,0.3818567395210266f,-0.1811094582080841f},
+ {0.44972071051597595f,-0.028732342645525932f,-0.12203003466129303f,-0.16853941977024078f,-0.002657646546140313f,-0.27321964502334595f,-0.39909395575523376f,-0.22719086706638336f,-0.04522088170051575f,0.39755183458328247f,0.26120442152023315f,0.2014879584312439f,0.2502625584602356f,-0.049918800592422485f,-0.2698128819465637f,-0.39346325397491455f},
+ {0.18112419545650482f,-0.0779980793595314f,-0.45121440291404724f,-0.5473600029945374f,-0.5366666913032532f,0.18330438435077667f,0.1997523307800293f,-0.09354571998119354f,-0.12511596083641052f,0.48358815908432007f,-0.172188401222229f,0.41015303134918213f,-0.3052411675453186f,-0.1892547309398651f,0.2224881649017334f,-0.10170229524374008f},
+ {0.10940403491258621f,-0.23863136768341064f,-0.6232994794845581f,0.16091983020305634f,-0.304459810256958f,0.2962200939655304f,0.18491721153259277f,0.01318122074007988f,0.18640142679214478f,0.13724061846733093f,0.24989944696426392f,0.35947704315185547f,-0.332058310508728f,0.40907591581344604f,-0.28181812167167664f,-0.16143277287483215f},
};
const float h2[16][16] = {
{0.04833245277404785f,0.8406158089637756f,0.3808237612247467f,0.3637321889400482f,0.644477128982544f,0.35276496410369873f,-1.0592999458312988f,-0.44195684790611267f,-2.155895948410034f,0.42358145117759705f,0.21362772583961487f,0.046894848346710205f,0.1278792917728424f,-0.37168267369270325f,0.04797150567173958f,0.41920337080955505f},
-{-0.0274239182472229f,0.9419358372688293f,-0.34082740545272827f,0.2642888128757477f,0.7276898622512817f,0.002614683471620083f,-1.1622995138168335f,0.30911850929260254f,-0.6000240445137024f,0.026576727628707886f,-0.29539918899536133f,-0.20509253442287445f,-0.1801471710205078f,-0.5503743290901184f,-0.09623444825410843f,0.16039973497390747f},
-{-0.35354936122894287f,-0.09730152785778046f,-0.24224385619163513f,-0.41592511534690857f,0.07027829438447952f,0.013607672415673733f,0.22128823399543762f,-0.10003288835287094f,-1.014364242553711f,-0.26868098974227905f,0.21410873532295227f,-0.228165864944458f,-0.05225086212158203f,-0.7968334555625916f,0.25415247678756714f,0.2862052619457245f},
-{-0.11117833852767944f,0.46299421787261963f,0.11902371048927307f,0.35618194937705994f,0.15819178521633148f,-0.5194926261901855f,-0.1352825164794922f,0.4493905305862427f,-0.4192030429840088f,-0.4216589629650116f,-0.4289160966873169f,-0.3305668830871582f,0.43245211243629456f,-0.18652261793613434f,-0.4980774223804474f,0.2913265824317932f},
-{0.39269885420799255f,-0.17476780712604523f,-0.07237827777862549f,0.09812912344932556f,-0.33486536145210266f,-0.47770196199417114f,0.9116359949111938f,0.20927821099758148f,0.6805155873298645f,-0.08264759182929993f,-0.15345615148544312f,0.3926151692867279f,-0.28366416692733765f,0.9803341031074524f,-0.7951028943061829f,-0.38231852650642395f},
-{-0.02339586615562439f,-0.21557484567165375f,0.0382651686668396f,-0.10381102561950684f,0.20424866676330566f,0.0569344200193882f,-0.15795065462589264f,-0.2686183750629425f,0.02425282634794712f,0.2855292856693268f,0.3241807520389557f,0.08525893092155457f,-0.07454681396484375f,-0.3016861081123352f,0.30999064445495605f,-0.2047392874956131f},
-{0.06456553936004639f,-0.29620951414108276f,-0.28450071811676025f,-0.3608437776565552f,0.14704784750938416f,-0.36015424132347107f,-0.315707266330719f,-0.2272750288248062f,-0.16718602180480957f,-0.3622615337371826f,0.3609362542629242f,0.3271537125110626f,-0.1134117841720581f,-0.24684466421604156f,-0.3105827271938324f,-0.29518258571624756f},
-{0.40061601996421814f,-0.2499115914106369f,-0.42576566338539124f,0.2771916687488556f,0.1725580394268036f,0.24982130527496338f,0.032843999564647675f,-0.13448423147201538f,-0.30728211998939514f,-0.24666368961334229f,0.034479737281799316f,0.11201921105384827f,-0.3116337060928345f,0.20088200271129608f,0.07602878659963608f,-0.32380038499832153f},
-{0.01738646626472473f,0.42550787329673767f,0.16973963379859924f,-0.19240723550319672f,-0.03196948766708374f,-0.12532320618629456f,-0.1082024872303009f,0.3715610206127167f,-0.23925665020942688f,0.15291878581047058f,0.21075895428657532f,-0.06213498115539551f,0.41508081555366516f,-0.27582913637161255f,0.06960871815681458f,-0.020523756742477417f},
-{0.2798302471637726f,0.3964071571826935f,0.23923692107200623f,0.1867220103740692f,-0.3472009599208832f,-0.4335924983024597f,-0.35373935103416443f,0.16244973242282867f,-0.7462126612663269f,-0.2594975531101227f,-0.315661758184433f,-0.2793809175491333f,0.27418074011802673f,-0.10312201827764511f,0.4119209051132202f,-0.05471741408109665f},
-{-0.14383485913276672f,1.081695318222046f,-0.404108464717865f,-0.22689129412174225f,-0.3717469274997711f,-0.41622504591941833f,-0.8190008401870728f,0.5285307168960571f,-3.050868034362793f,-0.16521230340003967f,-0.0948835015296936f,-0.3281487226486206f,-0.32793286442756653f,-1.8892099857330322f,-0.8883360028266907f,-0.44808080792427063f},
-{-0.3993529975414276f,0.41353103518486023f,0.2625258266925812f,-0.40873271226882935f,0.3280826508998871f,0.10209885239601135f,0.22779449820518494f,0.27592143416404724f,-0.24499990046024323f,0.11430332064628601f,0.1062566339969635f,-0.3577694296836853f,-0.05476459860801697f,0.17576757073402405f,0.21356281638145447f,-0.21355248987674713f},
-{0.26238974928855896f,0.13264122605323792f,0.17779704928398132f,0.3172527849674225f,-0.07196182012557983f,0.3440147936344147f,-0.20966216921806335f,0.19133642315864563f,-0.40352103114128113f,-0.01865154504776001f,0.4056650698184967f,0.4148031175136566f,-0.3938596844673157f,-0.15683236718177795f,-0.2691075801849365f,-0.31552961468696594f},
-{0.04255214333534241f,-0.10101297497749329f,-0.20923052728176117f,0.2541695535182953f,0.41217753291130066f,-0.301749050617218f,0.16364464163780212f,-0.16757863759994507f,-0.33475184440612793f,-0.3079894781112671f,-0.15280935168266296f,-0.3279605209827423f,-0.21071602404117584f,-0.09943103790283203f,0.25322189927101135f,-0.047403186559677124f},
-{-0.3407605290412903f,-1.6339330673217773f,-0.08496147394180298f,0.18828937411308289f,-1.2263431549072266f,-1.4933935403823853f,1.361119031906128f,1.3889564275741577f,1.7450194358825684f,0.3321857750415802f,0.10198095440864563f,0.14103266596794128f,0.07074931263923645f,1.626432180404663f,-1.5846821069717407f,-1.6313681602478027f},
-{-0.430133581161499f,2.0306808948516846f,0.3826504647731781f,0.4254762828350067f,1.4177906513214111f,0.9060602188110352f,-2.047635793685913f,-0.610949695110321f,-4.190510272979736f,-0.26547297835350037f,0.17560508847236633f,0.4241798222064972f,-0.07467690110206604f,-3.596113681793213f,0.9893742203712463f,1.3326054811477661f},
+ {-0.0274239182472229f,0.9419358372688293f,-0.34082740545272827f,0.2642888128757477f,0.7276898622512817f,0.002614683471620083f,-1.1622995138168335f,0.30911850929260254f,-0.6000240445137024f,0.026576727628707886f,-0.29539918899536133f,-0.20509253442287445f,-0.1801471710205078f,-0.5503743290901184f,-0.09623444825410843f,0.16039973497390747f},
+ {-0.35354936122894287f,-0.09730152785778046f,-0.24224385619163513f,-0.41592511534690857f,0.07027829438447952f,0.013607672415673733f,0.22128823399543762f,-0.10003288835287094f,-1.014364242553711f,-0.26868098974227905f,0.21410873532295227f,-0.228165864944458f,-0.05225086212158203f,-0.7968334555625916f,0.25415247678756714f,0.2862052619457245f},
+ {-0.11117833852767944f,0.46299421787261963f,0.11902371048927307f,0.35618194937705994f,0.15819178521633148f,-0.5194926261901855f,-0.1352825164794922f,0.4493905305862427f,-0.4192030429840088f,-0.4216589629650116f,-0.4289160966873169f,-0.3305668830871582f,0.43245211243629456f,-0.18652261793613434f,-0.4980774223804474f,0.2913265824317932f},
+ {0.39269885420799255f,-0.17476780712604523f,-0.07237827777862549f,0.09812912344932556f,-0.33486536145210266f,-0.47770196199417114f,0.9116359949111938f,0.20927821099758148f,0.6805155873298645f,-0.08264759182929993f,-0.15345615148544312f,0.3926151692867279f,-0.28366416692733765f,0.9803341031074524f,-0.7951028943061829f,-0.38231852650642395f},
+ {-0.02339586615562439f,-0.21557484567165375f,0.0382651686668396f,-0.10381102561950684f,0.20424866676330566f,0.0569344200193882f,-0.15795065462589264f,-0.2686183750629425f,0.02425282634794712f,0.2855292856693268f,0.3241807520389557f,0.08525893092155457f,-0.07454681396484375f,-0.3016861081123352f,0.30999064445495605f,-0.2047392874956131f},
+ {0.06456553936004639f,-0.29620951414108276f,-0.28450071811676025f,-0.3608437776565552f,0.14704784750938416f,-0.36015424132347107f,-0.315707266330719f,-0.2272750288248062f,-0.16718602180480957f,-0.3622615337371826f,0.3609362542629242f,0.3271537125110626f,-0.1134117841720581f,-0.24684466421604156f,-0.3105827271938324f,-0.29518258571624756f},
+ {0.40061601996421814f,-0.2499115914106369f,-0.42576566338539124f,0.2771916687488556f,0.1725580394268036f,0.24982130527496338f,0.032843999564647675f,-0.13448423147201538f,-0.30728211998939514f,-0.24666368961334229f,0.034479737281799316f,0.11201921105384827f,-0.3116337060928345f,0.20088200271129608f,0.07602878659963608f,-0.32380038499832153f},
+ {0.01738646626472473f,0.42550787329673767f,0.16973963379859924f,-0.19240723550319672f,-0.03196948766708374f,-0.12532320618629456f,-0.1082024872303009f,0.3715610206127167f,-0.23925665020942688f,0.15291878581047058f,0.21075895428657532f,-0.06213498115539551f,0.41508081555366516f,-0.27582913637161255f,0.06960871815681458f,-0.020523756742477417f},
+ {0.2798302471637726f,0.3964071571826935f,0.23923692107200623f,0.1867220103740692f,-0.3472009599208832f,-0.4335924983024597f,-0.35373935103416443f,0.16244973242282867f,-0.7462126612663269f,-0.2594975531101227f,-0.315661758184433f,-0.2793809175491333f,0.27418074011802673f,-0.10312201827764511f,0.4119209051132202f,-0.05471741408109665f},
+ {-0.14383485913276672f,1.081695318222046f,-0.404108464717865f,-0.22689129412174225f,-0.3717469274997711f,-0.41622504591941833f,-0.8190008401870728f,0.5285307168960571f,-3.050868034362793f,-0.16521230340003967f,-0.0948835015296936f,-0.3281487226486206f,-0.32793286442756653f,-1.8892099857330322f,-0.8883360028266907f,-0.44808080792427063f},
+ {-0.3993529975414276f,0.41353103518486023f,0.2625258266925812f,-0.40873271226882935f,0.3280826508998871f,0.10209885239601135f,0.22779449820518494f,0.27592143416404724f,-0.24499990046024323f,0.11430332064628601f,0.1062566339969635f,-0.3577694296836853f,-0.05476459860801697f,0.17576757073402405f,0.21356281638145447f,-0.21355248987674713f},
+ {0.26238974928855896f,0.13264122605323792f,0.17779704928398132f,0.3172527849674225f,-0.07196182012557983f,0.3440147936344147f,-0.20966216921806335f,0.19133642315864563f,-0.40352103114128113f,-0.01865154504776001f,0.4056650698184967f,0.4148031175136566f,-0.3938596844673157f,-0.15683236718177795f,-0.2691075801849365f,-0.31552961468696594f},
+ {0.04255214333534241f,-0.10101297497749329f,-0.20923052728176117f,0.2541695535182953f,0.41217753291130066f,-0.301749050617218f,0.16364464163780212f,-0.16757863759994507f,-0.33475184440612793f,-0.3079894781112671f,-0.15280935168266296f,-0.3279605209827423f,-0.21071602404117584f,-0.09943103790283203f,0.25322189927101135f,-0.047403186559677124f},
+ {-0.3407605290412903f,-1.6339330673217773f,-0.08496147394180298f,0.18828937411308289f,-1.2263431549072266f,-1.4933935403823853f,1.361119031906128f,1.3889564275741577f,1.7450194358825684f,0.3321857750415802f,0.10198095440864563f,0.14103266596794128f,0.07074931263923645f,1.626432180404663f,-1.5846821069717407f,-1.6313681602478027f},
+ {-0.430133581161499f,2.0306808948516846f,0.3826504647731781f,0.4254762828350067f,1.4177906513214111f,0.9060602188110352f,-2.047635793685913f,-0.610949695110321f,-4.190510272979736f,-0.26547297835350037f,0.17560508847236633f,0.4241798222064972f,-0.07467690110206604f,-3.596113681793213f,0.9893742203712463f,1.3326054811477661f},
};
const float h3[16][16] = {
{0.37046024203300476f,0.04798617959022522f,0.392690509557724f,-0.20555028319358826f,-0.04246211051940918f,-0.37363746762275696f,0.015186339616775513f,-0.061466217041015625f,-0.13781791925430298f,-0.11356508731842041f,0.30632129311561584f,-0.03401225805282593f,0.14026448130607605f,-0.41080594062805176f,-0.021881461143493652f,-0.1677863597869873f},
-{-0.21526779234409332f,-1.1129813194274902f,-0.12068668007850647f,0.09446603804826736f,-0.015299053862690926f,0.23865419626235962f,-0.16593411564826965f,-1.1282691955566406f,-0.2572314739227295f,0.025441110134124756f,0.08321377635002136f,-0.22488360106945038f,0.10835165530443192f,0.01774216815829277f,-0.05000334233045578f,-0.3466002941131592f},
-{-0.08673667907714844f,0.23056277632713318f,0.42949649691581726f,-0.3032870888710022f,0.4159357249736786f,0.03299093246459961f,-0.035138070583343506f,0.00458759069442749f,-0.2919609248638153f,0.13052961230278015f,-0.09829097986221313f,-0.24485144019126892f,0.3602481186389923f,0.3898719251155853f,-0.0538104772567749f,0.07017943263053894f},
-{0.2464427649974823f,0.39936527609825134f,0.21032699942588806f,-0.2072734385728836f,0.19923558831214905f,0.2847035825252533f,-0.22418056428432465f,-0.43023446202278137f,-0.21198244392871857f,0.23950514197349548f,-0.3551802337169647f,0.11705091595649719f,0.18541833758354187f,-0.023748815059661865f,0.1462016999721527f,-0.08909019827842712f},
-{0.34622398018836975f,-1.2129888534545898f,-0.20742447674274445f,-0.032929010689258575f,0.08672287315130234f,-0.08068259805440903f,0.45541882514953613f,-2.018517017364502f,0.10944971442222595f,0.04921719431877136f,-0.20607680082321167f,0.21179592609405518f,0.4078998863697052f,0.09382607787847519f,-0.6051720380783081f,-0.19378763437271118f},
-{0.07416597008705139f,-0.1509564220905304f,-0.3286089599132538f,0.2575420141220093f,-0.32409292459487915f,-0.04360950365662575f,-0.05032980069518089f,0.03308649733662605f,-0.14956387877464294f,0.06287035346031189f,-0.18942220509052277f,-0.22120867669582367f,0.36484384536743164f,0.34050697088241577f,0.033385224640369415f,0.028106003999710083f},
-{-0.24521887302398682f,0.3061828017234802f,-0.09969070553779602f,-0.835078239440918f,-1.1966766119003296f,0.7502152919769287f,-0.6856613159179688f,0.31733009219169617f,0.16320112347602844f,-0.08684051036834717f,-0.3433442711830139f,-0.3093454837799072f,-0.3787883520126343f,-0.6157321929931641f,0.27898073196411133f,-0.3274548649787903f},
-{-0.037380099296569824f,-0.053829364478588104f,-0.23550507426261902f,-0.32296717166900635f,0.1419943869113922f,0.3212539851665497f,-0.5291021466255188f,-0.3196764588356018f,0.23772326111793518f,-0.15706628561019897f,-0.03474658727645874f,-0.42883461713790894f,-0.43303486704826355f,-0.46498894691467285f,0.23065359890460968f,-0.06882688403129578f},
-{0.22414228320121765f,0.6167948842048645f,0.17398694157600403f,-1.2930227518081665f,-0.07601547241210938f,1.1093199253082275f,-0.4207235872745514f,0.2633194625377655f,-0.16447538137435913f,0.31832489371299744f,-0.01876583695411682f,0.425087034702301f,-1.1076250076293945f,0.1036156564950943f,-0.3688891530036926f,-0.37546294927597046f},
-{-0.37478217482566833f,-0.10878506302833557f,-0.33313462138175964f,0.08732470870018005f,-0.4272407591342926f,0.42993465065956116f,-0.3373659551143646f,-0.1347610354423523f,0.27883973717689514f,0.15846404433250427f,0.23034706711769104f,0.2679099142551422f,0.26554760336875916f,-0.39576658606529236f,0.27645888924598694f,0.3027357757091522f},
-{0.01714116334915161f,-0.41286614537239075f,-0.30441972613334656f,0.09042176604270935f,0.305792897939682f,0.25930657982826233f,-0.0077957212924957275f,0.2839451730251312f,0.06527414917945862f,0.30544695258140564f,-0.3269312381744385f,-0.23118992149829865f,-0.36912286281585693f,0.18185165524482727f,0.06934896111488342f,0.20831605792045593f},
-{-0.16316500306129456f,0.09996911883354187f,-0.11835616827011108f,0.22180476784706116f,0.2730047404766083f,-0.39772462844848633f,0.3122676908969879f,0.025347262620925903f,-0.0864812433719635f,-0.2908649444580078f,0.05068853497505188f,-0.23324716091156006f,-0.27006956934928894f,0.3646886646747589f,-0.22474992275238037f,0.03918728232383728f},
-{-0.2255314439535141f,0.0861915647983551f,-0.37378355860710144f,0.09740594029426575f,-0.17835164070129395f,-0.28151828050613403f,0.08090576529502869f,0.27042338252067566f,0.10048076510429382f,0.3275263011455536f,0.3301452696323395f,-0.33213010430336f,0.18207183480262756f,0.224577397108078f,-0.2358287274837494f,0.16939416527748108f},
-{0.18313375115394592f,3.035151958465576f,0.15208670496940613f,-2.073854684829712f,0.9186175465583801f,0.35607829689979553f,-1.5632147789001465f,-0.4666653871536255f,-0.15266752243041992f,-0.3787156641483307f,-0.23417361080646515f,0.41418468952178955f,-2.05690336227417f,-2.3053674697875977f,-0.29849734902381897f,-0.3566729426383972f},
-{0.19047978520393372f,0.24750559031963348f,-0.1217426061630249f,0.5728633403778076f,-0.08453264087438583f,0.24713492393493652f,0.41920486092567444f,0.4218023419380188f,-0.27937382459640503f,0.06346243619918823f,-0.3896019458770752f,0.36481839418411255f,0.7093786597251892f,-0.09001748263835907f,-0.4630468785762787f,-0.3792039752006531f},
-{-0.3329339325428009f,-0.4124312996864319f,-0.00382271409034729f,0.05026526004076004f,-0.49761754274368286f,0.3193882703781128f,0.4324275553226471f,-0.7607815861701965f,-0.3335152566432953f,0.10636988282203674f,0.262630432844162f,-0.17788830399513245f,0.3869096338748932f,0.1590534746646881f,-0.16216763854026794f,-0.017990201711654663f},
+ {-0.21526779234409332f,-1.1129813194274902f,-0.12068668007850647f,0.09446603804826736f,-0.015299053862690926f,0.23865419626235962f,-0.16593411564826965f,-1.1282691955566406f,-0.2572314739227295f,0.025441110134124756f,0.08321377635002136f,-0.22488360106945038f,0.10835165530443192f,0.01774216815829277f,-0.05000334233045578f,-0.3466002941131592f},
+ {-0.08673667907714844f,0.23056277632713318f,0.42949649691581726f,-0.3032870888710022f,0.4159357249736786f,0.03299093246459961f,-0.035138070583343506f,0.00458759069442749f,-0.2919609248638153f,0.13052961230278015f,-0.09829097986221313f,-0.24485144019126892f,0.3602481186389923f,0.3898719251155853f,-0.0538104772567749f,0.07017943263053894f},
+ {0.2464427649974823f,0.39936527609825134f,0.21032699942588806f,-0.2072734385728836f,0.19923558831214905f,0.2847035825252533f,-0.22418056428432465f,-0.43023446202278137f,-0.21198244392871857f,0.23950514197349548f,-0.3551802337169647f,0.11705091595649719f,0.18541833758354187f,-0.023748815059661865f,0.1462016999721527f,-0.08909019827842712f},
+ {0.34622398018836975f,-1.2129888534545898f,-0.20742447674274445f,-0.032929010689258575f,0.08672287315130234f,-0.08068259805440903f,0.45541882514953613f,-2.018517017364502f,0.10944971442222595f,0.04921719431877136f,-0.20607680082321167f,0.21179592609405518f,0.4078998863697052f,0.09382607787847519f,-0.6051720380783081f,-0.19378763437271118f},
+ {0.07416597008705139f,-0.1509564220905304f,-0.3286089599132538f,0.2575420141220093f,-0.32409292459487915f,-0.04360950365662575f,-0.05032980069518089f,0.03308649733662605f,-0.14956387877464294f,0.06287035346031189f,-0.18942220509052277f,-0.22120867669582367f,0.36484384536743164f,0.34050697088241577f,0.033385224640369415f,0.028106003999710083f},
+ {-0.24521887302398682f,0.3061828017234802f,-0.09969070553779602f,-0.835078239440918f,-1.1966766119003296f,0.7502152919769287f,-0.6856613159179688f,0.31733009219169617f,0.16320112347602844f,-0.08684051036834717f,-0.3433442711830139f,-0.3093454837799072f,-0.3787883520126343f,-0.6157321929931641f,0.27898073196411133f,-0.3274548649787903f},
+ {-0.037380099296569824f,-0.053829364478588104f,-0.23550507426261902f,-0.32296717166900635f,0.1419943869113922f,0.3212539851665497f,-0.5291021466255188f,-0.3196764588356018f,0.23772326111793518f,-0.15706628561019897f,-0.03474658727645874f,-0.42883461713790894f,-0.43303486704826355f,-0.46498894691467285f,0.23065359890460968f,-0.06882688403129578f},
+ {0.22414228320121765f,0.6167948842048645f,0.17398694157600403f,-1.2930227518081665f,-0.07601547241210938f,1.1093199253082275f,-0.4207235872745514f,0.2633194625377655f,-0.16447538137435913f,0.31832489371299744f,-0.01876583695411682f,0.425087034702301f,-1.1076250076293945f,0.1036156564950943f,-0.3688891530036926f,-0.37546294927597046f},
+ {-0.37478217482566833f,-0.10878506302833557f,-0.33313462138175964f,0.08732470870018005f,-0.4272407591342926f,0.42993465065956116f,-0.3373659551143646f,-0.1347610354423523f,0.27883973717689514f,0.15846404433250427f,0.23034706711769104f,0.2679099142551422f,0.26554760336875916f,-0.39576658606529236f,0.27645888924598694f,0.3027357757091522f},
+ {0.01714116334915161f,-0.41286614537239075f,-0.30441972613334656f,0.09042176604270935f,0.305792897939682f,0.25930657982826233f,-0.0077957212924957275f,0.2839451730251312f,0.06527414917945862f,0.30544695258140564f,-0.3269312381744385f,-0.23118992149829865f,-0.36912286281585693f,0.18185165524482727f,0.06934896111488342f,0.20831605792045593f},
+ {-0.16316500306129456f,0.09996911883354187f,-0.11835616827011108f,0.22180476784706116f,0.2730047404766083f,-0.39772462844848633f,0.3122676908969879f,0.025347262620925903f,-0.0864812433719635f,-0.2908649444580078f,0.05068853497505188f,-0.23324716091156006f,-0.27006956934928894f,0.3646886646747589f,-0.22474992275238037f,0.03918728232383728f},
+ {-0.2255314439535141f,0.0861915647983551f,-0.37378355860710144f,0.09740594029426575f,-0.17835164070129395f,-0.28151828050613403f,0.08090576529502869f,0.27042338252067566f,0.10048076510429382f,0.3275263011455536f,0.3301452696323395f,-0.33213010430336f,0.18207183480262756f,0.224577397108078f,-0.2358287274837494f,0.16939416527748108f},
+ {0.18313375115394592f,3.035151958465576f,0.15208670496940613f,-2.073854684829712f,0.9186175465583801f,0.35607829689979553f,-1.5632147789001465f,-0.4666653871536255f,-0.15266752243041992f,-0.3787156641483307f,-0.23417361080646515f,0.41418468952178955f,-2.05690336227417f,-2.3053674697875977f,-0.29849734902381897f,-0.3566729426383972f},
+ {0.19047978520393372f,0.24750559031963348f,-0.1217426061630249f,0.5728633403778076f,-0.08453264087438583f,0.24713492393493652f,0.41920486092567444f,0.4218023419380188f,-0.27937382459640503f,0.06346243619918823f,-0.3896019458770752f,0.36481839418411255f,0.7093786597251892f,-0.09001748263835907f,-0.4630468785762787f,-0.3792039752006531f},
+ {-0.3329339325428009f,-0.4124312996864319f,-0.00382271409034729f,0.05026526004076004f,-0.49761754274368286f,0.3193882703781128f,0.4324275553226471f,-0.7607815861701965f,-0.3335152566432953f,0.10636988282203674f,0.262630432844162f,-0.17788830399513245f,0.3869096338748932f,0.1590534746646881f,-0.16216763854026794f,-0.017990201711654663f},
};
const float hout[16] = { 0.28352612257003784f,7.027318000793457f,0.20054447650909424f,-1.0949580669403076f,-0.5842559933662415f,0.2186238020658493f,-1.2922117710113525f,2.97406268119812f,0.3234240412712097f,-0.5405333638191223f,-0.04778480529785156f,0.04107429087162018f,-1.542391300201416f,-0.6290411949157715f,0.531202495098114f,-0.49622899293899536f };
@@ -2356,7 +2356,7 @@
// valve_ref_pos_buffer[9] = valve_pos.ref;
// VALVE_POS_CONTROL(valve_ref_pos_buffer[0]);
////////////////////////////////////////////////////////////
-
+
V_out = Vout.ref;
} else if (CURRENT_CONTROL_MODE == 0) { //PWM
V_out = valve_pos.ref;
@@ -2396,10 +2396,10 @@
double wn_Pos = 2.0f * PI * 5.0f; // f_cut : 5Hz Position Control
if ((OPERATING_MODE & 0x01) == 0) { // Rotary Mode
- temp_vel_pos = (0.01f * (double) P_GAIN_JOINT_POSITION * wn_Pos * pos.err + 0.01f * (double) I_GAIN_JOINT_POSITION * wn_Pos * pos.err_sum + 0.01f * (double) VELOCITY_COMP_GAIN * vel.ref / ENC_PULSE_PER_POSITION) * PI / 180.0f; // rad/s
+ temp_vel_pos = (0.01f * (double) P_GAIN_JOINT_POSITION * wn_Pos * pos.err + 0.01f * (double) I_GAIN_JOINT_POSITION * wn_Pos * pos.err_sum + 0.01f * (double) D_GAIN_JOINT_POSITION * wn_Pos * vel.err + 0.01f * (double) VELOCITY_COMP_GAIN * vel.ref / ENC_PULSE_PER_POSITION) * PI / 180.0f; // rad/s
// L when P-gain = 100, f_cut = 10Hz L feedforward velocity
} else if ((OPERATING_MODE & 0x01) == 1) {
- temp_vel_pos = (0.01f * (double) P_GAIN_JOINT_POSITION * wn_Pos * pos.err + 0.01f * (double) I_GAIN_JOINT_POSITION * wn_Pos * pos.err_sum + 0.01f * (double) VELOCITY_COMP_GAIN * vel.ref / ENC_PULSE_PER_POSITION); // mm/s
+ temp_vel_pos = (0.01f * (double) P_GAIN_JOINT_POSITION * wn_Pos * pos.err + 0.01f * (double) I_GAIN_JOINT_POSITION * wn_Pos * pos.err_sum + 0.01f * (double) D_GAIN_JOINT_POSITION * wn_Pos * vel.err + 0.01f * (double) VELOCITY_COMP_GAIN * vel.ref / ENC_PULSE_PER_POSITION); // mm/s
// L when P-gain = 100, f_cut = 10Hz L feedforward velocity
}
if (temp_vel_pos > 0.0f) I_REF_POS = temp_vel_pos * ((double) PISTON_AREA_A * 0.00006f / (K_v * sqrt(2.0f * alpha3 / (alpha3 + 1.0f))));
@@ -2683,7 +2683,11 @@
double alpha_update_Iref = 1.0f / (1.0f + 5000.0f / (2.0f * 3.14f * 300.0f)); // f_cutoff : 500Hz
I_REF_fil = (1.0f - alpha_update_Iref) * I_REF_fil + alpha_update_Iref*I_REF;
- I_ERR = I_REF_fil - cur.sen;
+ if (I_REF_fil > 0) I_REF_fil_DZ = I_REF_fil + (double)VALVE_DEADZONE_PLUS/1000.0f; // unit: mA
+ else if (I_REF_fil < 0) I_REF_fil_DZ = I_REF_fil + (double)VALVE_DEADZONE_MINUS/1000.0f; // unit: mA
+ else I_REF_fil_DZ = I_REF_fil + (double)(VALVE_DEADZONE_PLUS+VALVE_DEADZONE_MINUS)/2.0/1000.0f; // unit: mA
+
+ I_ERR = I_REF_fil_DZ - cur.sen;
I_ERR_INT = I_ERR_INT + (I_ERR) * 0.0002f;
@@ -2735,11 +2739,14 @@
///////////////// Dead Zone Cancellation & Linearization //////////////////
////////////////////////////////////////////////////////////////////////////
// Dead Zone Cancellation (Mechanical Valve dead-zone)
+// FLAG_VALVE_DEADZONE = 1;
if (FLAG_VALVE_DEADZONE) {
- if (VALVE_PWM_RAW > 0) VALVE_PWM_RAW = VALVE_PWM_RAW + VALVE_DEADZONE_PLUS * mV_PER_pulse; // unit: mV
- else if (VALVE_PWM_RAW < 0) VALVE_PWM_RAW = VALVE_PWM_RAW + VALVE_DEADZONE_MINUS * mV_PER_pulse; // unit: mV
-
- VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW + (double)VALVE_CENTER * mV_PER_pulse; // unit: mV
+// if (VALVE_PWM_RAW > 0) VALVE_PWM_RAW = VALVE_PWM_RAW + VALVE_DEADZONE_PLUS * mV_PER_pulse; // unit: mV
+// else if (VALVE_PWM_RAW < 0) VALVE_PWM_RAW = VALVE_PWM_RAW + VALVE_DEADZONE_MINUS * mV_PER_pulse; // unit: mV
+// else VALVE_PWM_RAW = VALVE_PWM_RAW + (VALVE_DEADZONE_PLUS+VALVE_DEADZONE_MINUS)/2.0f * mV_PER_pulse; // unit: mV
+
+ //VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW + (double)VALVE_CENTER * mV_PER_pulse; // unit: mV
+ VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW; // unit: mV
} else {
VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW;
@@ -2803,83 +2810,88 @@
TIM4->CCR1 = (PWM_ARR)*(1.0f-dtc_w);
-
-/*
- if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/CAN_FREQ) == 0) {
-
- // Position, Velocity, and Torque (ID:1200)
- if (flag_data_request[0] == HIGH) {
- if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator
- if (SENSING_MODE == 0) {
- CAN_TX_POSITION_FT((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) (torq.sen*10.0f));
- } else if (SENSING_MODE == 1) {
- CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f));
- }
- } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator
- if (SENSING_MODE == 0) {
- CAN_TX_POSITION_FT((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) (torq.sen * 10.0f * (float)(TORQUE_SENSOR_PULSE_PER_TORQUE)));
- } else if (SENSING_MODE == 1) {
- CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f));
- }
- }
- }
+// if (TMR2_COUNT_CAN_TX % 5 == 0) {
+// if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/1000.0f) == 0) {
+// CAN_TX_POSITION_FT((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) (torq.sen*10.0f));
+// }
+// TMR2_COUNT_CAN_TX++;
+
- if (flag_data_request[1] == HIGH) {
- CAN_TX_TORQUE((int16_t) (return_G[0]*100.0f)); //1300
- }
-
-
- if (flag_data_request[2] == HIGH) {
- double t_value = 0.0f;
- if(value>=(float) VALVE_CENTER) {
- t_value = 10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
- } else {
- t_value = -10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
- }
- double t_value_ref = 0.0f;
- if(valve_pos.ref>=(float) VALVE_CENTER) {
- t_value_ref = 10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
- } else {
- t_value_ref = -10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
- }
-
-
- CAN_TX_PRES((int16_t) (t_value), (int16_t) (t_value_ref)); // 1400
- }
-
- //If it doesn't rest, below can can not work.
- for (int can_rest = 0; can_rest < 10000; can_rest++) {
- ;
- }
-
- if (flag_data_request[3] == HIGH) {
- //PWM
- CAN_TX_PWM((int16_t) (torq.ref)); //1500
-// CAN_TX_PWM((int16_t) (f_future[1])); //1500
- }
-
- 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) (logging2*1000.0f), (int16_t) (logging4*1000.0f)); //1600
- CAN_TX_VALVE_POSITION((int16_t) (a_hat*0.0001f), (int16_t) 0, (int16_t) 0, (int16_t) 0); //1600
- }
-
-
-
- // Others : Reference position, Reference FT, PWM, Current (ID:1300)
- if (flag_data_request[1] == HIGH) {
- CAN_TX_SOMETHING((int) (FORCE_VREF), (int16_t) (1), (int16_t) (2), (int16_t) (3));
- }
- if (flag_delay_test == 1){
- CAN_TX_PRES((int16_t) (0),(int16_t) torq_ref);
- }
-
- TMR2_COUNT_CAN_TX = 0;
- }
- TMR2_COUNT_CAN_TX++;
+// if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/CAN_FREQ) == 0) {
+//
+// // Position, Velocity, and Torque (ID:1200)
+// if (flag_data_request[0] == 0) {
+// if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator
+// if (SENSING_MODE == 0) {
+// CAN_TX_POSITION_FT((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) (torq.sen*10.0f));
+// } else if (SENSING_MODE == 1) {
+// CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f));
+// }
+// } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator
+// if (SENSING_MODE == 0) {
+// CAN_TX_POSITION_FT((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) (torq.sen * 10.0f * (float)(TORQUE_SENSOR_PULSE_PER_TORQUE)));
+// } else if (SENSING_MODE == 1) {
+// CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f));
+// }
+// }
+// }
+//
+// if (flag_data_request[1] == HIGH) {
+// CAN_TX_TORQUE((int16_t) (return_G[0]*100.0f)); //1300
+// }
+//
+//
+// if (flag_data_request[2] == HIGH) {
+// double t_value = 0.0f;
+// if(value>=(float) VALVE_CENTER) {
+// t_value = 10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
+// } else {
+// t_value = -10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
+// }
+// double t_value_ref = 0.0f;
+// if(valve_pos.ref>=(float) VALVE_CENTER) {
+// t_value_ref = 10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
+// } else {
+// t_value_ref = -10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
+// }
+//
+//
+// CAN_TX_PRES((int16_t) (t_value), (int16_t) (t_value_ref)); // 1400
+// }
+//
+// //If it doesn't rest, below can can not work.
+// for (int can_rest = 0; can_rest < 10000; can_rest++) {
+// ;
+// }
+//
+// if (flag_data_request[3] == HIGH) {
+// //PWM
+// CAN_TX_PWM((int16_t) (torq.ref)); //1500
+// // CAN_TX_PWM((int16_t) (f_future[1])); //1500
+// }
+//
+// 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) (logging2*1000.0f), (int16_t) (logging4*1000.0f)); //1600
+// CAN_TX_VALVE_POSITION((int16_t) (a_hat*0.0001f), (int16_t) 0, (int16_t) 0, (int16_t) 0); //1600
+// }
+//
+//
+//
+// // Others : Reference position, Reference FT, PWM, Current (ID:1300)
+//// if (flag_data_request[1] == HIGH) {
+//// CAN_TX_SOMETHING((int) (0), (int16_t) (1), (int16_t) (2), (int16_t) (3));
+//// }
+//// if (flag_delay_test == 1){
+//// CAN_TX_PRES((int16_t) (0),(int16_t) torq_ref);
+//// }
+////
+//// TMR2_COUNT_CAN_TX = 0;
+// }
+// TMR2_COUNT_CAN_TX++;
+
+
-*/
-
}
TIM3->SR = 0x0; // reset the status register