diff --git a/PathWeaver/Paths/Unnamed.path b/PathWeaver/Paths/Unnamed.path new file mode 100644 index 0000000..1d2458c --- /dev/null +++ b/PathWeaver/Paths/Unnamed.path @@ -0,0 +1,3 @@ +X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name +0.0,0.0,2.0574,0.0,true,false, +2.0574,-4.1148,0.0,-4.1148,true,false, diff --git a/PathWeaver/output/Unnamed.wpilib.json b/PathWeaver/output/Unnamed.wpilib.json new file mode 100644 index 0000000..7325468 --- /dev/null +++ b/PathWeaver/output/Unnamed.wpilib.json @@ -0,0 +1,932 @@ +[ + { + "acceleration": 1.0, + "curvature": 0.0, + "pose": { + "rotation": { + "radians": 0.0 + }, + "translation": { + "x": 0.0, + "y": 8.2296 + } + }, + "time": 0.0, + "velocity": 0.0 + }, + { + "acceleration": 1.0, + "curvature": -0.9707131987551469, + "pose": { + "rotation": { + "radians": -0.03286227737456197 + }, + "translation": { + "x": 0.06453134678006171, + "y": 8.228877583158015 + } + }, + "time": 0.3592642211988369, + "velocity": 0.3592642211988369 + }, + { + "acceleration": 0.32825450291433095, + "curvature": -1.5982027622993162, + "pose": { + "rotation": { + "radians": -0.12002174011582287 + }, + "translation": { + "x": 0.1303828119277954, + "y": 8.224062983322144 + } + }, + "time": 0.511004160181813, + "velocity": 0.511004160181813 + }, + { + "acceleration": -0.11701954363050833, + "curvature": -1.7560205632932218, + "pose": { + "rotation": { + "radians": -0.17758655147327887 + }, + "translation": { + "x": 0.16414000868517903, + "y": 8.219017841797694 + } + }, + "time": 0.5764237890816287, + "velocity": 0.532478447947162 + }, + { + "acceleration": 0.06477860873919201, + "curvature": -1.8118392266061383, + "pose": { + "rotation": { + "radians": -0.24071824344297915 + }, + "translation": { + "x": 0.19859442552924156, + "y": 8.211710526001452 + } + }, + "time": 0.6430566548038834, + "velocity": 0.5246811004095508 + }, + { + "acceleration": 0.20059134929694603, + "curvature": -1.779427900988438, + "pose": { + "rotation": { + "radians": -0.30668716541291297 + }, + "translation": { + "x": 0.23383163179066033, + "y": 8.201813911153748 + } + }, + "time": 0.7125166155383681, + "velocity": 0.5291806200290097 + }, + { + "acceleration": 0.30232465531924346, + "curvature": -1.6798625234895561, + "pose": { + "rotation": { + "radians": -0.37307681731832065 + }, + "translation": { + "x": 0.2699207336425781, + "y": 8.189039685058594 + } + }, + "time": 0.7838952905053327, + "velocity": 0.5434985647516613 + }, + { + "acceleration": 0.37461936093350684, + "curvature": -1.5369796884333593, + "pose": { + "rotation": { + "radians": -0.43793600289203194 + }, + "translation": { + "x": 0.30691506389770656, + "y": 8.17313696850948 + } + }, + "time": 0.8565179799489671, + "velocity": 0.5654541943060645 + }, + { + "acceleration": 0.41946609124125495, + "curvature": -1.3728617360232602, + "pose": { + "rotation": { + "radians": -0.49984664447575056 + }, + "translation": { + "x": 0.34485287180542945, + "y": 8.153890935695172 + } + }, + "time": 0.9299634073755572, + "velocity": 0.5929682733921019 + }, + { + "acceleration": 0.43889375949147225, + "curvature": -1.2049097836003586, + "pose": { + "rotation": { + "radians": -0.5579068366562434 + }, + "translation": { + "x": 0.3837580128489062, + "y": 8.131121434605493 + } + }, + "time": 1.0040438973252086, + "velocity": 0.6240425269485192 + }, + { + "acceleration": 0.4362106284779035, + "curvature": -1.0449061636087065, + "pose": { + "rotation": { + "radians": -0.6116541597559717 + }, + "translation": { + "x": 0.42364063854217526, + "y": 8.104681607437133 + } + }, + "time": 1.0787593919946574, + "velocity": 0.6568346912962587 + }, + { + "acceleration": 0.41607529990726916, + "curvature": -0.899555889941672, + "pose": { + "rotation": { + "radians": -0.6609622154001376 + }, + "translation": { + "x": 0.4644978862272575, + "y": 8.074456510999426 + } + }, + "time": 1.154241532737348, + "velocity": 0.6897608033484853 + }, + { + "acceleration": 0.36378148552133527, + "curvature": -0.7717206182191843, + "pose": { + "rotation": { + "radians": -0.7059372773217938 + }, + "translation": { + "x": 0.5063145688712597, + "y": 8.04036173712015 + } + }, + "time": 1.2307003935222725, + "velocity": 0.7215734467801409 + }, + { + "acceleration": 0.30299924929936645, + "curvature": -0.568473799977757, + "pose": { + "rotation": { + "radians": -0.7839716308415736 + }, + "translation": { + "x": 0.5927080078125, + "y": 7.9603699218749995 + } + }, + "time": 1.3876602405207572, + "velocity": 0.7786725330884512 + }, + { + "acceleration": 0.261968753395329, + "curvature": -0.49013718846863963, + "pose": { + "rotation": { + "radians": -0.8177218107249609 + }, + "translation": { + "x": 0.6371989763433114, + "y": 7.914444322909042 + } + }, + "time": 1.4685055468405372, + "velocity": 0.8031686002127219 + }, + { + "acceleration": 0.22367365371198333, + "curvature": -0.4247033525488515, + "pose": { + "rotation": { + "radians": -0.8484443233155562 + }, + "translation": { + "x": 0.6824791838943958, + "y": 7.864589172112941 + } + }, + "time": 1.5512427804250564, + "velocity": 0.8248431701542365 + }, + { + "acceleration": 0.1892254021002176, + "curvature": -0.3702090219567703, + "pose": { + "rotation": { + "radians": -0.8764864329295776 + }, + "translation": { + "x": 0.7284821685148402, + "y": 7.810852042493597 + } + }, + "time": 1.6360282101930044, + "velocity": 0.8438074370119741 + }, + { + "acceleration": 0.15903275574461928, + "curvature": -0.32487792971934376, + "pose": { + "rotation": { + "radians": -0.9021700531416887 + }, + "translation": { + "x": 0.7751332826614379, + "y": 7.753302764511108 + } + }, + "time": 1.7229762790532093, + "velocity": 0.8602602203038838 + }, + { + "acceleration": 0.13304567370627288, + "curvature": -0.28716941122302303, + "pose": { + "rotation": { + "radians": -0.9257877895925812 + }, + "translation": { + "x": 0.8223503829957917, + "y": 7.692032046484574 + } + }, + "time": 1.8121597544434918, + "velocity": 0.8744433141620829 + }, + { + "acceleration": 0.1109501443397911, + "curvature": -0.25578179205649754, + "pose": { + "rotation": { + "radians": -0.9476023238245057 + }, + "translation": { + "x": 0.8700445201814173, + "y": 7.627150094997883 + } + }, + "time": 1.9036115406250422, + "velocity": 0.8866105786662493 + }, + { + "acceleration": 0.0923063317209454, + "curvature": -0.2296336011796357, + "pose": { + "rotation": { + "radians": -0.9678477132739784 + }, + "translation": { + "x": 0.9181206286808475, + "y": 7.5587852353055025 + } + }, + "time": 1.9973274196618063, + "velocity": 0.8970083689723086 + }, + { + "acceleration": 0.07663738524277816, + "curvature": -0.20783594546662976, + "pose": { + "rotation": { + "radians": -0.9867316858513272 + }, + "translation": { + "x": 0.9664782165527344, + "y": 7.487082531738281 + } + }, + "time": 2.0932692482669553, + "velocity": 0.9058644072294496 + }, + { + "acceleration": 0.06348113848335224, + "curvature": -0.18966323520605685, + "pose": { + "rotation": { + "radians": -1.0044383571669404 + }, + "translation": { + "x": 1.0150120552489532, + "y": 7.412202408109232 + } + }, + "time": 2.1913683188854476, + "velocity": 0.9133824634963975 + }, + { + "acceleration": 0.05241651190475789, + "curvature": -0.17452578839593655, + "pose": { + "rotation": { + "radians": -1.0211310311039041 + }, + "translation": { + "x": 1.0636128694117069, + "y": 7.3343192681193345 + } + }, + "time": 2.291528714186948, + "velocity": 0.9197407594210794 + }, + { + "acceleration": 0.04307399954106254, + "curvature": -0.16194577561430387, + "pose": { + "rotation": { + "radians": -1.0369548940379958 + }, + "translation": { + "x": 1.112168026670627, + "y": 7.253620115763321 + } + }, + "time": 2.3936305618672695, + "velocity": 0.9250925821355127 + }, + { + "acceleration": 0.0351369025874004, + "curvature": -0.15153686838804653, + "pose": { + "rotation": { + "radians": -1.0520395062636407 + }, + "translation": { + "x": 1.1605622274398804, + "y": 7.170303175735473 + } + }, + "time": 2.4975331449987155, + "velocity": 0.9295680819536318 + }, + { + "acceleration": 0.02833768862152682, + "curvature": -0.14298741495544345, + "pose": { + "rotation": { + "radians": -1.066501050408053 + }, + "translation": { + "x": 1.2086781947152687, + "y": 7.084576513835414 + } + }, + "time": 2.6030778521223614, + "velocity": 0.9332765960464511 + }, + { + "acceleration": 0.022452175626054584, + "curvature": -0.13604673836268869, + "pose": { + "rotation": { + "radians": -1.0804443290862682 + }, + "translation": { + "x": 1.256397363871336, + "y": 6.996656657373904 + } + }, + "time": 2.71009096776375, + "velocity": 0.9363091003959162 + }, + { + "acceleration": 0.01729310392999324, + "curvature": -0.13051408597000735, + "pose": { + "rotation": { + "radians": -1.0939645215960148 + }, + "translation": { + "x": 1.303600572458468, + "y": 6.90676721557863 + } + }, + "time": 2.8183863127100217, + "velocity": 0.938740566500134 + }, + { + "acceleration": 0.012703937254456272, + "curvature": -0.12622977312711864, + "pose": { + "rotation": { + "radians": -1.1071487177940906 + }, + "translation": { + "x": 1.35016875, + "y": 6.8151375 + } + }, + "time": 2.9277677472260955, + "velocity": 0.9406321110152323 + }, + { + "acceleration": 0.008553300269580984, + "curvature": -0.12306811113537366, + "pose": { + "rotation": { + "radians": -1.120077250059912 + }, + "translation": { + "x": 1.395983607789315, + "y": 6.722001144916936 + } + }, + "time": 3.0380315513242944, + "velocity": 0.9420328954639334 + }, + { + "acceleration": 0.004730215404431994, + "curvature": -0.12093176643176069, + "pose": { + "rotation": { + "radians": -1.1328248436796182 + }, + "translation": { + "x": 1.4409283286869525, + "y": 6.627594727742672 + } + }, + "time": 3.1489686954193368, + "velocity": 0.9429817741684281 + }, + { + "acceleration": 0.0011401761676901984, + "curvature": -0.11974725206745888, + "pose": { + "rotation": { + "radians": -1.1454616034190548 + }, + "translation": { + "x": 1.4848882569177075, + "y": 6.532156389430537 + } + }, + "time": 3.2603670129042404, + "velocity": 0.943508712205823 + }, + { + "acceleration": -0.002297958121626943, + "curvature": -0.11946129834356706, + "pose": { + "rotation": { + "radians": -1.1580538502591688 + }, + "translation": { + "x": 1.5277515878677368, + "y": 6.4359244548797605 + } + }, + "time": 3.372013283767855, + "velocity": 0.9436360086230732 + }, + { + "acceleration": -0.005654269245082158, + "curvature": -0.12003788451419792, + "pose": { + "rotation": { + "radians": -1.170664817631233 + }, + "translation": { + "x": 1.5694100578816605, + "y": 6.339136053341253 + } + }, + "time": 3.4836952355361177, + "velocity": 0.9433793681749681 + }, + { + "acceleration": -0.008989292240720181, + "curvature": -0.12145573685466726, + "pose": { + "rotation": { + "radians": -1.1833552111571433 + }, + "translation": { + "x": 1.6097596340596672, + "y": 6.242025738823413 + } + }, + "time": 3.5952034646197997, + "velocity": 0.9427488706246867 + }, + { + "acceleration": -0.01235469911684124, + "curvature": -0.12370610969929342, + "pose": { + "rotation": { + "radians": -1.1961836298574748 + }, + "translation": { + "x": 1.6487012040546163, + "y": 6.144824110497906 + } + }, + "time": 3.7063332775691284, + "velocity": 0.9417498922594286 + }, + { + "acceleration": -0.015793070497524053, + "curvature": -0.12679066485150986, + "pose": { + "rotation": { + "radians": -1.2092068399185039 + }, + "translation": { + "x": 1.6861412658691406, + "y": 6.047756433105468 + } + }, + "time": 3.816886447687937, + "velocity": 0.9403840411061978 + }, + { + "acceleration": -0.019336612130139666, + "curvature": -0.13071925042014887, + "pose": { + "rotation": { + "radians": -1.2224798842267477 + }, + "translation": { + "x": 1.7219926176527514, + "y": 5.951041257361694 + } + }, + "time": 3.926672877830956, + "velocity": 0.9386501762752776 + }, + { + "acceleration": -0.023004607253431744, + "curvature": -0.13550735181928794, + "pose": { + "rotation": { + "radians": -1.2360560017774647 + }, + "translation": { + "x": 1.7561750474989415, + "y": 5.854889040362833 + } + }, + "time": 4.035512154862443, + "velocity": 0.9365455933907949 + }, + { + "acceleration": -0.026799328138561878, + "curvature": -0.14117294465574481, + "pose": { + "rotation": { + "radians": -1.2499863205533628 + }, + "translation": { + "x": 1.788616023242287, + "y": 5.759500765991582 + } + }, + "time": 4.143234975065483, + "velocity": 0.9340674722197919 + }, + { + "acceleration": -0.030700069062523747, + "curvature": -0.14773242148478702, + "pose": { + "rotation": { + "radians": -1.2643192754484534 + }, + "translation": { + "x": 1.8192513822555538, + "y": 5.665066565322875 + } + }, + "time": 4.249684412672446, + "velocity": 0.9312146988111976 + }, + { + "acceleration": -0.03465492258508358, + "curvature": -0.15519519369521248, + "pose": { + "rotation": { + "radians": -1.2790996893622868 + }, + "translation": { + "x": 1.848026021246798, + "y": 5.571764337029679 + } + }, + "time": 4.354716995646843, + "velocity": 0.9279901912600683 + }, + { + "acceleration": -0.038569922588567944, + "curvature": -0.16355649143698706, + "pose": { + "rotation": { + "radians": -1.294367441132383 + }, + "translation": { + "x": 1.8748945860564707, + "y": 5.479758367788788 + } + }, + "time": 4.458203544068471, + "velocity": 0.9244038729359193 + }, + { + "acceleration": -0.04229525004547591, + "curvature": -0.17278781025488785, + "pose": { + "rotation": { + "radians": -1.3101556294877001 + }, + "translation": { + "x": 1.8998221614545212, + "y": 5.38919795268662 + } + }, + "time": 4.560029717434675, + "velocity": 0.9204764453116947 + }, + { + "acceleration": -0.0456083749877134, + "curvature": -0.18282440504300387, + "pose": { + "rotation": { + "radians": -1.326488129541421 + }, + "translation": { + "x": 1.9227849609374998, + "y": 5.300216015624997 + } + }, + "time": 4.6600962088291995, + "velocity": 0.9162441080369899 + }, + { + "acceleration": -0.04819432264902452, + "curvature": -0.1935492477745208, + "pose": { + "rotation": { + "radians": -1.3433764306102893 + }, + "translation": { + "x": 1.9437710165256628, + "y": 5.212927729726953 + } + }, + "time": 4.758318517841781, + "velocity": 0.911764348135385 + }, + { + "acceleration": -0.04962371988201259, + "curvature": -0.20477300427880388, + "pose": { + "rotation": { + "radians": -1.360815646191625 + }, + "translation": { + "x": 1.9627808685600756, + "y": 5.127429137742517 + } + }, + "time": 4.854626232897319, + "velocity": 0.9071228630424081 + }, + { + "acceleration": -0.04932988359178645, + "curvature": -0.21620993224149593, + "pose": { + "rotation": { + "radians": -1.3787796057624737 + }, + "translation": { + "x": 1.9798282554997126, + "y": 5.043795772454512 + } + }, + "time": 4.948961761017647, + "velocity": 0.9024415832200432 + }, + { + "acceleration": -0.04658689436992434, + "curvature": -0.2274502682640764, + "pose": { + "rotation": { + "radians": -1.397214983126331 + }, + "translation": { + "x": 1.9949408037185667, + "y": 4.96208127708435 + } + }, + "time": 5.041278464104945, + "velocity": 0.8978876110031693 + }, + { + "acceleration": -0.04049126034403752, + "curvature": -0.23793078023655534, + "pose": { + "rotation": { + "radians": -1.4160344989260125 + }, + "translation": { + "x": 2.0081607173027463, + "y": 4.8823160256978095 + } + }, + "time": 5.131538202019646, + "velocity": 0.89368269012708 + }, + { + "acceleration": -0.029950358776442698, + "curvature": -0.24690681624676655, + "pose": { + "rotation": { + "radians": -1.435109368208452 + }, + "translation": { + "x": 2.0195454678475855, + "y": 4.804505743610857 + } + }, + "time": 5.219708351206326, + "velocity": 0.8901125696617895 + }, + { + "acceleration": -0.01368148711368845, + "curvature": -0.25343140151995497, + "pose": { + "rotation": { + "radians": -1.4542613582856863 + }, + "translation": { + "x": 2.0291684842547397, + "y": 4.728630127795411 + } + }, + "time": 5.305758470029964, + "velocity": 0.887535337730266 + }, + { + "acceleration": 0.009773316155429099, + "curvature": -0.2563495540021729, + "pose": { + "rotation": { + "radians": -1.4732550817164385 + }, + "translation": { + "x": 2.0371198425292976, + "y": 4.654641467285154 + } + }, + "time": 5.389656922047198, + "velocity": 0.8863874821401339 + }, + { + "acceleration": 0.04200804146330431, + "curvature": -0.25431853374604046, + "pose": { + "rotation": { + "radians": -1.4917914648303507 + }, + "translation": { + "x": 2.0435069555768735, + "y": 4.582463263581316 + } + }, + "time": 5.471367943487159, + "velocity": 0.8871860697860496 + }, + { + "acceleration": -0.9878242860338343, + "curvature": -0.24586633239208006, + "pose": { + "rotation": { + "radians": -1.509503673581512 + }, + "translation": { + "x": 2.048455263000726, + "y": 4.511988851058478 + } + }, + "time": 5.550849837941167, + "velocity": 0.8905249485038556 + }, + { + "acceleration": -1.0, + "curvature": -0.22950006831279324, + "pose": { + "rotation": { + "radians": -1.5259570881232554 + }, + "translation": { + "x": 2.0521089208988466, + "y": 4.443080017370354 + } + }, + "time": 5.6319900871990605, + "velocity": 0.8103726397120693 + }, + { + "acceleration": -0.9999999999999996, + "curvature": -0.20387160749102093, + "pose": { + "rotation": { + "radians": -1.5406551094397534 + }, + "translation": { + "x": 2.054631491661072, + "y": 4.375565623855588 + } + }, + "time": 5.720157209702984, + "velocity": 0.7222055172081453 + }, + { + "acceleration": -1.0000000000000004, + "curvature": -0.16799846857052722, + "pose": { + "rotation": { + "radians": -1.5530525488704952 + }, + "translation": { + "x": 2.0562066337661813, + "y": 4.309240225943546 + } + }, + "time": 5.818750178164357, + "velocity": 0.6236125487467731 + }, + { + "acceleration": -1.0, + "curvature": -0.12152358859742607, + "pose": { + "rotation": { + "radians": -1.5625779837923761 + }, + "translation": { + "x": 2.0570387915790076, + "y": 4.243862693560116 + } + }, + "time": 5.934300767601808, + "velocity": 0.5080619593093217 + }, + { + "acceleration": -1.0, + "curvature": -0.06497908072006661, + "pose": { + "rotation": { + "radians": -1.5686656690122298 + }, + "translation": { + "x": 2.057353885147535, + "y": 4.1791548315335 + } + }, + "time": 6.083601391046174, + "velocity": 0.35876133586495645 + }, + { + "acceleration": -1.0, + "curvature": -2.717474527174877e-30, + "pose": { + "rotation": { + "radians": -1.5707963267948961 + }, + "translation": { + "x": 2.0574, + "y": 4.114799999999994 + } + }, + "time": 6.4423627269111305, + "velocity": 0.0 + } +] \ No newline at end of file diff --git a/PathWeaver/pathweaver.json b/PathWeaver/pathweaver.json new file mode 100644 index 0000000..c5b70d2 --- /dev/null +++ b/PathWeaver/pathweaver.json @@ -0,0 +1,9 @@ +{ + "lengthUnit": "Meter", + "exportUnit": "Same as Project", + "maxVelocity": 1.0, + "maxAcceleration": 1.0, + "trackWidth": 1.0, + "gameName": "Rapid React", + "outputDir": "" +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 521b2c2..2580247 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -4,11 +4,14 @@ package frc4388.robot; +import java.io.IOException; + import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.livewindow.LiveWindow; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc4388.utility.RobotLogger; import frc4388.utility.RobotTime; /** @@ -64,6 +67,7 @@ public class Robot extends TimedRobot { @Override public void disabledInit() { m_robotTime.endMatchTime(); + RobotLogger.getInstance().setEnabled(false); } @Override @@ -96,6 +100,7 @@ public class Robot extends TimedRobot { m_autonomousCommand.schedule(); } m_robotTime.startMatchTime(); + RobotLogger.getInstance().setEnabled(false); } /** @@ -115,6 +120,7 @@ public class Robot extends TimedRobot { m_autonomousCommand.cancel(); } m_robotTime.startMatchTime(); + RobotLogger.getInstance().setEnabled(true); } /** @@ -126,6 +132,16 @@ public class Robot extends TimedRobot { // m_robotContainer.getOperatorController().updateInput(); } + @Override + public void testInit() { + RobotLogger.getInstance().setEnabled(false); + try { + RobotLogger.getInstance().exportPath(); + } catch (IOException e) { + e.printStackTrace(); + } + } + /** * This function is called periodically during test mode. */ diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index fafc14c..cd79844 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -21,6 +21,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.RobotMap; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.Gains; +import frc4388.utility.RobotLogger; public class SwerveDrive extends SubsystemBase { // private WPI_TalonFX m_leftFrontSteerMotor; @@ -142,6 +143,7 @@ public class SwerveDrive extends SubsystemBase { updateOdometry(); // m_gyro.setFusedHeadingToCompass(); // m_gyro.setYawToCompass(); + RobotLogger.getInstance().put("poseMeters", m_poseEstimator.getEstimatedPosition()); super.periodic(); } diff --git a/src/main/java/frc4388/utility/RobotLogger.java b/src/main/java/frc4388/utility/RobotLogger.java new file mode 100644 index 0000000..fed25f0 --- /dev/null +++ b/src/main/java/frc4388/utility/RobotLogger.java @@ -0,0 +1,67 @@ +package frc4388.utility; + +import java.io.IOException; +import java.nio.file.Path; +import java.util.HashMap; +import java.util.Hashtable; +import java.util.List; +import java.util.Map; +import java.util.Objects; +import java.util.Map.Entry; +import java.util.stream.Collectors; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.trajectory.Trajectory; +import edu.wpi.first.math.trajectory.TrajectoryUtil; +import edu.wpi.first.wpilibj.Filesystem; + +public final class RobotLogger { + private static final int SAMPLE_BASE = 15_000; // ms of sampling + private static final int SAMPLE_RATE = 20; // ms between samples + + private static RobotLogger instance = null; + + public static RobotLogger getInstance() { + return Objects.requireNonNullElseGet(instance, () -> instance = new RobotLogger()); + } + + private static long getTime() { + return RobotTime.getInstance().m_robotTime; + } + + private RobotLogger() { + data = new HashMap<>(); + } + + private final Map> data; + private boolean enabled = false; + + public void setEnabled(boolean value) { + enabled = value; + } + + public void put(String key, Object value) { + if (enabled) + data.compute(key, (k, v) -> { + v = v == null ? new Hashtable<>(SAMPLE_BASE / SAMPLE_RATE, 1) : v; + v.put(getTime(), value); + return v; + }); + } + + public void exportPath() throws IOException { + List states = data.get("poseMeters").entrySet().stream().map(entry -> { + double timeSeconds = entry.getKey(); + double velocityMetersPerSecond = 0; + double accelerationMetersPerSecondSq = 0; + Pose2d poseMeters = (Pose2d) entry.getValue(); + double curvatureRadPerMeter = 0; + return new Trajectory.State(timeSeconds, velocityMetersPerSecond, accelerationMetersPerSecondSq, poseMeters, curvatureRadPerMeter); + }).collect(Collectors.toUnmodifiableList()); + String pathPath = "paths/" + System.nanoTime() + ".wpilib.json"; + Path outputPath = Filesystem.getDeployDirectory().toPath().resolve(pathPath); + outputPath.toFile().createNewFile(); + TrajectoryUtil.toPathweaverJson(new Trajectory(states), outputPath); + } +}