mirror of
https://github.com/Team4388/2022NoWayHome.git
synced 2026-06-09 00:38:05 -06:00
Add auto path preloading
- Refactor recording to use commands
This commit is contained in:
nathanrsxtn
Vendored
+9
-2
@@ -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
|
||||
}
|
||||
]
|
||||
}
|
||||
|
||||
Vendored
+10
@@ -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"
|
||||
}
|
||||
]
|
||||
}
|
||||
@@ -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,
|
||||
@@ -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
|
||||
}
|
||||
]
|
||||
@@ -1,9 +0,0 @@
|
||||
{
|
||||
"lengthUnit": "Meter",
|
||||
"exportUnit": "Same as Project",
|
||||
"maxVelocity": 1.0,
|
||||
"maxAcceleration": 1.0,
|
||||
"trackWidth": 1.0,
|
||||
"gameName": "Rapid React",
|
||||
"outputDir": ""
|
||||
}
|
||||
@@ -88,10 +88,5 @@
|
||||
"buttonCount": 0,
|
||||
"povCount": 0
|
||||
}
|
||||
],
|
||||
"robotJoysticks": [
|
||||
{
|
||||
"guid": "Keyboard0"
|
||||
}
|
||||
]
|
||||
}
|
||||
|
||||
+10
-27
@@ -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
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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();
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -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<CharSequence, String> pathExtensionRemover = ((Function<CharSequence, Matcher>) Pattern
|
||||
.compile(".path")::matcher).andThen(m -> m.replaceFirst(""));
|
||||
|
||||
private final SendableRunChooser<File> 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<Command> 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<File> 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<Waypoint> 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;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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();
|
||||
}
|
||||
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
};
|
||||
|
||||
@@ -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<Waypoint>[] 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<Point> anchorPoint;
|
||||
public Optional<Point> prevControl;
|
||||
public Optional<Point> 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<Double> x;
|
||||
public Optional<Double> y;
|
||||
}
|
||||
|
||||
public Optional<Double> holonomicAngle;
|
||||
public Optional<Boolean> isReversal;
|
||||
public Optional<Double> velOverride;
|
||||
public Optional<Boolean> isLocked;
|
||||
}
|
||||
|
||||
public Optional<Waypoint[]> waypoints;
|
||||
public Optional<Double> maxVelocity;
|
||||
public Optional<Double> maxAcceleration;
|
||||
public Optional<Boolean> 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<Translation2d> anchorPoint;
|
||||
public Optional<Translation2d> prevControl;
|
||||
public Optional<Translation2d> nextControl;
|
||||
public Optional<Double> holonomicAngle;
|
||||
public Optional<Boolean> isReversal;
|
||||
public Optional<Double> velOverride;
|
||||
public Optional<Boolean> 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
/**
|
||||
* <p>
|
||||
* NavX:
|
||||
* <p>
|
||||
* 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.
|
||||
*
|
||||
* <p>
|
||||
* Pigeon:
|
||||
* <p>
|
||||
* 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);
|
||||
}
|
||||
}
|
||||
@@ -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<Waypoint> data = new ArrayList<>();
|
||||
private final SendableChooser<Boolean> 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;
|
||||
}
|
||||
}
|
||||
@@ -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}.
|
||||
*
|
||||
* <p>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 <V> The type of the values to be stored
|
||||
*/
|
||||
public class SendableRunChooser<V> 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<String, V> m_map = new LinkedHashMap<>();
|
||||
|
||||
private String m_defaultChoice = "";
|
||||
private final int m_instance;
|
||||
private static final AtomicInteger s_instances = new AtomicInteger();
|
||||
|
||||
private Consumer<String> m_consumer;
|
||||
|
||||
/** Instantiates a {@link SendableRunChooser}. */
|
||||
public SendableRunChooser(Consumer<String> 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<NetworkTableEntry> 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();
|
||||
}
|
||||
});
|
||||
}
|
||||
}
|
||||
@@ -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"
|
||||
]
|
||||
}
|
||||
]
|
||||
}
|
||||
Reference in New Issue
Block a user