diff --git a/.vscode/launch.json b/.vscode/launch.json index 4dbe5d3..4bca61f 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -4,17 +4,24 @@ // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 "version": "0.2.0", "configurations": [ + { + "type": "java", + "name": "Launch Main", + "request": "launch", + "mainClass": "frc4388.robot.Main", + "projectName": "2022NoWayHome" + }, { "type": "wpilib", "name": "WPILib Desktop Debug", "request": "launch", - "desktop": true, + "desktop": true }, { "type": "wpilib", "name": "WPILib roboRIO Debug", "request": "launch", - "desktop": false, + "desktop": false } ] } diff --git a/.vscode/tasks.json b/.vscode/tasks.json new file mode 100644 index 0000000..6ef8685 --- /dev/null +++ b/.vscode/tasks.json @@ -0,0 +1,10 @@ +{ + "version": "2.0.0", + "tasks": [ + { + "label": "downloadDeployDirectory", + "type": "shell", + "command": "scp -pr lvuser@roboRIO-4388-FRC.local:/home/lvuser/deploy ./deploy" + } + ] +} \ No newline at end of file diff --git a/PathWeaver/Paths/Unnamed.path b/PathWeaver/Paths/Unnamed.path deleted file mode 100644 index 1d2458c..0000000 --- a/PathWeaver/Paths/Unnamed.path +++ /dev/null @@ -1,3 +0,0 @@ -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 deleted file mode 100644 index 7325468..0000000 --- a/PathWeaver/output/Unnamed.wpilib.json +++ /dev/null @@ -1,932 +0,0 @@ -[ - { - "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 deleted file mode 100644 index c5b70d2..0000000 --- a/PathWeaver/pathweaver.json +++ /dev/null @@ -1,9 +0,0 @@ -{ - "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/simgui-ds.json b/simgui-ds.json index 69b1a3c..73cc713 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -88,10 +88,5 @@ "buttonCount": 0, "povCount": 0 } - ], - "robotJoysticks": [ - { - "guid": "Keyboard0" - } ] } diff --git a/simgui.json b/simgui.json index 662c52f..c7dcff2 100644 --- a/simgui.json +++ b/simgui.json @@ -1,44 +1,27 @@ { - "HALProvider": { - "Other Devices": { - "Talon FX[2]": { - "header": { - "open": true - } - }, - "Talon FX[3]": { - "header": { - "open": true - } - }, - "Talon FX[4]": { - "header": { - "open": true - } - } - } - }, "NTProvider": { "types": { "/FMSInfo": "FMSInfo", "/LiveWindow/LED": "Subsystem", "/LiveWindow/SwerveDrive": "Subsystem", "/LiveWindow/SwerveModule": "Subsystem", + "/LiveWindow/Ungrouped/PIDController[1]": "PIDController", + "/LiveWindow/Ungrouped/PIDController[2]": "PIDController", + "/LiveWindow/Ungrouped/PIDController[3]": "PIDController", "/LiveWindow/Ungrouped/Scheduler": "Scheduler", - "/SmartDashboard/Driver Controller": "Mechanism2d", - "/SmartDashboard/Field": "Field2d" + "/SmartDashboard/Auto Chooser": "String Chooser", + "/SmartDashboard/Field": "Field2d", + "/SmartDashboard/JVM Memory": "Command", + "/SmartDashboard/Recording": "Command", + "/SmartDashboard/Scheduler": "Scheduler", + "/SmartDashboard/Usable Deploy Space": "Command" }, "windows": { - "/SmartDashboard/Field": { + "/SmartDashboard/Scheduler": { "window": { "visible": true } } } - }, - "NetworkTables": { - "SmartDashboard": { - "open": true - } } } diff --git a/src/main/java/edu/wpi/first/wpilibj/DriverStation.java b/src/main/java/edu/wpi/first/wpilibj/DriverStation.java new file mode 100644 index 0000000..d58a61b --- /dev/null +++ b/src/main/java/edu/wpi/first/wpilibj/DriverStation.java @@ -0,0 +1,1389 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj; + +import edu.wpi.first.hal.AllianceStationID; +import edu.wpi.first.hal.ControlWord; +import edu.wpi.first.hal.HAL; +import edu.wpi.first.hal.MatchInfoData; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import java.nio.ByteBuffer; +import java.util.concurrent.TimeUnit; +import java.util.concurrent.locks.Condition; +import java.util.concurrent.locks.Lock; +import java.util.concurrent.locks.ReentrantLock; + +/** Provide access to the network communication data to / from the Driver Station. */ +public class DriverStation { + /** Number of Joystick Ports. */ + public static final int kJoystickPorts = 6; + + private static class HALJoystickButtons { + public int m_buttons; + public byte m_count; + } + + private static class HALJoystickAxes { + public float[] m_axes; + public short m_count; + + HALJoystickAxes(int count) { + m_axes = new float[count]; + } + } + + private static class HALJoystickPOVs { + public short[] m_povs; + public short m_count; + + HALJoystickPOVs(int count) { + m_povs = new short[count]; + for (int i = 0; i < count; i++) { + m_povs[i] = -1; + } + } + } + + /** The robot alliance that the robot is a part of. */ + public enum Alliance { + Red, + Blue, + Invalid + } + + public enum MatchType { + None, + Practice, + Qualification, + Elimination + } + + private static final double JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL = 1.0; + private static double m_nextMessageTime; + + private static class DriverStationTask implements Runnable { + DriverStationTask() {} + + @Override + public void run() { + DriverStation.run(); + } + } /* DriverStationTask */ + + private static class MatchDataSender { + @SuppressWarnings("MemberName") + NetworkTable table; + + @SuppressWarnings("MemberName") + NetworkTableEntry typeMetadata; + + @SuppressWarnings("MemberName") + NetworkTableEntry gameSpecificMessage; + + @SuppressWarnings("MemberName") + NetworkTableEntry eventName; + + @SuppressWarnings("MemberName") + NetworkTableEntry matchNumber; + + @SuppressWarnings("MemberName") + NetworkTableEntry replayNumber; + + @SuppressWarnings("MemberName") + NetworkTableEntry matchType; + + @SuppressWarnings("MemberName") + NetworkTableEntry alliance; + + @SuppressWarnings("MemberName") + NetworkTableEntry station; + + @SuppressWarnings("MemberName") + NetworkTableEntry controlWord; + + @SuppressWarnings("MemberName") + boolean oldIsRedAlliance = true; + + @SuppressWarnings("MemberName") + int oldStationNumber = 1; + + @SuppressWarnings("MemberName") + String oldEventName = ""; + + @SuppressWarnings("MemberName") + String oldGameSpecificMessage = ""; + + @SuppressWarnings("MemberName") + int oldMatchNumber; + + @SuppressWarnings("MemberName") + int oldReplayNumber; + + @SuppressWarnings("MemberName") + int oldMatchType; + + @SuppressWarnings("MemberName") + int oldControlWord; + + MatchDataSender() { + table = NetworkTableInstance.getDefault().getTable("FMSInfo"); + typeMetadata = table.getEntry(".type"); + typeMetadata.forceSetString("FMSInfo"); + gameSpecificMessage = table.getEntry("GameSpecificMessage"); + gameSpecificMessage.forceSetString(""); + eventName = table.getEntry("EventName"); + eventName.forceSetString(""); + matchNumber = table.getEntry("MatchNumber"); + matchNumber.forceSetDouble(0); + replayNumber = table.getEntry("ReplayNumber"); + replayNumber.forceSetDouble(0); + matchType = table.getEntry("MatchType"); + matchType.forceSetDouble(0); + alliance = table.getEntry("IsRedAlliance"); + alliance.forceSetBoolean(true); + station = table.getEntry("StationNumber"); + station.forceSetDouble(1); + controlWord = table.getEntry("FMSControlData"); + controlWord.forceSetDouble(0); + } + + private void sendMatchData() { + AllianceStationID allianceID = HAL.getAllianceStation(); + boolean isRedAlliance = false; + int stationNumber = 1; + switch (allianceID) { + case Blue1: + isRedAlliance = false; + stationNumber = 1; + break; + case Blue2: + isRedAlliance = false; + stationNumber = 2; + break; + case Blue3: + isRedAlliance = false; + stationNumber = 3; + break; + case Red1: + isRedAlliance = true; + stationNumber = 1; + break; + case Red2: + isRedAlliance = true; + stationNumber = 2; + break; + default: + isRedAlliance = true; + stationNumber = 3; + break; + } + + String currentEventName; + String currentGameSpecificMessage; + int currentMatchNumber; + int currentReplayNumber; + int currentMatchType; + int currentControlWord; + m_cacheDataMutex.lock(); + try { + currentEventName = DriverStation.m_matchInfo.eventName; + currentGameSpecificMessage = DriverStation.m_matchInfo.gameSpecificMessage; + currentMatchNumber = DriverStation.m_matchInfo.matchNumber; + currentReplayNumber = DriverStation.m_matchInfo.replayNumber; + currentMatchType = DriverStation.m_matchInfo.matchType; + } finally { + m_cacheDataMutex.unlock(); + } + currentControlWord = HAL.nativeGetControlWord(); + + if (oldIsRedAlliance != isRedAlliance) { + alliance.setBoolean(isRedAlliance); + oldIsRedAlliance = isRedAlliance; + } + if (oldStationNumber != stationNumber) { + station.setDouble(stationNumber); + oldStationNumber = stationNumber; + } + if (!oldEventName.equals(currentEventName)) { + eventName.setString(currentEventName); + oldEventName = currentEventName; + } + if (!oldGameSpecificMessage.equals(currentGameSpecificMessage)) { + gameSpecificMessage.setString(currentGameSpecificMessage); + oldGameSpecificMessage = currentGameSpecificMessage; + } + if (currentMatchNumber != oldMatchNumber) { + matchNumber.setDouble(currentMatchNumber); + oldMatchNumber = currentMatchNumber; + } + if (currentReplayNumber != oldReplayNumber) { + replayNumber.setDouble(currentReplayNumber); + oldReplayNumber = currentReplayNumber; + } + if (currentMatchType != oldMatchType) { + matchType.setDouble(currentMatchType); + oldMatchType = currentMatchType; + } + if (currentControlWord != oldControlWord) { + controlWord.setDouble(currentControlWord); + oldControlWord = currentControlWord; + } + } + } + + private static DriverStation instance = new DriverStation(); + + // Joystick User Data + private static HALJoystickAxes[] m_joystickAxes = new HALJoystickAxes[kJoystickPorts]; + private static HALJoystickPOVs[] m_joystickPOVs = new HALJoystickPOVs[kJoystickPorts]; + private static HALJoystickButtons[] m_joystickButtons = new HALJoystickButtons[kJoystickPorts]; + private static MatchInfoData m_matchInfo = new MatchInfoData(); + + // Joystick Cached Data + private static HALJoystickAxes[] m_joystickAxesCache = new HALJoystickAxes[kJoystickPorts]; + private static HALJoystickPOVs[] m_joystickPOVsCache = new HALJoystickPOVs[kJoystickPorts]; + private static HALJoystickButtons[] m_joystickButtonsCache = + new HALJoystickButtons[kJoystickPorts]; + private static MatchInfoData m_matchInfoCache = new MatchInfoData(); + + // Joystick button rising/falling edge flags + private static int[] m_joystickButtonsPressed = new int[kJoystickPorts]; + private static int[] m_joystickButtonsReleased = new int[kJoystickPorts]; + + // preallocated byte buffer for button count + private static final ByteBuffer m_buttonCountBuffer = ByteBuffer.allocateDirect(1); + + private static final MatchDataSender m_matchDataSender; + + // Internal Driver Station thread + private static Thread m_thread; + + private static volatile boolean m_threadKeepAlive = true; + + private static final ReentrantLock m_cacheDataMutex = new ReentrantLock(); + + private static final Lock m_waitForDataMutex; + private static final Condition m_waitForDataCond; + private static int m_waitForDataCount; + private static final ThreadLocal m_lastCount = ThreadLocal.withInitial(() -> 0); + + private static boolean m_silenceJoystickWarning; + + // Robot state status variables + private static boolean m_userInDisabled; + private static boolean m_userInAutonomous; + private static boolean m_userInTeleop; + private static boolean m_userInTest; + + // Control word variables + private static final ReentrantLock m_controlWordMutex = new ReentrantLock(); + private static final ControlWord m_controlWordCache; + private static long m_lastControlWordUpdate; + + /** + * Gets an instance of the DriverStation. + * + * @return The DriverStation. + * @deprecated Use the static methods + */ + @Deprecated + public static DriverStation getInstance() { + return DriverStation.instance; + } + + /** + * DriverStation constructor. + * + *

The single DriverStation instance is created statically with the instance static member + * variable. + */ + private DriverStation() {} + + static { + HAL.initialize(500, 0); + m_waitForDataCount = 0; + m_waitForDataMutex = new ReentrantLock(); + m_waitForDataCond = m_waitForDataMutex.newCondition(); + + for (int i = 0; i < kJoystickPorts; i++) { + m_joystickButtons[i] = new HALJoystickButtons(); + m_joystickAxes[i] = new HALJoystickAxes(HAL.kMaxJoystickAxes); + m_joystickPOVs[i] = new HALJoystickPOVs(HAL.kMaxJoystickPOVs); + + m_joystickButtonsCache[i] = new HALJoystickButtons(); + m_joystickAxesCache[i] = new HALJoystickAxes(HAL.kMaxJoystickAxes); + m_joystickPOVsCache[i] = new HALJoystickPOVs(HAL.kMaxJoystickPOVs); + } + + m_controlWordCache = new ControlWord(); + m_lastControlWordUpdate = 0; + + m_matchDataSender = new MatchDataSender(); + + m_thread = new Thread(new DriverStationTask(), "FRCDriverStation"); + m_thread.setPriority((Thread.NORM_PRIORITY + Thread.MAX_PRIORITY) / 2); + + m_thread.start(); + } + + /** Kill the thread. */ + public static synchronized void release() { + m_threadKeepAlive = false; + if (m_thread != null) { + try { + m_thread.join(); + } catch (InterruptedException ex) { + Thread.currentThread().interrupt(); + } + m_thread = null; + } + } + + /** + * Report error to Driver Station. Optionally appends Stack trace to error message. + * + * @param error The error to report. + * @param printTrace If true, append stack trace to error string + */ + public static void reportError(String error, boolean printTrace) { + reportErrorImpl(true, 1, error, printTrace); + } + + /** + * Report error to Driver Station. Appends provided stack trace to error message. + * + * @param error The error to report. + * @param stackTrace The stack trace to append + */ + public static void reportError(String error, StackTraceElement[] stackTrace) { + reportErrorImpl(true, 1, error, stackTrace); + } + + /** + * Report warning to Driver Station. Optionally appends Stack trace to warning message. + * + * @param warning The warning to report. + * @param printTrace If true, append stack trace to warning string + */ + public static void reportWarning(String warning, boolean printTrace) { + reportErrorImpl(false, 1, warning, printTrace); + } + + /** + * Report warning to Driver Station. Appends provided stack trace to warning message. + * + * @param warning The warning to report. + * @param stackTrace The stack trace to append + */ + public static void reportWarning(String warning, StackTraceElement[] stackTrace) { + reportErrorImpl(false, 1, warning, stackTrace); + } + + private static void reportErrorImpl(boolean isError, int code, String error, boolean printTrace) { + reportErrorImpl(isError, code, error, printTrace, Thread.currentThread().getStackTrace(), 3); + } + + private static void reportErrorImpl( + boolean isError, int code, String error, StackTraceElement[] stackTrace) { + reportErrorImpl(isError, code, error, true, stackTrace, 0); + } + + private static void reportErrorImpl( + boolean isError, + int code, + String error, + boolean printTrace, + StackTraceElement[] stackTrace, + int stackTraceFirst) { + String locString; + if (stackTrace.length >= stackTraceFirst + 1) { + locString = stackTrace[stackTraceFirst].toString(); + } else { + locString = ""; + } + StringBuilder traceString = new StringBuilder(); + if (printTrace) { + boolean haveLoc = false; + for (int i = stackTraceFirst; i < stackTrace.length; i++) { + String loc = stackTrace[i].toString(); + traceString.append("\tat ").append(loc).append('\n'); + // get first user function + if (!haveLoc && !loc.startsWith("edu.wpi.first")) { + locString = loc; + haveLoc = true; + } + } + } + // HAL.sendError(isError, code, false, error, locString, traceString.toString(), true); + // for (StackTraceElement traceElement : stackTrace) + // System.err.println("\tat " + traceElement); + java.util.logging.Logger.getLogger(HAL.class.getName()).log(isError ? java.util.logging.Level.SEVERE : java.util.logging.Level.FINEST, "Warning at {0}: {1}", new Object[] {locString, error.split("\n")[0]}); + } + + /** + * The state of one joystick button. Button indexes begin at 1. + * + * @param stick The joystick to read. + * @param button The button index, beginning at 1. + * @return The state of the joystick button. + */ + public static boolean getStickButton(final int stick, final int button) { + if (stick < 0 || stick >= kJoystickPorts) { + throw new IllegalArgumentException("Joystick index is out of range, should be 0-5"); + } + if (button <= 0) { + reportJoystickUnpluggedError("Button indexes begin at 1 in WPILib for C++ and Java\n"); + return false; + } + + m_cacheDataMutex.lock(); + try { + if (button <= m_joystickButtons[stick].m_count) { + return (m_joystickButtons[stick].m_buttons & 1 << (button - 1)) != 0; + } + } finally { + m_cacheDataMutex.unlock(); + } + + reportJoystickUnpluggedWarning( + "Joystick Button " + + button + + " on port " + + stick + + " not available, check if controller is plugged in"); + return false; + } + + /** + * Whether one joystick button was pressed since the last check. Button indexes begin at 1. + * + * @param stick The joystick to read. + * @param button The button index, beginning at 1. + * @return Whether the joystick button was pressed since the last check. + */ + public static boolean getStickButtonPressed(final int stick, final int button) { + if (button <= 0) { + reportJoystickUnpluggedError("Button indexes begin at 1 in WPILib for C++ and Java\n"); + return false; + } + if (stick < 0 || stick >= kJoystickPorts) { + throw new IllegalArgumentException("Joystick index is out of range, should be 0-5"); + } + + m_cacheDataMutex.lock(); + try { + if (button <= m_joystickButtons[stick].m_count) { + // If button was pressed, clear flag and return true + if ((m_joystickButtonsPressed[stick] & 1 << (button - 1)) != 0) { + m_joystickButtonsPressed[stick] &= ~(1 << (button - 1)); + return true; + } else { + return false; + } + } + } finally { + m_cacheDataMutex.unlock(); + } + + reportJoystickUnpluggedWarning( + "Joystick Button " + + button + + " on port " + + stick + + " not available, check if controller is plugged in"); + return false; + } + + /** + * Whether one joystick button was released since the last check. Button indexes begin at 1. + * + * @param stick The joystick to read. + * @param button The button index, beginning at 1. + * @return Whether the joystick button was released since the last check. + */ + public static boolean getStickButtonReleased(final int stick, final int button) { + if (button <= 0) { + reportJoystickUnpluggedError("Button indexes begin at 1 in WPILib for C++ and Java\n"); + return false; + } + if (stick < 0 || stick >= kJoystickPorts) { + throw new IllegalArgumentException("Joystick index is out of range, should be 0-5"); + } + + m_cacheDataMutex.lock(); + try { + if (button <= m_joystickButtons[stick].m_count) { + // If button was released, clear flag and return true + if ((m_joystickButtonsReleased[stick] & 1 << (button - 1)) != 0) { + m_joystickButtonsReleased[stick] &= ~(1 << (button - 1)); + return true; + } else { + return false; + } + } + } finally { + m_cacheDataMutex.unlock(); + } + + reportJoystickUnpluggedWarning( + "Joystick Button " + + button + + " on port " + + stick + + " not available, check if controller is plugged in"); + return false; + } + + /** + * Get the value of the axis on a joystick. This depends on the mapping of the joystick connected + * to the specified port. + * + * @param stick The joystick to read. + * @param axis The analog axis value to read from the joystick. + * @return The value of the axis on the joystick. + */ + public static double getStickAxis(int stick, int axis) { + if (stick < 0 || stick >= kJoystickPorts) { + throw new IllegalArgumentException("Joystick index is out of range, should be 0-5"); + } + if (axis < 0 || axis >= HAL.kMaxJoystickAxes) { + throw new IllegalArgumentException("Joystick axis is out of range"); + } + + m_cacheDataMutex.lock(); + try { + if (axis < m_joystickAxes[stick].m_count) { + return m_joystickAxes[stick].m_axes[axis]; + } + } finally { + m_cacheDataMutex.unlock(); + } + + reportJoystickUnpluggedWarning( + "Joystick axis " + + axis + + " on port " + + stick + + " not available, check if controller is plugged in"); + return 0.0; + } + + /** + * Get the state of a POV on the joystick. + * + * @param stick The joystick to read. + * @param pov The POV to read. + * @return the angle of the POV in degrees, or -1 if the POV is not pressed. + */ + public static int getStickPOV(int stick, int pov) { + if (stick < 0 || stick >= kJoystickPorts) { + throw new IllegalArgumentException("Joystick index is out of range, should be 0-5"); + } + if (pov < 0 || pov >= HAL.kMaxJoystickPOVs) { + throw new IllegalArgumentException("Joystick POV is out of range"); + } + + m_cacheDataMutex.lock(); + try { + if (pov < m_joystickPOVs[stick].m_count) { + return m_joystickPOVs[stick].m_povs[pov]; + } + } finally { + m_cacheDataMutex.unlock(); + } + + reportJoystickUnpluggedWarning( + "Joystick POV " + + pov + + " on port " + + stick + + " not available, check if controller is plugged in"); + return -1; + } + + /** + * The state of the buttons on the joystick. + * + * @param stick The joystick to read. + * @return The state of the buttons on the joystick. + */ + public static int getStickButtons(final int stick) { + if (stick < 0 || stick >= kJoystickPorts) { + throw new IllegalArgumentException("Joystick index is out of range, should be 0-5"); + } + + m_cacheDataMutex.lock(); + try { + return m_joystickButtons[stick].m_buttons; + } finally { + m_cacheDataMutex.unlock(); + } + } + + /** + * Returns the number of axes on a given joystick port. + * + * @param stick The joystick port number + * @return The number of axes on the indicated joystick + */ + public static int getStickAxisCount(int stick) { + if (stick < 0 || stick >= kJoystickPorts) { + throw new IllegalArgumentException("Joystick index is out of range, should be 0-5"); + } + + m_cacheDataMutex.lock(); + try { + return m_joystickAxes[stick].m_count; + } finally { + m_cacheDataMutex.unlock(); + } + } + + /** + * Returns the number of POVs on a given joystick port. + * + * @param stick The joystick port number + * @return The number of POVs on the indicated joystick + */ + public static int getStickPOVCount(int stick) { + if (stick < 0 || stick >= kJoystickPorts) { + throw new IllegalArgumentException("Joystick index is out of range, should be 0-5"); + } + + m_cacheDataMutex.lock(); + try { + return m_joystickPOVs[stick].m_count; + } finally { + m_cacheDataMutex.unlock(); + } + } + + /** + * Gets the number of buttons on a joystick. + * + * @param stick The joystick port number + * @return The number of buttons on the indicated joystick + */ + public static int getStickButtonCount(int stick) { + if (stick < 0 || stick >= kJoystickPorts) { + throw new IllegalArgumentException("Joystick index is out of range, should be 0-5"); + } + + m_cacheDataMutex.lock(); + try { + return m_joystickButtons[stick].m_count; + } finally { + m_cacheDataMutex.unlock(); + } + } + + /** + * Gets the value of isXbox on a joystick. + * + * @param stick The joystick port number + * @return A boolean that returns the value of isXbox + */ + public static boolean getJoystickIsXbox(int stick) { + if (stick < 0 || stick >= kJoystickPorts) { + throw new IllegalArgumentException("Joystick index is out of range, should be 0-5"); + } + + return HAL.getJoystickIsXbox((byte) stick) == 1; + } + + /** + * Gets the value of type on a joystick. + * + * @param stick The joystick port number + * @return The value of type + */ + public static int getJoystickType(int stick) { + if (stick < 0 || stick >= kJoystickPorts) { + throw new IllegalArgumentException("Joystick index is out of range, should be 0-5"); + } + + return HAL.getJoystickType((byte) stick); + } + + /** + * Gets the name of the joystick at a port. + * + * @param stick The joystick port number + * @return The value of name + */ + public static String getJoystickName(int stick) { + if (stick < 0 || stick >= kJoystickPorts) { + throw new IllegalArgumentException("Joystick index is out of range, should be 0-5"); + } + + return HAL.getJoystickName((byte) stick); + } + + /** + * Returns the types of Axes on a given joystick port. + * + * @param stick The joystick port number + * @param axis The target axis + * @return What type of axis the axis is reporting to be + */ + public static int getJoystickAxisType(int stick, int axis) { + if (stick < 0 || stick >= kJoystickPorts) { + throw new IllegalArgumentException("Joystick index is out of range, should be 0-5"); + } + + return HAL.getJoystickAxisType((byte) stick, (byte) axis); + } + + /** + * Returns if a joystick is connected to the Driver Station. + * + *

This makes a best effort guess by looking at the reported number of axis, buttons, and POVs + * attached. + * + * @param stick The joystick port number + * @return true if a joystick is connected + */ + public static boolean isJoystickConnected(int stick) { + return getStickAxisCount(stick) > 0 + || getStickButtonCount(stick) > 0 + || getStickPOVCount(stick) > 0; + } + + /** + * Gets a value indicating whether the Driver Station requires the robot to be enabled. + * + * @return True if the robot is enabled, false otherwise. + */ + public static boolean isEnabled() { + m_controlWordMutex.lock(); + try { + updateControlWord(false); + return m_controlWordCache.getEnabled() && m_controlWordCache.getDSAttached(); + } finally { + m_controlWordMutex.unlock(); + } + } + + /** + * Gets a value indicating whether the Driver Station requires the robot to be disabled. + * + * @return True if the robot should be disabled, false otherwise. + */ + public static boolean isDisabled() { + return !isEnabled(); + } + + /** + * Gets a value indicating whether the Robot is e-stopped. + * + * @return True if the robot is e-stopped, false otherwise. + */ + public static boolean isEStopped() { + m_controlWordMutex.lock(); + try { + updateControlWord(false); + return m_controlWordCache.getEStop(); + } finally { + m_controlWordMutex.unlock(); + } + } + + /** + * Gets a value indicating whether the Driver Station requires the robot to be running in + * autonomous mode. + * + * @return True if autonomous mode should be enabled, false otherwise. + */ + public static boolean isAutonomous() { + m_controlWordMutex.lock(); + try { + updateControlWord(false); + return m_controlWordCache.getAutonomous(); + } finally { + m_controlWordMutex.unlock(); + } + } + + /** + * Gets a value indicating whether the Driver Station requires the robot to be running in + * autonomous mode and enabled. + * + * @return True if autonomous should be set and the robot should be enabled. + */ + public static boolean isAutonomousEnabled() { + m_controlWordMutex.lock(); + try { + updateControlWord(false); + return m_controlWordCache.getAutonomous() && m_controlWordCache.getEnabled(); + } finally { + m_controlWordMutex.unlock(); + } + } + + /** + * Gets a value indicating whether the Driver Station requires the robot to be running in + * operator-controlled mode. + * + * @return True if operator-controlled mode should be enabled, false otherwise. + * @deprecated Use isTeleop() instead. + */ + @Deprecated(since = "2022", forRemoval = true) + public static boolean isOperatorControl() { + return isTeleop(); + } + + /** + * Gets a value indicating whether the Driver Station requires the robot to be running in + * operator-controlled mode. + * + * @return True if operator-controlled mode should be enabled, false otherwise. + */ + public static boolean isTeleop() { + return !(isAutonomous() || isTest()); + } + + /** + * Gets a value indicating whether the Driver Station requires the robot to be running in + * operator-controller mode and enabled. + * + * @return True if operator-controlled mode should be set and the robot should be enabled. + * @deprecated Use isTeleopEnabled() instead. + */ + @Deprecated(since = "2022", forRemoval = true) + public static boolean isOperatorControlEnabled() { + return isTeleopEnabled(); + } + + /** + * Gets a value indicating whether the Driver Station requires the robot to be running in + * operator-controller mode and enabled. + * + * @return True if operator-controlled mode should be set and the robot should be enabled. + */ + public static boolean isTeleopEnabled() { + m_controlWordMutex.lock(); + try { + updateControlWord(false); + return !m_controlWordCache.getAutonomous() + && !m_controlWordCache.getTest() + && m_controlWordCache.getEnabled(); + } finally { + m_controlWordMutex.unlock(); + } + } + + /** + * Gets a value indicating whether the Driver Station requires the robot to be running in test + * mode. + * + * @return True if test mode should be enabled, false otherwise. + */ + public static boolean isTest() { + m_controlWordMutex.lock(); + try { + updateControlWord(false); + return m_controlWordCache.getTest(); + } finally { + m_controlWordMutex.unlock(); + } + } + + /** + * Gets a value indicating whether the Driver Station is attached. + * + * @return True if Driver Station is attached, false otherwise. + */ + public static boolean isDSAttached() { + m_controlWordMutex.lock(); + try { + updateControlWord(false); + return m_controlWordCache.getDSAttached(); + } finally { + m_controlWordMutex.unlock(); + } + } + + /** + * Gets if a new control packet from the driver station arrived since the last time this function + * was called. + * + * @return True if the control data has been updated since the last call. + */ + public static boolean isNewControlData() { + m_waitForDataMutex.lock(); + try { + int currentCount = m_waitForDataCount; + if (m_lastCount.get() != currentCount) { + m_lastCount.set(currentCount); + return true; + } + } finally { + m_waitForDataMutex.unlock(); + } + return false; + } + + /** + * Gets if the driver station attached to a Field Management System. + * + * @return true if the robot is competing on a field being controlled by a Field Management System + */ + public static boolean isFMSAttached() { + m_controlWordMutex.lock(); + try { + updateControlWord(false); + return m_controlWordCache.getFMSAttached(); + } finally { + m_controlWordMutex.unlock(); + } + } + + /** + * Get the game specific message. + * + * @return the game specific message + */ + public static String getGameSpecificMessage() { + m_cacheDataMutex.lock(); + try { + return m_matchInfo.gameSpecificMessage; + } finally { + m_cacheDataMutex.unlock(); + } + } + + /** + * Get the event name. + * + * @return the event name + */ + public static String getEventName() { + m_cacheDataMutex.lock(); + try { + return m_matchInfo.eventName; + } finally { + m_cacheDataMutex.unlock(); + } + } + + /** + * Get the match type. + * + * @return the match type + */ + public static MatchType getMatchType() { + int matchType; + m_cacheDataMutex.lock(); + try { + matchType = m_matchInfo.matchType; + } finally { + m_cacheDataMutex.unlock(); + } + switch (matchType) { + case 1: + return MatchType.Practice; + case 2: + return MatchType.Qualification; + case 3: + return MatchType.Elimination; + default: + return MatchType.None; + } + } + + /** + * Get the match number. + * + * @return the match number + */ + public static int getMatchNumber() { + m_cacheDataMutex.lock(); + try { + return m_matchInfo.matchNumber; + } finally { + m_cacheDataMutex.unlock(); + } + } + + /** + * Get the replay number. + * + * @return the replay number + */ + public static int getReplayNumber() { + m_cacheDataMutex.lock(); + try { + return m_matchInfo.replayNumber; + } finally { + m_cacheDataMutex.unlock(); + } + } + + /** + * Get the current alliance from the FMS. + * + * @return the current alliance + */ + public static Alliance getAlliance() { + AllianceStationID allianceStationID = HAL.getAllianceStation(); + if (allianceStationID == null) { + return Alliance.Invalid; + } + + switch (allianceStationID) { + case Red1: + case Red2: + case Red3: + return Alliance.Red; + + case Blue1: + case Blue2: + case Blue3: + return Alliance.Blue; + + default: + return Alliance.Invalid; + } + } + + /** + * Gets the location of the team's driver station controls. + * + * @return the location of the team's driver station controls: 1, 2, or 3 + */ + public static int getLocation() { + AllianceStationID allianceStationID = HAL.getAllianceStation(); + if (allianceStationID == null) { + return 0; + } + switch (allianceStationID) { + case Red1: + case Blue1: + return 1; + + case Red2: + case Blue2: + return 2; + + case Blue3: + case Red3: + return 3; + + default: + return 0; + } + } + + /** + * Wait for new data from the driver station. + * + *

Checks if new control data has arrived since the last waitForData call on the current + * thread. If new data has not arrived, returns immediately. + */ + public static void waitForData() { + waitForData(0); + } + + /** + * Wait for new data or for timeout, which ever comes first. If timeout is 0, wait for new data + * only. + * + *

Checks if new control data has arrived since the last waitForData call on the current + * thread. If new data has not arrived, returns immediately. + * + * @param timeoutSeconds The maximum time in seconds to wait. + * @return true if there is new data, otherwise false + */ + public static boolean waitForData(double timeoutSeconds) { + long startTimeMicroS = RobotController.getFPGATime(); + long timeoutMicroS = (long) (timeoutSeconds * 1e6); + m_waitForDataMutex.lock(); + try { + int currentCount = m_waitForDataCount; + if (m_lastCount.get() != currentCount) { + m_lastCount.set(currentCount); + return true; + } + while (m_waitForDataCount == currentCount) { + if (timeoutMicroS > 0) { + long nowMicroS = RobotController.getFPGATime(); + if (nowMicroS < startTimeMicroS + timeoutMicroS) { + // We still have time to wait + boolean signaled = + m_waitForDataCond.await( + startTimeMicroS + timeoutMicroS - nowMicroS, TimeUnit.MICROSECONDS); + if (!signaled) { + // Return false if a timeout happened + return false; + } + } else { + // Time has elapsed. + return false; + } + } else { + m_waitForDataCond.await(); + } + } + m_lastCount.set(m_waitForDataCount); + // Return true if we have received a proper signal + return true; + } catch (InterruptedException ex) { + // return false on a thread interrupt + Thread.currentThread().interrupt(); + return false; + } finally { + m_waitForDataMutex.unlock(); + } + } + + /** + * Return the approximate match time. The FMS does not send an official match time to the robots, + * but does send an approximate match time. The value will count down the time remaining in the + * current period (auto or teleop). Warning: This is not an official time (so it cannot be used to + * dispute ref calls or guarantee that a function will trigger before the match ends) The Practice + * Match function of the DS approximates the behavior seen on the field. + * + * @return Time remaining in current match period (auto or teleop) in seconds + */ + public static double getMatchTime() { + return HAL.getMatchTime(); + } + + /** + * Only to be used to tell the Driver Station what code you claim to be executing for diagnostic + * purposes only. + * + * @param entering If true, starting disabled code; if false, leaving disabled code + */ + public static void inDisabled(boolean entering) { + m_userInDisabled = entering; + } + + /** + * Only to be used to tell the Driver Station what code you claim to be executing for diagnostic + * purposes only. + * + * @param entering If true, starting autonomous code; if false, leaving autonomous code + */ + public static void inAutonomous(boolean entering) { + m_userInAutonomous = entering; + } + + /** + * Only to be used to tell the Driver Station what code you claim to be executing for diagnostic + * purposes only. + * + * @param entering If true, starting teleop code; if false, leaving teleop code + * @deprecated Use {@link #inTeleop(boolean)} instead. + */ + @Deprecated(since = "2022", forRemoval = true) + public static void inOperatorControl(boolean entering) { + m_userInTeleop = entering; + } + + /** + * Only to be used to tell the Driver Station what code you claim to be executing for diagnostic + * purposes only. + * + * @param entering If true, starting teleop code; if false, leaving teleop code + */ + public static void inTeleop(boolean entering) { + m_userInTeleop = entering; + } + + /** + * Only to be used to tell the Driver Station what code you claim to be executing for diagnostic + * purposes only. + * + * @param entering If true, starting test code; if false, leaving test code + */ + public static void inTest(boolean entering) { + m_userInTest = entering; + } + + /** Forces waitForData() to return immediately. */ + public static void wakeupWaitForData() { + m_waitForDataMutex.lock(); + try { + m_waitForDataCount++; + m_waitForDataCond.signalAll(); + } finally { + m_waitForDataMutex.unlock(); + } + } + + /** + * Allows the user to specify whether they want joystick connection warnings to be printed to the + * console. This setting is ignored when the FMS is connected -- warnings will always be on in + * that scenario. + * + * @param silence Whether warning messages should be silenced. + */ + public static void silenceJoystickConnectionWarning(boolean silence) { + m_silenceJoystickWarning = silence; + } + + /** + * Returns whether joystick connection warnings are silenced. This will always return false when + * connected to the FMS. + * + * @return Whether joystick connection warnings are silenced. + */ + public static boolean isJoystickConnectionWarningSilenced() { + return !isFMSAttached() && m_silenceJoystickWarning; + } + + /** + * Copy data from the DS task for the user. If no new data exists, it will just be returned, + * otherwise the data will be copied from the DS polling loop. + */ + protected static void getData() { + // Get the status of all of the joysticks + for (byte stick = 0; stick < kJoystickPorts; stick++) { + m_joystickAxesCache[stick].m_count = + HAL.getJoystickAxes(stick, m_joystickAxesCache[stick].m_axes); + m_joystickPOVsCache[stick].m_count = + HAL.getJoystickPOVs(stick, m_joystickPOVsCache[stick].m_povs); + m_joystickButtonsCache[stick].m_buttons = HAL.getJoystickButtons(stick, m_buttonCountBuffer); + m_joystickButtonsCache[stick].m_count = m_buttonCountBuffer.get(0); + } + + HAL.getMatchInfo(m_matchInfoCache); + + m_controlWordMutex.lock(); + try { + // Force a control word update, to make sure the data is the newest. + updateControlWord(true); + } finally { + m_controlWordMutex.unlock(); + } + + // lock joystick mutex to swap cache data + m_cacheDataMutex.lock(); + try { + for (int i = 0; i < kJoystickPorts; i++) { + // If buttons weren't pressed and are now, set flags in m_buttonsPressed + m_joystickButtonsPressed[i] |= + ~m_joystickButtons[i].m_buttons & m_joystickButtonsCache[i].m_buttons; + + // If buttons were pressed and aren't now, set flags in m_buttonsReleased + m_joystickButtonsReleased[i] |= + m_joystickButtons[i].m_buttons & ~m_joystickButtonsCache[i].m_buttons; + } + + // move cache to actual data + HALJoystickAxes[] currentAxes = m_joystickAxes; + m_joystickAxes = m_joystickAxesCache; + m_joystickAxesCache = currentAxes; + + HALJoystickButtons[] currentButtons = m_joystickButtons; + m_joystickButtons = m_joystickButtonsCache; + m_joystickButtonsCache = currentButtons; + + HALJoystickPOVs[] currentPOVs = m_joystickPOVs; + m_joystickPOVs = m_joystickPOVsCache; + m_joystickPOVsCache = currentPOVs; + + MatchInfoData currentInfo = m_matchInfo; + m_matchInfo = m_matchInfoCache; + m_matchInfoCache = currentInfo; + } finally { + m_cacheDataMutex.unlock(); + } + + wakeupWaitForData(); + m_matchDataSender.sendMatchData(); + } + + /** + * Reports errors related to unplugged joysticks Throttles the errors so that they don't overwhelm + * the DS. + */ + private static void reportJoystickUnpluggedError(String message) { + double currentTime = Timer.getFPGATimestamp(); + if (currentTime > m_nextMessageTime) { + reportError(message, false); + m_nextMessageTime = currentTime + JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL; + } + } + + /** + * Reports errors related to unplugged joysticks Throttles the errors so that they don't overwhelm + * the DS. + */ + private static void reportJoystickUnpluggedWarning(String message) { + if (isFMSAttached() || !m_silenceJoystickWarning) { + double currentTime = Timer.getFPGATimestamp(); + if (currentTime > m_nextMessageTime) { + reportWarning(message, false); + m_nextMessageTime = currentTime + JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL; + } + } + } + + /** Provides the service routine for the DS polling m_thread. */ + private static void run() { + int safetyCounter = 0; + while (m_threadKeepAlive) { + HAL.waitForDSData(); + getData(); + + if (isDisabled()) { + safetyCounter = 0; + } + + safetyCounter++; + if (safetyCounter >= 4) { + MotorSafety.checkMotors(); + safetyCounter = 0; + } + if (m_userInDisabled) { + HAL.observeUserProgramDisabled(); + } + if (m_userInAutonomous) { + HAL.observeUserProgramAutonomous(); + } + if (m_userInTeleop) { + HAL.observeUserProgramTeleop(); + } + if (m_userInTest) { + HAL.observeUserProgramTest(); + } + } + } + + /** + * Forces a control word cache update, and update the passed in control word. + * + * @param word Word to update. + */ + public static void updateControlWordFromCache(ControlWord word) { + m_controlWordMutex.lock(); + try { + updateControlWord(true); + word.update(m_controlWordCache); + } finally { + m_controlWordMutex.unlock(); + } + } + + /** + * Updates the data in the control word cache. Updates if the force parameter is set, or if 50ms + * have passed since the last update. + * + *

Must be called with m_controlWordMutex lock held. + * + * @param force True to force an update to the cache, otherwise update if 50ms have passed. + */ + private static void updateControlWord(boolean force) { + long now = System.currentTimeMillis(); + if (now - m_lastControlWordUpdate > 50 || force) { + HAL.getControlWord(m_controlWordCache); + m_lastControlWordUpdate = now; + } + } +} diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 3a33a29..28fc2fe 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -5,22 +5,23 @@ package frc4388.robot; import java.io.File; -import java.io.IOException; - +import java.nio.file.Files; +import java.nio.file.Path; import java.util.logging.Level; import java.util.logging.Logger; +import java.util.stream.StreamSupport; + +import com.diffplug.common.base.Errors; -import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import frc4388.utility.PathPlannerUtil; -import frc4388.utility.RobotLogger; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; import frc4388.utility.RobotTime; -import frc4388.utility.PathPlannerUtil.Path; /** * The VM is configured to automatically run this class, and to call the @@ -61,6 +62,33 @@ public class Robot extends TimedRobot { // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); + // addPeriodic(m_robotContainer::recordPeriodic, kDefaultPeriod); + SmartDashboard.putData(CommandScheduler.getInstance()); + SmartDashboard.putData("JVM Memory", new RunCommand(() -> {}) { + @Override public boolean runsWhenDisabled() { return true; } + @Override public String getName() { + if (isScheduled()) { + Runtime runtime = Runtime.getRuntime(); + long totalMemory = runtime.totalMemory() / 1_000_000; + long freeMemory = runtime.freeMemory() / 1_000_000; + long maxMemory = runtime.maxMemory() / 1_000_000; + return totalMemory - freeMemory + " MB / " + totalMemory + " MB / " + maxMemory + " MB"; + } + return "Not Running"; + } + }); + SmartDashboard.putData("Usable Deploy Space", new RunCommand(() -> {}) { + @Override public boolean runsWhenDisabled() { return true; } + @Override public String getName() { + if (isScheduled()) { + File deploy = Filesystem.getDeployDirectory(); + long usedSpace = Errors.suppress().getWithDefault(() -> Files.walk(deploy.toPath()).map(Path::toFile).filter(File::isFile).mapToLong(File::length).sum(), 0l) / 1_000_000; + long usableSpace = deploy.getUsableSpace() / 1_000_000; + return usedSpace + " MB / " + usableSpace + " MB"; + } + return "Not Running"; + } + }); } /** @@ -94,26 +122,13 @@ public class Robot extends TimedRobot { LOGGER.fine("disabledInit()"); m_robotTime.endMatchTime(); if (isTest()) { - try { - // IMPORTANT: Had to chown the pathplanner folder in order to save autos. - Path path = RobotLogger.getInstance().createPath(0.1, 0.1, false); - String pathPath = "recording." + System.currentTimeMillis() + ".path"; - File outputFile = Filesystem.getDeployDirectory().toPath().resolve("pathplanner").resolve(pathPath).toFile(); - outputFile.createNewFile(); - path.write(outputFile); - LOGGER.log(Level.WARNING, "----------------------------------------------------------------------"); - LOGGER.log(Level.WARNING, "----------------------------------------------------------------------"); - LOGGER.log(Level.WARNING, "----------------------------------------------------------------------"); - LOGGER.log(Level.WARNING, "----------------------------------------------------------------------"); - LOGGER.log(Level.WARNING, "Recorded path to {0} in the deploy directory on the RoboRIO", pathPath); - LOGGER.log(Level.WARNING, "----------------------------------------------------------------------"); - LOGGER.log(Level.WARNING, "----------------------------------------------------------------------"); - LOGGER.log(Level.WARNING, "----------------------------------------------------------------------"); - LOGGER.log(Level.WARNING, "----------------------------------------------------------------------"); - // LOGGER.finest(path::toString); - } catch (IOException e) { - e.printStackTrace(); - } + // IMPORTANT: Had to chown the pathplanner folder in order to save autos. + File outputFile = Filesystem.getDeployDirectory().toPath().resolve("pathplanner").resolve("recording." + System.currentTimeMillis() + ".path").toFile(); + if (Boolean.TRUE.equals(Errors.log().getWithDefault(outputFile::createNewFile, false))) { + m_robotContainer.createPath(null, null, false).write(outputFile); + LOGGER.log(Level.SEVERE, "Recorded path to {0}.", outputFile.getPath()); + } else + LOGGER.log(Level.SEVERE, "Unable to record path to {0}", outputFile.getPath()); } } @@ -130,12 +145,7 @@ public class Robot extends TimedRobot { @Override public void autonomousInit() { LOGGER.fine("autonomousInit()"); - try { - m_autonomousCommand = m_robotContainer.getAutonomousCommand(); - } catch (IOException e) { - // TODO Auto-generated catch block - e.printStackTrace(); - } + m_autonomousCommand = m_robotContainer.getAutonomousCommand(); // schedule the autonomous command (example) if (m_autonomousCommand != null) { @@ -176,6 +186,7 @@ public class Robot extends TimedRobot { @Override public void testInit() { + CommandScheduler.getInstance().cancelAll(); } /** diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index e5cc5a6..2599576 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -6,14 +6,27 @@ package frc4388.robot; import java.io.File; import java.io.IOException; +import java.nio.file.FileSystem; +import java.nio.file.FileSystems; +import java.nio.file.Files; +import java.nio.file.Path; +import java.nio.file.StandardWatchEventKinds; +import java.nio.file.WatchEvent; +import java.nio.file.WatchKey; +import java.nio.file.WatchService; import java.util.ArrayList; import java.util.Arrays; import java.util.Comparator; +import java.util.List; import java.util.Optional; +import java.util.function.Function; +import java.util.logging.Level; import java.util.logging.Logger; +import java.util.regex.Matcher; +import java.util.regex.Pattern; -import com.ctre.phoenix.motorcontrol.TalonFXControlMode; -import com.ctre.phoenix.motorcontrol.can.TalonFX; +import com.diffplug.common.base.Errors; +import com.diffplug.common.base.Errors.Plugins.Log; import com.pathplanner.lib.PathPlanner; import com.pathplanner.lib.PathPlannerTrajectory; import com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState; @@ -23,12 +36,23 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.networktables.NTSendableBuilder; +import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj.command.PrintCommand; +import edu.wpi.first.wpilibj.shuffleboard.EventImportance; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.NotifierCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.JoystickButton; @@ -38,6 +62,9 @@ import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.SwerveDrive; import frc4388.utility.LEDPatterns; +import frc4388.utility.PathPlannerUtil; +import frc4388.utility.SendableRunChooser; +import frc4388.utility.PathPlannerUtil.Path.Waypoint; import frc4388.utility.controller.DeadbandedXboxController; /** @@ -48,6 +75,7 @@ import frc4388.utility.controller.DeadbandedXboxController; * commands, and button mappings) should be declared here. */ public class RobotContainer { + private static final Logger LOGGER = Logger.getLogger(RobotContainer.class.getName()); /* RobotMap */ private final RobotMap m_robotMap = new RobotMap(); @@ -55,17 +83,28 @@ public class RobotContainer { private final SwerveDrive m_robotSwerveDrive = new SwerveDrive( m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro); - private final TalonFX m_testMotor = new TalonFX(23); - private final LED m_robotLED = new LED(m_robotMap.LEDController); /* Controllers */ private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); private final XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); + private String loadedPathName = ""; + private PathPlannerTrajectory loadedPathTrajectory = null; + private static final Function pathExtensionRemover = ((Function) Pattern + .compile(".path")::matcher).andThen(m -> m.replaceFirst("")); + + private final SendableRunChooser autoChooser = new SendableRunChooser<>(selectedAuto -> { + if (selectedAuto != null && !selectedAuto.equals(loadedPathName)) { + loadedPathTrajectory = PathPlanner.loadPath(pathExtensionRemover.apply(selectedAuto), 5.5, 50); + loadedPathName = selectedAuto; + } + }); + /** * The container for the robot. Contains subsystems, OI devices, and commands. */ + @SuppressWarnings("unchecked") public RobotContainer() { configureButtonBindings(); /* Default Commands */ @@ -77,14 +116,67 @@ public class RobotContainer { getDriverController().getRightX(), getDriverController().getRightY(), true), - m_robotSwerveDrive)); + m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand")); // continually sends updates to the Blinkin LED controller to keep the lights on - m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED)); + m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED).withName("LED update defaultCommand")); + try (WatchService watcher = FileSystems.getDefault().newWatchService()) { + WatchKey watchKey = Filesystem.getDeployDirectory().toPath().resolve("pathplanner").register( + FileSystems.getDefault().newWatchService(), + StandardWatchEventKinds.ENTRY_CREATE, StandardWatchEventKinds.ENTRY_MODIFY, + StandardWatchEventKinds.ENTRY_DELETE); + new NotifierCommand(() -> {}, TimedRobot.kDefaultPeriod) { + @Override + public void execute() { + var selectedAuto = autoChooser.getSelected(); + if (selectedAuto != null && !selectedAuto.getName().equals(loadedPathName)) { + setName("Path Watcher: Loading Path"); + // loadedPathTrajectory = PathPlanner.loadPath(pathExtensionRemover.apply(selectedAuto.getName()), 5.5, 50); + loadedPathName = selectedAuto.getName(); + } + if (!watchKey.pollEvents().isEmpty()) { + updateAutoChooser(); + LOGGER.info("Updated autonomous chooser."); + } + if (!watchKey.reset()) + LOGGER.severe("File watch key invalid."); + setName("Path Watcher: Waiting"); + } - m_testMotor.set(TalonFXControlMode.PercentOutput, 0); + @Override + public boolean runsWhenDisabled() { + return true; + } + }.schedule(); + pathLoaderCommand.schedule(); + } catch (IOException exception) { + LOGGER.log(Level.SEVERE, "Exception with path file watcher.", exception); + } + updateAutoChooser(); + recordInit(); } + private Command pathLoaderCommand = new CommandBase() { + @Override + public void execute() { + var selectedAuto = autoChooser.getSelected(); + if (selectedAuto != null && !selectedAuto.getName().equals(loadedPathName)) { + setName("LOADING PATH"); + Thread thread = new Thread(() -> { + loadedPathTrajectory = PathPlanner.loadPath(pathExtensionRemover.apply(selectedAuto.getName()), 5.5, 50); + setName("Path loader waiting"); + }, "Path Loader Thread"); + thread.start(); + } + } + + @Override + public boolean runsWhenDisabled() { + return true; + } + }.withName("Path loader"); + private Thread pathLoaderThread = new Thread(); + /** * Use this method to define your button->command mappings. Buttons can be * created by instantiating a {@link GenericHID} or one of its subclasses @@ -108,7 +200,7 @@ public class RobotContainer { .whenPressed(() -> m_robotSwerveDrive.highSpeed(true)); new JoystickButton(getDriverController(), XboxController.Button.kA.value) - .whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0)))); + .whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0)))); /* Operator Buttons */ // activates "Lit Mode" @@ -118,59 +210,39 @@ public class RobotContainer { .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); } - public Command[] buildAuto(double maxVel, double maxAccel, Object... inputs) { - ArrayList commands = new ArrayList<>(); - commands.add(new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset())); - - PIDController xController = SwerveDriveConstants.X_CONTROLLER; - PIDController yController = SwerveDriveConstants.Y_CONTROLLER; - ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER; - thetaController.enableContinuousInput(-Math.PI, Math.PI); - - // parse input - for (int i = 0; i < inputs.length; i++) { - if (inputs[i] instanceof String) { - PathPlannerTrajectory traj = PathPlanner.loadPath(inputs[i].toString(), maxVel, maxAccel); - PathPlannerState initState = (PathPlannerState) traj.sample(0); - - commands.add(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(new Pose2d(initState.poseMeters.getTranslation(), initState.holonomicRotation)))); - commands.add(new PPSwerveControllerCommand(traj, m_robotSwerveDrive::getOdometry, m_robotSwerveDrive.m_kinematics, xController, yController, thetaController, m_robotSwerveDrive::setModuleStates, m_robotSwerveDrive)); - } else if (inputs[i] instanceof Command) - commands.add((Command) inputs[i]); - } - - commands.add(new InstantCommand(m_robotSwerveDrive::stopModules)); - Command[] ret = new Command[commands.size()]; - ret = commands.toArray(ret); - return ret; - } - /** * Use this to pass the autonomous command to the main {@link Robot} class. * * @return the command to run in autonomous - * @throws IOException */ - public Command getAutonomousCommand() throws IOException { - // https://github.com/mjansen4857/pathplanner/wiki <-- Pathplanner Wiki + public Command getAutonomousCommand() { + var selectedAuto = autoChooser.getSelected(); + if (loadedPathTrajectory != null) { + // if (selectedAuto != null && !selectedAuto.getName().equals(loadedPathName)) + // loadedPathTrajectory = + // PathPlanner.loadPath(pathExtensionRemover.apply(selectedAuto.getName()), 5.5, + // 50); + PIDController xController = SwerveDriveConstants.X_CONTROLLER; + PIDController yController = SwerveDriveConstants.Y_CONTROLLER; + ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER; + thetaController.enableContinuousInput(-Math.PI, Math.PI); - // Command[] trajCommands = buildAuto(0.5, 0.5, "Move Forward", "Move Down"); - // SequentialCommandGroup ret = new SequentialCommandGroup(trajCommands); - // return (new ParallelCommandGroup(buildAuto(0.1, 0.1, ret, new InstantCommand(() -> m_testMotor.set(TalonFXControlMode.PercentOutput, 0.2))))); - Optional recordedAuto = Arrays.stream(Filesystem.getDeployDirectory().toPath().resolve("pathplanner").toFile().listFiles()).filter(f -> f.getName().startsWith("recording")).max(Comparator.comparingLong(File::lastModified)); - if (recordedAuto.isPresent()) { - String name = recordedAuto.orElseThrow().getName().replace(".path", ""); - Logger.getLogger(this.getClass().getName()).fine(name); - return new SequentialCommandGroup(buildAuto(1.0, 1.0, name)); + PathPlannerState initialState = loadedPathTrajectory.getInitialState(); + Pose2d initialPosition = new Pose2d(initialState.poseMeters.getTranslation(), initialState.holonomicRotation); + return new SequentialCommandGroup( + new InstantCommand(m_robotSwerveDrive.m_gyro::reset), + new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(initialPosition)), + new PPSwerveControllerCommand(loadedPathTrajectory, m_robotSwerveDrive::getOdometry, + m_robotSwerveDrive.m_kinematics, xController, yController, thetaController, + m_robotSwerveDrive::setModuleStates, m_robotSwerveDrive), + new InstantCommand(m_robotSwerveDrive::stopModules)).withName("Run Autonomous Path"); + } else { + Logger.getLogger(this.getClass().getName()).severe("No auto selected."); + return new RunCommand(() -> { + }).withName("No Autonomous Path"); } - Logger.getLogger(this.getClass().getName()).severe("No recorded autos found."); - return new InstantCommand(); - // return new PrintCommand("No recorded autos found."); } - /** - * Add your docs here. - */ public XboxController getDriverController() { return m_driverXbox; } @@ -183,10 +255,67 @@ public class RobotContainer { m_robotSwerveDrive.resetOdometry(pose); } - /** - * Add your docs here. - */ public XboxController getOperatorController() { return m_operatorXbox; } + + private void updateAutoChooser() { + Arrays.stream(Filesystem.getDeployDirectory().toPath().resolve("pathplanner").toFile().listFiles()) + .filter(file -> file.getName().endsWith(".path")).sorted(Comparator.comparingLong(File::lastModified)) + .forEachOrdered(file -> autoChooser.addOption(file.getName(), file)); + SmartDashboard.putData("Auto Chooser", autoChooser); + } + + private final List pathPoints = new ArrayList<>(); + + public void recordInit() { + SmartDashboard.putData("Recording", + new RunCommand(this::recordPeriodic) { + @Override + public void end(boolean interupted) { + new InstantCommand(RobotContainer.this::saveRecording) { + @Override + public boolean runsWhenDisabled() { + return true; + } + }.withName("Save Recording").schedule(); + } + }.withName("Record Path (Cancel to Save)")); + } + + private void saveRecording() { + // IMPORTANT: Had to chown the pathplanner folder in order to save autos. + File outputFile = Filesystem.getDeployDirectory().toPath().resolve("pathplanner") + .resolve("recording." + System.currentTimeMillis() + ".path").toFile(); + if (Boolean.TRUE.equals(Errors.log().getWithDefault(outputFile::createNewFile, false))) { + createPath(null, null, false).write(outputFile); + autoChooser.setDefaultOption(outputFile.getName(), outputFile); + LOGGER.log(Level.SEVERE, "Recorded path to {0}.", outputFile.getPath()); + } else + LOGGER.log(Level.SEVERE, "Unable to record path to {0}", outputFile.getPath()); + } + + public void recordPeriodic() { + Translation2d position = m_robotSwerveDrive.m_poseEstimator.getEstimatedPosition().getTranslation(); + Rotation2d rotation = m_robotSwerveDrive.m_gyro.getRotation2d(); + Translation2d velocity = new Translation2d(m_robotSwerveDrive.chassisSpeeds.vxMetersPerSecond, + m_robotSwerveDrive.chassisSpeeds.vyMetersPerSecond); + Waypoint waypoint = new Waypoint(position, position, position, rotation.getDegrees(), false, velocity.getNorm(), + false); + pathPoints.add(waypoint); + } + + public PathPlannerUtil.Path createPath(Double maxVelocity, Double maxAcceleration, Boolean isReversed) { + PathPlannerUtil.Path path = new PathPlannerUtil.Path(); + for (int i = 0; i < pathPoints.size() - 2; i++) + pathPoints.get(i).nextControl = pathPoints.get(i + 1).anchorPoint; + for (int i = 1; i < pathPoints.size() - 1; i++) + pathPoints.get(i).prevControl = pathPoints.get(i - 1).anchorPoint; + path.waypoints = Optional.ofNullable(pathPoints.toArray(PathPlannerUtil.Path.Waypoint[]::new)); + path.maxVelocity = Optional.ofNullable(maxVelocity); + path.maxAcceleration = Optional.ofNullable(maxAcceleration); + path.isReversed = Optional.ofNullable(isReversed); + pathPoints.clear(); + return path; + } } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index f8ed27d..a8b36f6 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -4,8 +4,8 @@ package frc4388.robot.subsystems; -import com.ctre.phoenix.sensors.WPI_PigeonIMU; import com.ctre.phoenix.sensors.PigeonIMU.FusionStatus; +import com.ctre.phoenix.sensors.WPI_PigeonIMU; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; @@ -23,8 +23,6 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.OIConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.Gains; -import frc4388.utility.RobotLogger; -import frc4388.utility.PathPlannerUtil.Path.Waypoint.Point; public class SwerveDrive extends SubsystemBase { @@ -57,7 +55,7 @@ public class SwerveDrive extends SubsystemBase { public double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW; public boolean ignoreAngles; public Rotation2d rotTarget = new Rotation2d();; - private ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); + public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); private final Field2d m_field = new Field2d(); @@ -160,23 +158,7 @@ public class SwerveDrive extends SubsystemBase { @Override public void periodic() { - // System.err.println(m_gyro.getFusedHeading() +" aaa"); updateOdometry(); - // m_gyro.setFusedHeadingToCompass(); - // m_gyro.setYawToCompass(); - Pose2d pos = m_poseEstimator.getEstimatedPosition(); - RobotLogger.getInstance().put( - /* anchorPointX */ pos.getX(), - /* anchorPointY */ pos.getY(), - /* prevControlX */ pos.getX(), - /* prevControlY */ pos.getY(), - /* nextControlX */ pos.getX(), - /* nextControlY */ pos.getY(), - /* holonomicAngle */ m_gyro.getRotation2d().getDegrees(), - /* isReversal */ false, - /* velOverride */ null, - /* isLocked */ false - ); SmartDashboard.putNumber("Pigeon Fused Heading", m_gyro.getFusedHeading(fstatus)); SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw()); SmartDashboard.putNumber("Pigeon Get Angle", m_gyro.getAngle()); @@ -186,8 +168,7 @@ public class SwerveDrive extends SubsystemBase { // m_gyro.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_9_SixDeg_YPR, 1, SwerveDriveConstants.SWERVE_TIMEOUT_MS); // m_gyro.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_6_SensorFusion, 1, SwerveDriveConstants.SWERVE_TIMEOUT_MS); // m_gyro.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_1_General, 1, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - - // m_field.setRobotPose(m_poseEstimator.getEstimatedPosition()); + m_field.setRobotPose(m_poseEstimator.getEstimatedPosition()); super.periodic(); } diff --git a/src/main/java/frc4388/utility/AnsiLogging.java b/src/main/java/frc4388/utility/AnsiLogging.java index 1b6c6c6..c37a3a6 100644 --- a/src/main/java/frc4388/utility/AnsiLogging.java +++ b/src/main/java/frc4388/utility/AnsiLogging.java @@ -19,6 +19,10 @@ import java.util.logging.Logger; import org.fusesource.jansi.Ansi; import org.fusesource.jansi.Ansi.Attribute; import org.fusesource.jansi.Ansi.Color; + +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + import org.fusesource.jansi.AnsiConsole; public class AnsiLogging extends ConsoleHandler { @@ -35,7 +39,7 @@ public class AnsiLogging extends ConsoleHandler { // Replaces standard output streams with org.fusesource.jansi.AnsiPrintStreams. AnsiConsole.systemInstall(); // Replaces standard output stream with java.util.logging.Logger. - System.setOut(printStreamLogger(Logger.getGlobal(), Level.INFO)); + System.setOut(printStreamLogger(Logger.getGlobal(), Level.ALL)); // Replaces standard error output stream with java.util.logging.Logger. System.setErr(printStreamLogger(Logger.getGlobal(), Level.SEVERE)); } catch (IOException exception) { @@ -66,6 +70,7 @@ public class AnsiLogging extends ConsoleHandler { } else source = logRecord.getLoggerName(); String message = formatMessage(logRecord); + boolean multiline = message.contains("\n"); String throwable = ""; if (logRecord.getThrown() != null) { StringWriter sw = new StringWriter(); @@ -74,6 +79,7 @@ public class AnsiLogging extends ConsoleHandler { logRecord.getThrown().printStackTrace(pw); } throwable = sw.toString(); + multiline = true; } Ansi ansi; if (logRecord.getLevel() == Level.SEVERE) ansi = ansi().fgBright(Color.RED); @@ -84,7 +90,7 @@ public class AnsiLogging extends ConsoleHandler { else if (logRecord.getLevel() == Level.FINER) ansi = ansi().fg(Color.MAGENTA); else if (logRecord.getLevel() == Level.FINEST) ansi = ansi().fgBright(Color.BLACK); else ansi = ansi().fg(Color.DEFAULT); - String format = ansi.bold().a(Attribute.UNDERLINE).a("%1$tb %1$td, %1$tY %1$tl:%1$tM:%1$tS %1$Tp %2$s %4$s:").boldOff().a(Attribute.UNDERLINE_OFF).a("%n%5$s%6$s").reset().a("%n").toString(); + String format = ansi.bold().a(Attribute.UNDERLINE).a("%1$tb %1$td, %1$tY %1$tl:%1$tM:%1$tS %1$Tp %2$s %4$s:").boldOff().a(Attribute.UNDERLINE_OFF).a(multiline ? "%n%5$s%6$s" : " %5$s%6$s").reset().a("%n").toString(); return String.format(format, zdt, source, logRecord.getLoggerName(), logRecord.getLevel().getLocalizedName(), message, throwable); } }; diff --git a/src/main/java/frc4388/utility/PathPlannerUtil.java b/src/main/java/frc4388/utility/PathPlannerUtil.java index 4c2c7b2..2d5cf78 100644 --- a/src/main/java/frc4388/utility/PathPlannerUtil.java +++ b/src/main/java/frc4388/utility/PathPlannerUtil.java @@ -2,53 +2,26 @@ package frc4388.utility; import java.io.File; import java.util.Optional; + import com.diffplug.common.base.Errors; +import com.fasterxml.jackson.annotation.JsonInclude.Include; import com.fasterxml.jackson.databind.ObjectMapper; import com.fasterxml.jackson.datatype.jdk8.Jdk8Module; -public class PathPlannerUtil { +import edu.wpi.first.math.geometry.Translation2d; + +public final class PathPlannerUtil { public static final class Path { - public Optional[] waypoints; - - public static final class Waypoint { - public Waypoint() {} - public Waypoint(Double anchorPointX, Double anchorPointY, Double prevControlX, Double prevControlY, Double nextControlX, Double nextControlY, Double holonomicAngle, Boolean isReversal, Double velOverride, Boolean isLocked) { - this.anchorPoint = anchorPointX == null && anchorPointY == null ? Optional.empty() : Optional.of(Point.of(anchorPointX, anchorPointY)); - this.prevControl = prevControlX == null && prevControlY == null ? Optional.empty() : Optional.of(Point.of(prevControlX, prevControlY)); - this.nextControl = nextControlX == null && nextControlY == null ? Optional.empty() : Optional.of(Point.of(nextControlX, nextControlY)); - this.holonomicAngle = Optional.ofNullable(holonomicAngle); - this.isReversal = Optional.ofNullable(isReversal); - this.velOverride = Optional.ofNullable(velOverride); - this.isLocked = Optional.ofNullable(isLocked); - } - public Optional anchorPoint; - public Optional prevControl; - public Optional nextControl; - - public static final class Point { - public Point() {} - public static Point of(Double x, Double y) { - Point point = new Point(); - point.x = Optional.ofNullable(x); - point.y = Optional.ofNullable(y); - return point; - } - public Optional x; - public Optional y; - } - - public Optional holonomicAngle; - public Optional isReversal; - public Optional velOverride; - public Optional isLocked; - } - + public Optional waypoints; public Optional maxVelocity; public Optional maxAcceleration; public Optional isReversed; private static final ObjectMapper objectMapper = new ObjectMapper(); - static { objectMapper.registerModule(new Jdk8Module()); } + static { + objectMapper.registerModule(new Jdk8Module()); + objectMapper.setSerializationInclusion(Include.NON_EMPTY); + } public static Path read(File src) { return Errors.log().getWithDefault(() -> objectMapper.readValue(src, Path.class), null); @@ -58,9 +31,27 @@ public class PathPlannerUtil { Errors.log().run(() -> objectMapper.writeValue(resultFile, this)); } - @Override - public String toString() { - return Errors.log().getWithDefault(() -> objectMapper.writeValueAsString(this), super.toString()); + public static final class Waypoint { + public Optional anchorPoint; + public Optional prevControl; + public Optional nextControl; + public Optional holonomicAngle; + public Optional isReversal; + public Optional velOverride; + public Optional isLocked; + + public Waypoint() { + } + + public Waypoint(Translation2d anchorPoint, Translation2d prevControl, Translation2d nextControl, Double holonomicAngle, Boolean isReversal, Double velOverride, Boolean isLocked) { + this.anchorPoint = Optional.ofNullable(anchorPoint); + this.prevControl = Optional.ofNullable(prevControl); + this.nextControl = Optional.ofNullable(nextControl); + this.holonomicAngle = Optional.ofNullable(holonomicAngle); + this.isReversal = Optional.ofNullable(isReversal); + this.velOverride = Optional.ofNullable(velOverride); + this.isLocked = Optional.ofNullable(isLocked); + } } } } diff --git a/src/main/java/frc4388/utility/RobotGyro.java.old b/src/main/java/frc4388/utility/RobotGyro.java.old deleted file mode 100644 index 56390d4..0000000 --- a/src/main/java/frc4388/utility/RobotGyro.java.old +++ /dev/null @@ -1,245 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc4388.utility; - -import com.ctre.phoenix.sensors.PigeonIMU; -import com.ctre.phoenix.sensors.PigeonIMU.CalibrationMode; -import com.kauailabs.navx.frc.AHRS; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.util.sendable.Sendable; -import edu.wpi.first.util.sendable.SendableBuilder; -import edu.wpi.first.wpilibj.PIDSource; -import edu.wpi.first.wpilibj.PIDSourceType; -import edu.wpi.first.wpilibj.interfaces.Gyro; - -/** - * Gyro class that allows for interchangeable use between a pigeon and a navX - */ -public class RobotGyro implements Gyro, PIDSource, Sendable { - private RobotTime m_robotTime = RobotTime.getInstance(); - - private PigeonIMU m_pigeon = null; - private AHRS m_navX = null; - public boolean m_isGyroAPigeon; // true if pigeon, false if navX - - private double m_lastPigeonAngle; - private double m_deltaPigeonAngle; - - /** - * Creates a Gyro based on a pigeon - * - * @param gyro the gyroscope to use for Gyro - */ - public RobotGyro(PigeonIMU gyro) { - m_pigeon = gyro; - m_isGyroAPigeon = true; - } - - /** - * Creates a Gyro based on a navX - * - * @param gyro the gyroscope to use for Gyro - */ - public RobotGyro(AHRS gyro) { - m_navX = gyro; - m_isGyroAPigeon = false; - } - - /** - * Run in periodic if you are using a pigeon. Updates a delta angle so that it - * can calculate getRate(). Note - * that the getRate() method for a navX will likely be much more accurate than - * for a pigeon. - */ - public void updatePigeonDeltas() { - double currentPigeonAngle = getAngle(); - m_deltaPigeonAngle = currentPigeonAngle - m_lastPigeonAngle; - m_lastPigeonAngle = currentPigeonAngle; - } - - /** - *

- * NavX: - *

- * Calibrate the gyro by running for a number of samples and computing the - * center value. Then use - * the center value as the Accumulator center value for subsequent measurements. - * It's important to - * make sure that the robot is not moving while the centering calculations are - * in progress, this - * is typically done when the robot is first turned on while it's sitting at - * rest before the - * competition starts. - * - *

- * Pigeon: - *

- * Calibrate the gyro by collecting data at a range of tempuratures. Allow - * pigeon to cool, then boot - * into calibration mode. For faster calibration, use a heat lamp to heat up the - * pigeon. Once the pigeon - * has seen a reasonable range of tempuratures, it will exit calibration mode. - * It's important to - * make sure that the robot is not moving while the tempurature calculations are - * in progress, this - * is typically done when the robot is first turned on while it's sitting at - * rest before the - * competition starts. - */ - @Override - public void calibrate() { - if (m_isGyroAPigeon) - m_pigeon.enterCalibrationMode(CalibrationMode.Temperature); - else - m_navX.calibrate(); - } - - @Override - public void reset() { - if (m_isGyroAPigeon) - m_pigeon.setYaw(0); - else - m_navX.reset(); - } - - /** - * Get Yaw, Pitch, and Roll data. - * - * @return ypr_deg Array with yaw[0], pitch[1], and roll[2] data. - * Yaw is within [-368,640, +368,640] degrees. - * Pitch is within [-90,+90] degrees. - * Roll is within [-90,+90] degrees. - */ - private double[] getPigeonAngles() { - double[] angles = new double[3]; - m_pigeon.getYawPitchRoll(angles); - return angles; - } - - @Override - public double getAngle() { - if (m_isGyroAPigeon) { - return getPigeonAngles()[0]; - } else { - return m_navX.getAngle(); - } - } - - /** - * Gets an absolute heading of the robot - * - * @return heading from -180 to 180 degrees - */ - public double getHeading() { - return getHeading(getAngle()); - } - - /** - * Gets an absolute heading of the robot - * - * @return heading from -180 to 180 degrees - */ - public double getHeading(double angle) { - return Math.IEEEremainder(angle, 360); - } - - /** - * Returns the current pitch value (in degrees, from -90 to 90) - * reported by the sensor. Pitch is a measure of rotation around - * the Y Axis. - * - * @return The current pitch value in degrees (-90 to 90). - */ - public double getPitch() { - if (m_isGyroAPigeon) { - return MathUtil.clamp(getPigeonAngles()[1], -90, 90); - } else { - return MathUtil.clamp(m_navX.getPitch(), -90, 90); - } - } - - /** - * Returns the current roll value (in degrees, from -90 to 90) - * reported by the sensor. Roll is a measure of rotation around - * the X Axis. - * - * @return The current roll value in degrees (-90 to 90). - */ - public double getRoll() { - if (m_isGyroAPigeon) { - return MathUtil.clamp(getPigeonAngles()[2], -90, 90); - } else { - return MathUtil.clamp(m_navX.getRoll(), -90, 90); - } - } - - @Override - public double getRate() { - if (m_isGyroAPigeon) { - return m_deltaPigeonAngle / m_robotTime.m_deltaTime * 1000; - } else { - return m_navX.getRate(); - } - } - - public PigeonIMU getPigeon() { - return m_pigeon; - } - - public AHRS getNavX() { - return m_navX; - } - - @Override - public void close() throws Exception { - - } - - // Begin old GyroBase class - private PIDSourceType m_pidSource = PIDSourceType.kDisplacement; - - /** - * Set which parameter of the gyro you are using as a process control variable. - * The Gyro class - * supports the rate and displacement parameters - * - * @param pidSource An enum to select the parameter. - */ - @Override - public void setPIDSourceType(PIDSourceType pidSource) { - m_pidSource = pidSource; - } - - @Override - public PIDSourceType getPIDSourceType() { - return m_pidSource; - } - - /** - * Get the output of the gyro for use with PIDControllers. May be the angle or - * rate depending on - * the set PIDSourceType - * - * @return the output according to the gyro - */ - @Override - public double pidGet() { - switch (m_pidSource) { - case kRate: - return getRate(); - case kDisplacement: - return getAngle(); - default: - return 0.0; - } - } - - @Override - public void initSendable(SendableBuilder builder) { - builder.setSmartDashboardType("Gyro"); - builder.addDoubleProperty("Value", this::getAngle, null); - } -} diff --git a/src/main/java/frc4388/utility/RobotLogger.java b/src/main/java/frc4388/utility/RobotLogger.java deleted file mode 100644 index 16c1a33..0000000 --- a/src/main/java/frc4388/utility/RobotLogger.java +++ /dev/null @@ -1,51 +0,0 @@ -package frc4388.utility; - -import java.util.ArrayList; -import java.util.List; -import java.util.Objects; -import java.util.Optional; - -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc4388.utility.PathPlannerUtil.Path.Waypoint; - -public final class RobotLogger { - private static RobotLogger instance = null; - - public static RobotLogger getInstance() { - return Objects.requireNonNullElseGet(instance, () -> instance = new RobotLogger()); - } - - public static double getTime() { - return DriverStation.getMatchTime(); - } - - private RobotLogger() { - sendableChooser.setDefaultOption("Disable", false); - sendableChooser.addOption("Enable", true); - SmartDashboard.putData("Recording", sendableChooser); - } - - private final List data = new ArrayList<>(); - private final SendableChooser sendableChooser = new SendableChooser<>(); - - double lastVelocityMetersPerSecond = 0; - public void put(Double anchorPointX, Double anchorPointY, Double prevControlX, Double prevControlY, Double nextControlX, Double nextControlY, Double holonomicAngle, Boolean isReversal, Double velOverride, Boolean isLocked) { - if (Boolean.TRUE.equals(sendableChooser.getSelected())) { - Waypoint waypoint = new Waypoint(anchorPointX, anchorPointY, prevControlX, prevControlY, nextControlX, nextControlY, holonomicAngle, isReversal, velOverride, isLocked); - data.add(waypoint); - } - } - - @SuppressWarnings("unchecked") - public PathPlannerUtil.Path createPath(Double maxVelocity, Double maxAcceleration, Boolean isReversed) { - PathPlannerUtil.Path path = new PathPlannerUtil.Path(); - path.waypoints = data.stream().map(Optional::ofNullable).toArray(Optional[]::new); - path.maxVelocity = Optional.ofNullable(maxVelocity); - path.maxAcceleration = Optional.ofNullable(maxAcceleration); - path.isReversed = Optional.ofNullable(isReversed); - data.clear(); - return path; - } -} diff --git a/src/main/java/frc4388/utility/SendableRunChooser.java b/src/main/java/frc4388/utility/SendableRunChooser.java new file mode 100644 index 0000000..f2e97b3 --- /dev/null +++ b/src/main/java/frc4388/utility/SendableRunChooser.java @@ -0,0 +1,182 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +// package edu.wpi.first.wpilibj.smartdashboard; +package frc4388.utility; + +import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam; + +import edu.wpi.first.networktables.NTSendable; +import edu.wpi.first.networktables.NTSendableBuilder; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.util.sendable.SendableRegistry; +import java.util.ArrayList; +import java.util.LinkedHashMap; +import java.util.List; +import java.util.Map; +import java.util.concurrent.atomic.AtomicInteger; +import java.util.concurrent.locks.ReentrantLock; +import java.util.function.Consumer; + +/** + * The {@link SendableRunChooser} class is a useful tool for presenting a selection of options to the + * {@link SmartDashboard}. + * + *

For instance, you may wish to be able to select between multiple autonomous modes. You can do + * this by putting every possible Command you want to run as an autonomous into a {@link + * SendableRunChooser} and then put it into the {@link SmartDashboard} to have a list of options appear + * on the laptop. Once autonomous starts, simply ask the {@link SendableRunChooser} what the selected + * value is. + * + * @param The type of the values to be stored + */ +public class SendableRunChooser implements NTSendable, AutoCloseable { + /** The key for the default value. */ + private static final String DEFAULT = "default"; + /** The key for the selected option. */ + private static final String SELECTED = "selected"; + /** The key for the active option. */ + private static final String ACTIVE = "active"; + /** The key for the option array. */ + private static final String OPTIONS = "options"; + /** The key for the instance number. */ + private static final String INSTANCE = ".instance"; + /** A map linking strings to the objects the represent. */ + private final Map m_map = new LinkedHashMap<>(); + + private String m_defaultChoice = ""; + private final int m_instance; + private static final AtomicInteger s_instances = new AtomicInteger(); + + private Consumer m_consumer; + + /** Instantiates a {@link SendableRunChooser}. */ + public SendableRunChooser(Consumer consumer) { + m_consumer = consumer; + m_instance = s_instances.getAndIncrement(); + SendableRegistry.add(this, "SendableChooser", m_instance); + } + + @Override + public void close() { + SendableRegistry.remove(this); + } + + /** + * Adds the given object to the list of options. On the {@link SmartDashboard} on the desktop, the + * object will appear as the given name. + * + * @param name the name of the option + * @param object the option + */ + public void addOption(String name, V object) { + m_map.put(name, object); + } + + /** + * Adds the given object to the list of options. + * + * @param name the name of the option + * @param object the option + * @deprecated Use {@link #addOption(String, Object)} instead. + */ + @Deprecated + public void addObject(String name, V object) { + addOption(name, object); + } + + /** + * Adds the given object to the list of options and marks it as the default. Functionally, this is + * very close to {@link #addOption(String, Object)} except that it will use this as the default + * option if none other is explicitly selected. + * + * @param name the name of the option + * @param object the option + */ + public void setDefaultOption(String name, V object) { + requireNonNullParam(name, "name", "setDefaultOption"); + + m_defaultChoice = name; + addOption(name, object); + } + + /** + * Adds the given object to the list of options and marks it as the default. + * + * @param name the name of the option + * @param object the option + * @deprecated Use {@link #setDefaultOption(String, Object)} instead. + */ + @Deprecated + public void addDefault(String name, V object) { + setDefaultOption(name, object); + } + + /** + * Returns the selected option. If there is none selected, it will return the default. If there is + * none selected and no default, then it will return {@code null}. + * + * @return the option selected + */ + public V getSelected() { + m_mutex.lock(); + try { + if (m_selected != null) { + return m_map.get(m_selected); + } else { + return m_map.get(m_defaultChoice); + } + } finally { + m_mutex.unlock(); + } + } + + private String m_selected; + private final List m_activeEntries = new ArrayList<>(); + private final ReentrantLock m_mutex = new ReentrantLock(); + + @Override + public void initSendable(NTSendableBuilder builder) { + builder.setSmartDashboardType("String Chooser"); + builder.getEntry(INSTANCE).setDouble(m_instance); + builder.addStringProperty(DEFAULT, () -> m_defaultChoice, null); + builder.addStringArrayProperty(OPTIONS, () -> m_map.keySet().toArray(new String[0]), null); + builder.addStringProperty( + ACTIVE, + () -> { + m_mutex.lock(); + try { + if (m_selected != null) { + return m_selected; + } else { + return m_defaultChoice; + } + } finally { + m_mutex.unlock(); + } + }, + null); + m_mutex.lock(); + try { + m_activeEntries.add(builder.getEntry(ACTIVE)); + } finally { + m_mutex.unlock(); + } + builder.addStringProperty( + SELECTED, + null, + val -> { + m_mutex.lock(); + try { + m_selected = val; + m_consumer.accept(val); + for (NetworkTableEntry entry : m_activeEntries) { + entry.setString(val); + } + } finally { + m_mutex.unlock(); + } + }); + } +} diff --git a/vendordeps/WPILibOldCommands.json b/vendordeps/WPILibOldCommands.json deleted file mode 100644 index acc8879..0000000 --- a/vendordeps/WPILibOldCommands.json +++ /dev/null @@ -1,37 +0,0 @@ -{ - "fileName": "WPILibOldCommands.json", - "name": "WPILib-Old-Commands", - "version": "2020.0.0", - "uuid": "b066afc2-5c18-43c4-b758-43381fcb275e", - "mavenUrls": [], - "jsonUrl": "", - "javaDependencies": [ - { - "groupId": "edu.wpi.first.wpilibOldCommands", - "artifactId": "wpilibOldCommands-java", - "version": "wpilib" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "edu.wpi.first.wpilibOldCommands", - "artifactId": "wpilibOldCommands-cpp", - "version": "wpilib", - "libName": "wpilibOldCommands", - "headerClassifier": "headers", - "sourcesClassifier": "sources", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "linuxraspbian", - "linuxaarch64bionic", - "windowsx86-64", - "windowsx86", - "linuxx86-64", - "osxx86-64" - ] - } - ] -} \ No newline at end of file