diff --git a/.gitignore b/.gitignore index d63147d2..57928290 100644 --- a/.gitignore +++ b/.gitignore @@ -175,4 +175,5 @@ simgui*.json src/main/java/frc/robot/BuildConstants.java # sim files -ctre_sim \ No newline at end of file +ctre_sim +logs \ No newline at end of file diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json new file mode 100644 index 00000000..2aaab015 --- /dev/null +++ b/.pathplanner/settings.json @@ -0,0 +1,12 @@ +{ + "robotWidth": 0.9, + "robotLength": 0.9, + "holonomicMode": true, + "pathFolders": [], + "autoFolders": [], + "defaultMaxVel": 3.0, + "defaultMaxAccel": 3.0, + "defaultMaxAngVel": 540.0, + "defaultMaxAngAccel": 720.0, + "maxModuleSpeed": 4.5 +} \ No newline at end of file diff --git a/2024Crescendo.chor b/2024Crescendo.chor new file mode 100644 index 00000000..f02e322e --- /dev/null +++ b/2024Crescendo.chor @@ -0,0 +1,750 @@ +{ + "version": "v0.2.2", + "robotConfiguration": { + "mass": 74.08797700309194, + "rotationalInertia": 6, + "motorMaxTorque": 1.162295081967213, + "motorMaxVelocity": 4800, + "gearing": 6.75, + "wheelbase": 0.5778496879611685, + "trackWidth": 0.5778496879611685, + "bumperLength": 0.8762995267982555, + "bumperWidth": 0.8762995267982555, + "wheelRadius": 0.050799972568014815 + }, + "paths": { + "NewPath": { + "waypoints": [ + { + "x": 1.373390793800354, + "y": 5.677787780761719, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "x": 1.5862760543823242, + "y": 2.3567769527435303, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 12 + }, + { + "x": 2.395240306854248, + "y": 4.7978620529174805, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 16 + }, + { + "x": 5.460788726806641, + "y": 7.480216979980469, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 11 + }, + { + "x": 7.007755279541016, + "y": 5.691980361938477, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 20 + }, + { + "x": 7.3199872970581055, + "y": 1.8174675703048706, + "heading": 0.9445353135416473, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 1.373390793800354, + "y": 5.677787780761719, + "heading": 0, + "angularVelocity": 1.7597793138716488e-8, + "velocityX": -0.24611796880717063, + "velocityY": -3.3913013287386686, + "timestamp": 0 + }, + { + "x": 1.352399870888487, + "y": 5.388550285801571, + "heading": 3.752867032965106e-9, + "angularVelocity": 4.895503936124812e-8, + "velocityX": -0.27382037475691573, + "velocityY": -3.773017489334137, + "timestamp": 0.07665946308963235 + }, + { + "x": 1.3314089479766167, + "y": 5.099312790841379, + "heading": 7.505725037785982e-9, + "angularVelocity": 4.8954921591779524e-8, + "velocityX": -0.27382037475695803, + "velocityY": -3.7730174893347197, + "timestamp": 0.1533189261792647 + }, + { + "x": 1.3104180250647461, + "y": 4.8100752958811865, + "heading": 1.1258583081678353e-8, + "angularVelocity": 4.895492210145562e-8, + "velocityX": -0.27382037475695814, + "velocityY": -3.77301748933472, + "timestamp": 0.22997838926889708 + }, + { + "x": 1.2894271021528758, + "y": 4.520837800920995, + "heading": 1.5011441030111597e-8, + "angularVelocity": 4.895492085621956e-8, + "velocityX": -0.2738203747569581, + "velocityY": -3.77301748933472, + "timestamp": 0.3066378523585294 + }, + { + "x": 1.2684361792410237, + "y": 4.231600305960802, + "heading": 1.876429908089443e-8, + "angularVelocity": 4.895492219133962e-8, + "velocityX": -0.2738203747567213, + "velocityY": -3.773017489334737, + "timestamp": 0.38329731544816176 + }, + { + "x": 1.2474452563299916, + "y": 3.9423628110005486, + "heading": 2.251715713543734e-8, + "angularVelocity": 4.8954922240388724e-8, + "velocityX": -0.27382037474602466, + "velocityY": -3.7730174893355133, + "timestamp": 0.4599567785377941 + }, + { + "x": 1.2264544553180678, + "y": 3.6531253071936995, + "heading": 2.6270015297030064e-8, + "angularVelocity": 4.8954923636821905e-8, + "velocityX": -0.27381878460824305, + "velocityY": -3.773017604736736, + "timestamp": 0.5366162416274265 + }, + { + "x": 1.221016329808459, + "y": 3.3631781154569036, + "heading": 3.0068452251973244e-8, + "angularVelocity": 4.954948550189996e-8, + "velocityX": -0.07093873724696509, + "velocityY": -3.782275273670803, + "timestamp": 0.6132757047170588 + }, + { + "x": 1.2644718237504313, + "y": 3.076455070529721, + "heading": 0.0000024276011016837486, + "angularVelocity": 0.00003127510359229755, + "velocityX": 0.5668640529240702, + "velocityY": -3.740217233088862, + "timestamp": 0.6899351678066912 + }, + { + "x": 1.3174690376414906, + "y": 2.8354946463151376, + "heading": 0.053981349204806524, + "angularVelocity": 0.7041390511774281, + "velocityX": 0.6913329647129592, + "velocityY": -3.1432573944960427, + "timestamp": 0.7665946308963235 + }, + { + "x": 1.3769500323372545, + "y": 2.642804109504741, + "heading": 0.127260696547334, + "angularVelocity": 0.9559073908050616, + "velocityX": 0.7759119657049679, + "velocityY": -2.5135910042194918, + "timestamp": 0.8432540939859559 + }, + { + "x": 1.4418762753700956, + "y": 2.498725528039788, + "heading": 0.2108122800980793, + "angularVelocity": 1.0899056709157284, + "velocityX": 0.8469436181274491, + "velocityY": -1.879462438922806, + "timestamp": 0.9199135570755882 + }, + { + "x": 1.5117489112481097, + "y": 2.403367219851462, + "heading": 0.3004574317472213, + "angularVelocity": 1.1693944626813566, + "velocityX": 0.9114678483505286, + "velocityY": -1.2439208982827146, + "timestamp": 0.9965730201652205 + }, + { + "x": 1.5862760543823242, + "y": 2.3567769527435303, + "heading": 0.393778947091327, + "angularVelocity": 1.2173515386481575, + "velocityX": 0.9721845174818851, + "velocityY": -0.607756240784745, + "timestamp": 1.073232483254853 + }, + { + "x": 1.6648735689075516, + "y": 2.3587409751636828, + "heading": 0.4887400714151268, + "angularVelocity": 1.2445985149202732, + "velocityX": 1.0301304934108237, + "velocityY": 0.025741264173079863, + "timestamp": 1.1495310833137506 + }, + { + "x": 1.747708145966923, + "y": 2.4090584282818597, + "heading": 0.5850785373582581, + "angularVelocity": 1.2626505056287294, + "velocityX": 1.0856631313736873, + "velocityY": 0.6594806861375633, + "timestamp": 1.2258296833726483 + }, + { + "x": 1.8345057249855814, + "y": 2.507753992923127, + "heading": 0.6817510390114973, + "angularVelocity": 1.2670285113830946, + "velocityX": 1.1376038217169926, + "velocityY": 1.2935435848767918, + "timestamp": 1.302128283431546 + }, + { + "x": 1.9248152988345435, + "y": 2.6548610401322725, + "heading": 0.7770428443944881, + "angularVelocity": 1.248932553276624, + "velocityX": 1.1836334320583706, + "velocityY": 1.9280438578897694, + "timestamp": 1.3784268834904436 + }, + { + "x": 2.017756729148212, + "y": 2.8504188772247816, + "heading": 0.8676164076432323, + "angularVelocity": 1.187093382825206, + "velocityX": 1.2181275965997216, + "velocityY": 2.5630593083169373, + "timestamp": 1.4547254835493413 + }, + { + "x": 2.110868096637194, + "y": 3.094357922878678, + "heading": 0.9442027964608976, + "angularVelocity": 1.003771874694233, + "velocityX": 1.2203548612570354, + "velocityY": 3.197162798080057, + "timestamp": 1.531024083608239 + }, + { + "x": 2.1793890973429906, + "y": 3.3747396477963205, + "heading": 0.9442028150467319, + "angularVelocity": 2.435933862401011e-7, + "velocityX": 0.898063668965131, + "velocityY": 3.6747951430459493, + "timestamp": 1.6073226836671366 + }, + { + "x": 2.2112713834031283, + "y": 3.6616064511690984, + "heading": 0.9442028192329045, + "angularVelocity": 5.486565302861873e-8, + "velocityX": 0.41786200579730515, + "velocityY": 3.7597911777062087, + "timestamp": 1.6836212837260343 + }, + { + "x": 2.243152894403425, + "y": 3.9484733406807293, + "heading": 0.9442028234190755, + "angularVelocity": 5.486563331370058e-8, + "velocityX": 0.41785184755271976, + "velocityY": 3.7597923066765064, + "timestamp": 1.759919883784932 + }, + { + "x": 2.275034438637313, + "y": 4.235340226498878, + "heading": 0.9442028276052465, + "angularVelocity": 5.4865633459900915e-8, + "velocityX": 0.41785228312547945, + "velocityY": 3.7597922582682526, + "timestamp": 1.8362184838438296 + }, + { + "x": 2.3114043136255, + "y": 4.521672682225187, + "heading": 0.9442028318036487, + "angularVelocity": 5.5025940885755316e-8, + "velocityX": 0.4766781429818065, + "velocityY": 3.752787803515112, + "timestamp": 1.9125170839027272 + }, + { + "x": 2.395240306854248, + "y": 4.7978620529174805, + "heading": 0.944202836290881, + "angularVelocity": 5.881146304501388e-8, + "velocityX": 1.098788092625971, + "velocityY": 3.6198484700779985, + "timestamp": 1.988815683961625 + }, + { + "x": 2.5138560537660655, + "y": 5.041648695718218, + "heading": 0.9442028403664345, + "angularVelocity": 5.686795868350027e-8, + "velocityX": 1.6550967466461282, + "velocityY": 3.401660318130064, + "timestamp": 2.0604826406831447 + }, + { + "x": 2.669379769810612, + "y": 5.263716246351053, + "heading": 0.9442028441855257, + "angularVelocity": 5.328942931675909e-8, + "velocityX": 2.170089580458573, + "velocityY": 3.098604444663902, + "timestamp": 2.1321495974046645 + }, + { + "x": 2.829793194306078, + "y": 5.482277808796993, + "heading": 0.9442028479893113, + "angularVelocity": 5.307586429308322e-8, + "velocityX": 2.2383177943328927, + "velocityY": 3.049683877260457, + "timestamp": 2.2038165541261843 + }, + { + "x": 2.9902066606790894, + "y": 5.70083934050687, + "heading": 0.9442028517930969, + "angularVelocity": 5.307586336486897e-8, + "velocityX": 2.2383183786683687, + "velocityY": 3.049683448386907, + "timestamp": 2.275483510847704 + }, + { + "x": 3.1506201270524525, + "y": 5.919400872216489, + "heading": 0.9442028555968824, + "angularVelocity": 5.307586343565819e-8, + "velocityX": 2.2383183786732843, + "velocityY": 3.0496834483832997, + "timestamp": 2.347150467569224 + }, + { + "x": 3.3110335934258157, + "y": 6.137962403926107, + "heading": 0.9442028594006682, + "angularVelocity": 5.3075864810896055e-8, + "velocityX": 2.2383183786732843, + "velocityY": 3.0496834483832993, + "timestamp": 2.4188174242907436 + }, + { + "x": 3.4714470597991793, + "y": 6.3565239356357255, + "heading": 0.9442028632044539, + "angularVelocity": 5.307586438143788e-8, + "velocityX": 2.2383183786732848, + "velocityY": 3.0496834483832993, + "timestamp": 2.4904843810122634 + }, + { + "x": 3.631860526174069, + "y": 6.575085467344224, + "heading": 0.9442028670082396, + "angularVelocity": 5.307586459415007e-8, + "velocityX": 2.2383183786945806, + "velocityY": 3.0496834483676687, + "timestamp": 2.5621513377337832 + }, + { + "x": 3.7922741739851236, + "y": 6.793646865887341, + "heading": 0.9442028708120256, + "angularVelocity": 5.307586821274769e-8, + "velocityX": 2.2383209103517525, + "velocityY": 3.0496815902535848, + "timestamp": 2.633818294455303 + }, + { + "x": 3.9681792086390733, + "y": 6.999945295226872, + "heading": 0.9442028746807025, + "angularVelocity": 5.3981320792667595e-8, + "velocityX": 2.4544789216804146, + "velocityY": 2.878571084595562, + "timestamp": 2.705485251176823 + }, + { + "x": 4.17437520362896, + "y": 7.175970389393745, + "heading": 0.9442028789510033, + "angularVelocity": 5.958535155683375e-8, + "velocityX": 2.8771417738737903, + "velocityY": 2.4561541639177027, + "timestamp": 2.7771522078983426 + }, + { + "x": 4.405717344151184, + "y": 7.3173296257195375, + "heading": 0.9442028842473172, + "angularVelocity": 7.390175547435562e-8, + "velocityX": 3.228016803073774, + "velocityY": 1.9724464773225439, + "timestamp": 2.8488191646198624 + }, + { + "x": 4.656433083655096, + "y": 7.420495670437542, + "heading": 0.9442028919014063, + "angularVelocity": 1.0680080940487753e-7, + "velocityX": 3.498344997097253, + "velocityY": 1.4395203792296967, + "timestamp": 2.920486121341382 + }, + { + "x": 4.920266408469536, + "y": 7.482894237840949, + "heading": 0.9442029059804885, + "angularVelocity": 1.964515145326873e-7, + "velocityX": 3.6813803304029413, + "velocityY": 0.8706741608391041, + "timestamp": 2.992153078062902 + }, + { + "x": 5.190633947862971, + "y": 7.502968306176006, + "heading": 0.9442029492980424, + "angularVelocity": 6.044285388451107e-7, + "velocityX": 3.772555048542354, + "velocityY": 0.2801021454427761, + "timestamp": 3.0638200347844218 + }, + { + "x": 5.460788726806641, + "y": 7.480216979980469, + "heading": 0.9442042125234751, + "angularVelocity": 0.00001762633003205625, + "velocityX": 3.7695863100958116, + "velocityY": -0.31745908067434947, + "timestamp": 3.1354869915059416 + }, + { + "x": 5.6861880959283475, + "y": 7.430105467011311, + "heading": 0.94453500375596, + "angularVelocity": 0.00541646085327845, + "velocityX": 3.6907473331858487, + "velocityY": -0.8205388221514872, + "timestamp": 3.196558462612879 + }, + { + "x": 5.902931349588688, + "y": 7.350124119825956, + "heading": 0.944535163065715, + "angularVelocity": 0.0000026085789673009567, + "velocityX": 3.5490098687948497, + "velocityY": -1.3096351821180778, + "timestamp": 3.2576299337198167 + }, + { + "x": 6.106969137969694, + "y": 7.241757655990003, + "heading": 0.9445351882967145, + "angularVelocity": 4.1313888378756724e-7, + "velocityX": 3.340967307365667, + "velocityY": -1.7744203942000976, + "timestamp": 3.3187014048267542 + }, + { + "x": 6.294604237914379, + "y": 7.106969701935095, + "heading": 0.9445352004387211, + "angularVelocity": 1.988163445738462e-7, + "velocityX": 3.0723854615542603, + "velocityY": -2.207052681257527, + "timestamp": 3.379772875933692 + }, + { + "x": 6.462436717197839, + "y": 6.94820261304716, + "heading": 0.9445352081247251, + "angularVelocity": 1.2585260943351558e-7, + "velocityX": 2.7481322496649945, + "velocityY": -2.599693212071642, + "timestamp": 3.4408443470406294 + }, + { + "x": 6.607425485391203, + "y": 6.7683332434118935, + "heading": 0.944535213822349, + "angularVelocity": 9.329435992491954e-8, + "velocityX": 2.3740834397043513, + "velocityY": -2.945227393005009, + "timestamp": 3.501915818147567 + }, + { + "x": 6.726943413314801, + "y": 6.570620826362678, + "heading": 0.9445352185350564, + "angularVelocity": 7.716708691487089e-8, + "velocityX": 1.9570173398037076, + "velocityY": -3.237394047754587, + "timestamp": 3.5629872892545045 + }, + { + "x": 6.818825069558712, + "y": 6.358647957429321, + "heading": 0.9445352227854475, + "angularVelocity": 6.959699896596626e-8, + "velocityX": 1.5044939081788649, + "velocityY": -3.470898360417538, + "timestamp": 3.624058760361442 + }, + { + "x": 6.885266799619373, + "y": 6.137378364635986, + "heading": 0.944535226910566, + "angularVelocity": 6.754575173659361e-8, + "velocityX": 1.0879340034945393, + "velocityY": -3.623125311749637, + "timestamp": 3.6851302314683796 + }, + { + "x": 6.951706477654193, + "y": 5.916108155680916, + "heading": 0.9445352310356804, + "angularVelocity": 6.754568510636454e-8, + "velocityX": 1.087900403094797, + "velocityY": -3.6231354009406083, + "timestamp": 3.746201702575317 + }, + { + "x": 7.007755279541016, + "y": 5.691980361938477, + "heading": 0.9445352351916124, + "angularVelocity": 6.805030176915139e-8, + "velocityX": 0.917757520343414, + "velocityY": -3.669926230366784, + "timestamp": 3.8072731736822547 + }, + { + "x": 7.033287944264436, + "y": 5.499297549749691, + "heading": 0.9445352391429122, + "angularVelocity": 7.690359033060341e-8, + "velocityX": 0.49693866281360616, + "velocityY": -3.750158476346985, + "timestamp": 3.858653085599064 + }, + { + "x": 7.048377702483028, + "y": 5.30551704932481, + "heading": 0.9445352430586353, + "angularVelocity": 7.621117031703296e-8, + "velocityX": 0.29368984211229937, + "velocityY": -3.7715226281165486, + "timestamp": 3.9100329975158736 + }, + { + "x": 7.063467124411225, + "y": 5.111736522713165, + "heading": 0.9445352469743582, + "angularVelocity": 7.621116311616718e-8, + "velocityX": 0.2936832969396488, + "velocityY": -3.7715231377858505, + "timestamp": 3.961412909432683 + }, + { + "x": 7.0785565463316305, + "y": 4.917955996100912, + "heading": 0.9445352508900811, + "angularVelocity": 7.621116383171076e-8, + "velocityX": 0.29368329678799265, + "velocityY": -3.7715231377976592, + "timestamp": 4.0127928213494926 + }, + { + "x": 7.093645968252035, + "y": 4.72417546948866, + "heading": 0.9445352548058039, + "angularVelocity": 7.621116339702062e-8, + "velocityX": 0.29368329678798916, + "velocityY": -3.77152313779766, + "timestamp": 4.064172733266302 + }, + { + "x": 7.1087353901724395, + "y": 4.530394942876407, + "heading": 0.9445352587215268, + "angularVelocity": 7.621116320689e-8, + "velocityX": 0.2936832967879892, + "velocityY": -3.77152313779766, + "timestamp": 4.1155526451831115 + }, + { + "x": 7.1238248120928445, + "y": 4.336614416264154, + "heading": 0.9445352626372497, + "angularVelocity": 7.621116316535163e-8, + "velocityX": 0.29368329678798916, + "velocityY": -3.77152313779766, + "timestamp": 4.166932557099921 + }, + { + "x": 7.138914234013249, + "y": 4.142833889651901, + "heading": 0.9445352665529726, + "angularVelocity": 7.621116395669404e-8, + "velocityX": 0.2936832967879891, + "velocityY": -3.7715231377976606, + "timestamp": 4.21831246901673 + }, + { + "x": 7.1540036559336535, + "y": 3.9490533630396487, + "heading": 0.9445352704686956, + "angularVelocity": 7.621116465419702e-8, + "velocityX": 0.29368329678798905, + "velocityY": -3.7715231377976597, + "timestamp": 4.26969238093354 + }, + { + "x": 7.1690930778540585, + "y": 3.755272836427396, + "heading": 0.9445352743844184, + "angularVelocity": 7.621116300190572e-8, + "velocityX": 0.29368329678798905, + "velocityY": -3.7715231377976597, + "timestamp": 4.321072292850349 + }, + { + "x": 7.184182499774463, + "y": 3.561492309815143, + "heading": 0.9445352783001413, + "angularVelocity": 7.621116375934217e-8, + "velocityX": 0.29368329678798927, + "velocityY": -3.7715231377976597, + "timestamp": 4.372452204767159 + }, + { + "x": 7.199271921694868, + "y": 3.367711783202891, + "heading": 0.9445352822158641, + "angularVelocity": 7.621116358329164e-8, + "velocityX": 0.2936832967879893, + "velocityY": -3.7715231377976597, + "timestamp": 4.423832116683968 + }, + { + "x": 7.2143613436152725, + "y": 3.173931256590638, + "heading": 0.944535286131587, + "angularVelocity": 7.621116395825444e-8, + "velocityX": 0.2936832967879889, + "velocityY": -3.77152313779766, + "timestamp": 4.475212028600778 + }, + { + "x": 7.2294507655356774, + "y": 2.980150729978386, + "heading": 0.94453529004731, + "angularVelocity": 7.621116318952045e-8, + "velocityX": 0.29368329678798905, + "velocityY": -3.77152313779766, + "timestamp": 4.526591940517587 + }, + { + "x": 7.244540187456082, + "y": 2.786370203366133, + "heading": 0.9445352939630328, + "angularVelocity": 7.621116338095618e-8, + "velocityX": 0.29368329678798905, + "velocityY": -3.77152313779766, + "timestamp": 4.577971852434397 + }, + { + "x": 7.2596296093764865, + "y": 2.5925896767538807, + "heading": 0.9445352978787557, + "angularVelocity": 7.621116372521823e-8, + "velocityX": 0.2936832967879889, + "velocityY": -3.77152313779766, + "timestamp": 4.629351764351206 + }, + { + "x": 7.2747190312968915, + "y": 2.398809150141628, + "heading": 0.9445353017944785, + "angularVelocity": 7.621116368417063e-8, + "velocityX": 0.2936832967879889, + "velocityY": -3.77152313779766, + "timestamp": 4.680731676268016 + }, + { + "x": 7.289808453217296, + "y": 2.2050286235293757, + "heading": 0.9445353057102015, + "angularVelocity": 7.621116284744578e-8, + "velocityX": 0.29368329678798905, + "velocityY": -3.77152313779766, + "timestamp": 4.732111588184825 + }, + { + "x": 7.3048978751377005, + "y": 2.011248096917123, + "heading": 0.9445353096259244, + "angularVelocity": 7.621116384489279e-8, + "velocityX": 0.29368329678798877, + "velocityY": -3.77152313779766, + "timestamp": 4.7834915001016345 + }, + { + "x": 7.3199872970581055, + "y": 1.8174675703048704, + "heading": 0.9445353135416472, + "angularVelocity": 7.621116322392135e-8, + "velocityX": 0.2936832967879892, + "velocityY": -3.77152313779766, + "timestamp": 4.834871412018444 + } + ], + "constraints": [], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + } + }, + "splitTrajectoriesAtStopPoints": false, + "usesObstacles": true +} \ No newline at end of file diff --git a/Crescendo2024.chor b/Crescendo2024.chor new file mode 100644 index 00000000..a180f910 --- /dev/null +++ b/Crescendo2024.chor @@ -0,0 +1,2246 @@ +{ + "version": "v0.2.2", + "robotConfiguration": { + "mass": 74.08797700309194, + "rotationalInertia": 6, + "motorMaxTorque": 0.7759834368530021, + "motorMaxVelocity": 5800, + "gearing": 5.731, + "wheelbase": 0.4724397448825378, + "trackWidth": 0.4724397448825378, + "bumperLength": 0.6095996708161777, + "bumperWidth": 0.5587996982481629, + "wheelRadius": 0.050799972568014815 + }, + "paths": { + "Test1": { + "waypoints": [ + { + "x": 1.4869295358657837, + "y": 5.564249038696289, + "heading": 3.141, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "x": 2.6648950576782227, + "y": 6.813176155090332, + "heading": 3.8960730372009595, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 28 + }, + { + "x": 2.1113932132720947, + "y": 6.231289386749268, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 25 + }, + { + "x": 2.59393310546875, + "y": 5.550056457519531, + "heading": 3.14, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 25 + }, + { + "x": 1.941084861755371, + "y": 4.939785003662109, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 23 + }, + { + "x": 2.5655484199523926, + "y": 4.244359970092773, + "heading": 2.649783348569472, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 1.4869295358657837, + "y": 5.564249038696289, + "heading": 3.141, + "angularVelocity": 8.029589971503427e-22, + "velocityX": 1.7351458241819956e-21, + "velocityY": 6.201477276822387e-22, + "timestamp": 0 + }, + { + "x": 1.506153170031305, + "y": 5.579314508687984, + "heading": 3.1569331073832556, + "angularVelocity": 0.2189381700102546, + "velocityX": 0.2641535755648618, + "velocityY": 0.20701589156378014, + "timestamp": 0.07277446131256805 + }, + { + "x": 1.5445753142190985, + "y": 5.609481391551554, + "heading": 3.1887853931444132, + "angularVelocity": 0.4376849403852184, + "velocityX": 0.5279619181620503, + "velocityY": 0.41452567727023953, + "timestamp": 0.1455489226251361 + }, + { + "x": 1.6021601427736685, + "y": 5.654796415146542, + "heading": 3.2365776272564357, + "angularVelocity": 0.6567171127073494, + "velocityX": 0.7912779774108067, + "velocityY": 0.6226775544288833, + "timestamp": 0.21832338393770417 + }, + { + "x": 1.6788531458253628, + "y": 5.715327275578862, + "heading": 3.3003664786278266, + "angularVelocity": 0.8765279772723583, + "velocityX": 1.0538450119513225, + "velocityY": 0.8317596494783837, + "timestamp": 0.2910978452502722 + }, + { + "x": 1.7745670450726352, + "y": 5.791182757948707, + "heading": 3.380219753358853, + "angularVelocity": 1.097270571169936, + "velocityX": 1.3152127480020601, + "velocityY": 1.0423365697486087, + "timestamp": 0.36387230656284025 + }, + { + "x": 1.8891522077377947, + "y": 5.8825573533612445, + "heading": 3.4761247222373646, + "angularVelocity": 1.317838251891654, + "velocityX": 1.5745243674565124, + "velocityY": 1.255586008669736, + "timestamp": 0.4366467678754083 + }, + { + "x": 2.0223089309237166, + "y": 5.989857408352276, + "heading": 3.5877133538349852, + "angularVelocity": 1.5333487817703626, + "velocityX": 1.829717744168665, + "velocityY": 1.4744190895508729, + "timestamp": 0.5094212291879764 + }, + { + "x": 2.1730917573654285, + "y": 6.114310741146903, + "heading": 3.713064984762661, + "angularVelocity": 1.7224673143135665, + "velocityX": 2.0719195130019052, + "velocityY": 1.7101237240368758, + "timestamp": 0.5821956905005444 + }, + { + "x": 2.304244374220996, + "y": 6.249503988496409, + "heading": 3.8044927424626787, + "angularVelocity": 1.2563165161379026, + "velocityX": 1.8021791503514413, + "velocityY": 1.8577017941616667, + "timestamp": 0.6549701518131125 + }, + { + "x": 2.414916720860036, + "y": 6.372175349075486, + "heading": 3.8745670686179303, + "angularVelocity": 0.9628972154706921, + "velocityX": 1.5207580330096784, + "velocityY": 1.6856374937933305, + "timestamp": 0.7277446131256805 + }, + { + "x": 2.505647288657887, + "y": 6.481209512574969, + "heading": 3.9250064522902046, + "angularVelocity": 0.6930918176863726, + "velocityX": 1.2467363709936818, + "velocityY": 1.4982476205653745, + "timestamp": 0.8005190744382485 + }, + { + "x": 2.576640808380743, + "y": 6.576219510667492, + "heading": 3.9564510372491593, + "angularVelocity": 0.432082689336569, + "velocityX": 0.975527931672858, + "velocityY": 1.305540382971075, + "timestamp": 0.8732935357508166 + }, + { + "x": 2.628005259573955, + "y": 6.657012046349283, + "heading": 3.969203722939461, + "angularVelocity": 0.17523572775796842, + "velocityX": 0.7058032483758355, + "velocityY": 1.1101770349736413, + "timestamp": 0.9460679970633846 + }, + { + "x": 2.6598078437532253, + "y": 6.723475709345355, + "heading": 3.963391945850161, + "angularVelocity": -0.07986011829532402, + "velocityX": 0.4370019867639682, + "velocityY": 0.9132827890076579, + "timestamp": 1.0188424583759528 + }, + { + "x": 2.6720941279928496, + "y": 6.775543377099927, + "heading": 3.9390315825966313, + "angularVelocity": -0.3347378024400806, + "velocityX": 0.16882686615644266, + "velocityY": 0.7154662063514367, + "timestamp": 1.0916169196885208 + }, + { + "x": 2.6648950576782227, + "y": 6.813176155090332, + "heading": 3.896073037200959, + "angularVelocity": -0.5902969890929726, + "velocityX": -0.09892303130498589, + "velocityY": 0.5171151707845827, + "timestamp": 1.1643913810010889 + }, + { + "x": 2.659637231596543, + "y": 6.825031823463049, + "heading": 3.877816032306183, + "angularVelocity": -0.6840416135896586, + "velocityX": -0.19699681615987222, + "velocityY": 0.44420049020077496, + "timestamp": 1.191081284849143 + }, + { + "x": 2.6517927610129908, + "y": 6.834893701518374, + "heading": 3.857106562184998, + "angularVelocity": -0.775928989444263, + "velocityX": -0.29391153404714393, + "velocityY": 0.36949844823228756, + "timestamp": 1.2177711886971971 + }, + { + "x": 2.641397527843252, + "y": 6.842708311525597, + "heading": 3.834005474054525, + "angularVelocity": -0.865536581247652, + "velocityX": -0.38948185159896215, + "velocityY": 0.2927927373478789, + "timestamp": 1.2444610925452513 + }, + { + "x": 2.628493660297315, + "y": 6.848415370045661, + "heading": 3.808587288042731, + "angularVelocity": -0.9523521012476956, + "velocityX": -0.4834737367133058, + "velocityY": 0.2138283656829084, + "timestamp": 1.2711509963933054 + }, + { + "x": 2.6131312860336138, + "y": 6.85194659507741, + "heading": 3.7809426101756807, + "angularVelocity": -1.0357728534516963, + "velocityX": -0.5755874712460319, + "velocityY": 0.13230564830254915, + "timestamp": 1.2978409002413596 + }, + { + "x": 2.5953708985643273, + "y": 6.853224371337149, + "heading": 3.7511803744930092, + "angularVelocity": -1.1151121357389708, + "velocityX": -0.6654346741148351, + "velocityY": 0.0478748918322195, + "timestamp": 1.3245308040894137 + }, + { + "x": 2.5752865454465885, + "y": 6.852160328589509, + "heading": 3.7194297145938124, + "angularVelocity": -1.1896131241218921, + "velocityX": -0.752507511157751, + "velocityY": -0.03986686327897337, + "timestamp": 1.3512207079374678 + }, + { + "x": 2.5529701073140734, + "y": 6.848653945190506, + "heading": 3.6858413880737997, + "angularVelocity": -1.2584656247257915, + "velocityX": -0.8361378242335784, + "velocityY": -0.1313748981249787, + "timestamp": 1.377910611785522 + }, + { + "x": 2.5285370169793717, + "y": 6.842591376045072, + "heading": 3.650589102058915, + "angularVelocity": -1.3208097794422922, + "velocityX": -0.9154431755842751, + "velocityY": -0.2271484071260898, + "timestamp": 1.4046005156335761 + }, + { + "x": 2.502133887928051, + "y": 6.833844846711091, + "heading": 3.613872024847483, + "angularVelocity": -1.375691625584805, + "velocityX": -0.9892553079859069, + "velocityY": -0.32770928601974636, + "timestamp": 1.4312904194816303 + }, + { + "x": 2.4739487064187284, + "y": 6.822273243464229, + "heading": 3.5759212160148097, + "angularVelocity": -1.4219162814796094, + "velocityX": -1.0560240932219476, + "velocityY": -0.43355732237695227, + "timestamp": 1.4579803233296844 + }, + { + "x": 2.4442244144939402, + "y": 6.807725217915028, + "heading": 3.5370136666138436, + "angularVelocity": -1.4577628163243674, + "velocityX": -1.113690483637882, + "velocityY": -0.54507598199017, + "timestamp": 1.4846702271777386 + }, + { + "x": 2.4132762941421024, + "y": 6.79004764808573, + "heading": 3.497494895227568, + "angularVelocity": -1.4806636850869153, + "velocityX": -1.1595440930782523, + "velocityY": -0.6623317165148433, + "timestamp": 1.5113601310257927 + }, + { + "x": 2.3815108616317744, + "y": 6.769104478274178, + "heading": 3.45780088591702, + "angularVelocity": -1.4872293859328474, + "velocityX": -1.1901666147307672, + "velocityY": -0.7846850978100616, + "timestamp": 1.5380500348738468 + }, + { + "x": 2.3494374003667597, + "y": 6.744810785756249, + "heading": 3.4184626098399313, + "angularVelocity": -1.4739010039542162, + "velocityX": -1.2017076362510986, + "velocityY": -0.910220308631834, + "timestamp": 1.564739938721901 + }, + { + "x": 2.317656105280672, + "y": 6.717179467626764, + "heading": 3.3800987305060795, + "angularVelocity": -1.4373929390026206, + "velocityX": -1.190760943427104, + "velocityY": -1.0352722994728694, + "timestamp": 1.5914298425699551 + }, + { + "x": 2.2868057964945567, + "y": 6.686361396326711, + "heading": 3.343389515038899, + "angularVelocity": -1.3753970668521929, + "velocityX": -1.1558793527974576, + "velocityY": -1.1546714995865741, + "timestamp": 1.6181197464180093 + }, + { + "x": 2.25748075399746, + "y": 6.652639268574606, + "heading": 3.308925014353095, + "angularVelocity": -1.291293549876024, + "velocityX": -1.0987316651286732, + "velocityY": -1.263478802474685, + "timestamp": 1.6448096502660634 + }, + { + "x": 2.2301787041350396, + "y": 6.616374519308364, + "heading": 3.2770382136187797, + "angularVelocity": -1.194713960599006, + "velocityX": -1.022935489683713, + "velocityY": -1.3587440956212415, + "timestamp": 1.6714995541141175 + }, + { + "x": 2.2052911587380817, + "y": 6.577965167741144, + "heading": 3.2479010535236887, + "angularVelocity": -1.0916922091952401, + "velocityX": -0.9324704029899484, + "velocityY": -1.4390966631383846, + "timestamp": 1.6981894579621717 + }, + { + "x": 2.183095873519221, + "y": 6.537812097974725, + "heading": 3.2217255041499464, + "angularVelocity": -0.9807285002883435, + "velocityX": -0.8315985454731583, + "velocityY": -1.504429165238335, + "timestamp": 1.7248793618102258 + }, + { + "x": 2.163764936777705, + "y": 6.4962772278935255, + "heading": 3.198795874429834, + "angularVelocity": -0.8591124887766933, + "velocityX": -0.72427899521733, + "velocityY": -1.5562015628702863, + "timestamp": 1.75156926565828 + }, + { + "x": 2.1473967460074728, + "y": 6.453660043976164, + "heading": 3.1793849811459, + "angularVelocity": -0.7272747550699427, + "velocityX": -0.61327275150245, + "velocityY": -1.596752995439064, + "timestamp": 1.7782591695063341 + }, + { + "x": 2.134047988560311, + "y": 6.410199882298709, + "heading": 3.163697789085614, + "angularVelocity": -0.5877575336948715, + "velocityX": -0.5001425828716374, + "velocityY": -1.628337139199698, + "timestamp": 1.8049490733543883 + }, + { + "x": 2.1237534679344243, + "y": 6.366088523528368, + "heading": 3.151864532890396, + "angularVelocity": -0.443360765276075, + "velocityX": -0.38570841935190137, + "velocityY": -1.6527357693556206, + "timestamp": 1.8316389772024424 + }, + { + "x": 2.116536197487218, + "y": 6.321482513089734, + "heading": 3.143955647543306, + "angularVelocity": -0.29632498461271933, + "velocityX": -0.27041200628876155, + "velocityY": -1.671269057115243, + "timestamp": 1.8583288810504965 + }, + { + "x": 2.1124122616124033, + "y": 6.276512434450225, + "heading": 3.14, + "angularVelocity": -0.1482076355848078, + "velocityX": -0.15451295359818015, + "velocityY": -1.6849097282449244, + "timestamp": 1.8850187848985507 + }, + { + "x": 2.1113932132720947, + "y": 6.231289386749268, + "heading": 3.14, + "angularVelocity": 1.1480104978612554e-22, + "velocityX": -0.038181042019109025, + "velocityY": -1.6943878089038094, + "timestamp": 1.9117086887466048 + }, + { + "x": 2.1139722457961088, + "y": 6.184041017313733, + "heading": 3.14, + "angularVelocity": 1.326030822415817e-23, + "velocityX": 0.09287464315336291, + "velocityY": -1.7014812376514836, + "timestamp": 1.9394776516890055 + }, + { + "x": 2.1201947185383916, + "y": 6.136700429098736, + "heading": 3.14, + "angularVelocity": 1.3260179507784046e-23, + "velocityX": 0.224080126981686, + "velocityY": -1.7048021675563763, + "timestamp": 1.9672466146314063 + }, + { + "x": 2.13006161842343, + "y": 6.089396035622716, + "heading": 3.14, + "angularVelocity": 1.326007041519161e-23, + "velocityX": 0.35532115136976367, + "velocityY": -1.7034987433322955, + "timestamp": 1.995015577573807 + }, + { + "x": 2.1435678181376194, + "y": 6.042288248235824, + "heading": 3.14, + "angularVelocity": 1.3260437690465338e-23, + "velocityX": 0.4863775338749469, + "velocityY": -1.6964186773774967, + "timestamp": 2.022784540516208 + }, + { + "x": 2.1606964661173613, + "y": 5.995581737791835, + "heading": 3.14, + "angularVelocity": 1.325999769616951e-23, + "velocityX": 0.6168270675167464, + "velocityY": -1.6819681217793025, + "timestamp": 2.050553503458609 + }, + { + "x": 2.181407907708738, + "y": 5.949543614413858, + "heading": 3.14, + "angularVelocity": 1.3260185229673763e-23, + "velocityX": 0.7458485804578618, + "velocityY": -1.6578985493073877, + "timestamp": 2.07832246640101 + }, + { + "x": 2.2056170475643233, + "y": 5.904529822011414, + "heading": 3.14, + "angularVelocity": 1.3260157963898354e-23, + "velocityX": 0.871805688451557, + "velocityY": -1.6210109284892367, + "timestamp": 2.1060914293434108 + }, + { + "x": 2.2331463169007804, + "y": 5.861019319967013, + "heading": 3.14, + "angularVelocity": 1.326020172336188e-23, + "velocityX": 0.9913682910506778, + "velocityY": -1.566875296519094, + "timestamp": 2.1338603922858117 + }, + { + "x": 2.2636325105277852, + "y": 5.819639483049468, + "heading": 3.14, + "angularVelocity": 1.32601511090322e-23, + "velocityX": 1.0978513562152263, + "velocityY": -1.4901470034504147, + "timestamp": 2.1616293552282126 + }, + { + "x": 2.2963834267173127, + "y": 5.781115195207252, + "heading": 3.14, + "angularVelocity": 1.3260107576996925e-23, + "velocityX": 1.1794072489296905, + "velocityY": -1.387314604514559, + "timestamp": 2.1893983181706136 + }, + { + "x": 2.3303148904385163, + "y": 5.746039013141612, + "heading": 3.14, + "angularVelocity": 1.3260240855176863e-23, + "velocityX": 1.221920450957617, + "velocityY": -1.2631433928013844, + "timestamp": 2.2171672811130145 + }, + { + "x": 2.3642062447518297, + "y": 5.714607213055508, + "heading": 3.14, + "angularVelocity": 1.326023555417506e-23, + "velocityX": 1.2204760539171806, + "velocityY": -1.1319039948052778, + "timestamp": 2.2449362440554155 + }, + { + "x": 2.397064742551735, + "y": 5.68667060145179, + "heading": 3.14, + "angularVelocity": 1.3260077501265654e-23, + "velocityX": 1.1832814163085987, + "velocityY": -1.0060372676381828, + "timestamp": 2.2727052069978164 + }, + { + "x": 2.4282178000518853, + "y": 5.661954954768778, + "heading": 3.14, + "angularVelocity": 1.3260249833501151e-23, + "velocityX": 1.1218660763374204, + "velocityY": -0.8900457224231765, + "timestamp": 2.3004741699402174 + }, + { + "x": 2.457231034537111, + "y": 5.640189613922927, + "heading": 3.14, + "angularVelocity": 1.3260141947431602e-23, + "velocityX": 1.0448079946451803, + "velocityY": -0.7838009972139435, + "timestamp": 2.3282431328826183 + }, + { + "x": 2.483818861573163, + "y": 5.621144668272307, + "heading": 3.14, + "angularVelocity": 1.3260165607649488e-23, + "velocityX": 0.9574656097601072, + "velocityY": -0.6858356824532662, + "timestamp": 2.3560120958250192 + }, + { + "x": 2.507786212179532, + "y": 5.604632755958765, + "heading": 3.14, + "angularVelocity": 1.3260126001429812e-23, + "velocityX": 0.8630985123961222, + "velocityY": -0.5946175356923091, + "timestamp": 2.38378105876742 + }, + { + "x": 2.5289943781295285, + "y": 5.590502031840209, + "heading": 3.14, + "angularVelocity": 1.3260018717887446e-23, + "velocityX": 0.763736333761809, + "velocityY": -0.5088675492802951, + "timestamp": 2.411550021709821 + }, + { + "x": 2.547341071809568, + "y": 5.578628548447832, + "heading": 3.14, + "angularVelocity": 1.3260377381509874e-23, + "velocityX": 0.6606906321310845, + "velocityY": -0.4275810881740915, + "timestamp": 2.439318984652222 + }, + { + "x": 2.562748457639991, + "y": 5.568909973472095, + "heading": 3.14, + "angularVelocity": 1.3260175762620676e-23, + "velocityX": 0.5548419601546171, + "velocityY": -0.3499797596293578, + "timestamp": 2.467087947594623 + }, + { + "x": 2.575155705150767, + "y": 5.561260778200535, + "heading": 3.14, + "angularVelocity": 1.3260285574981282e-23, + "velocityX": 0.4468026961075851, + "velocityY": -0.2754584421256751, + "timestamp": 2.494856910537024 + }, + { + "x": 2.5845141850851863, + "y": 5.555608625671519, + "heading": 3.14, + "angularVelocity": 1.326006595026007e-23, + "velocityX": 0.33701222310069334, + "velocityY": -0.2035420818825477, + "timestamp": 2.522625873479425 + }, + { + "x": 2.5907842648688533, + "y": 5.55189166019714, + "heading": 3.14, + "angularVelocity": 1.3260302513246253e-23, + "velocityX": 0.22579452450827125, + "velocityY": -0.1338532332694335, + "timestamp": 2.550394836421826 + }, + { + "x": 2.59393310546875, + "y": 5.550056457519531, + "heading": 3.14, + "angularVelocity": 1.3260089047889746e-23, + "velocityX": 0.11339424545409957, + "velocityY": -0.06608826845335893, + "timestamp": 2.578163799364227 + }, + { + "x": 2.59393310546875, + "y": 5.550056457519531, + "heading": 3.14, + "angularVelocity": 4.554749859713047e-24, + "velocityX": -4.596278509340535e-25, + "velocityY": -1.5195442763313795e-23, + "timestamp": 2.6059327623066277 + }, + { + "x": 2.5899783566862378, + "y": 5.5489813066194555, + "heading": 3.1367490851985087, + "angularVelocity": -0.1085265960704612, + "velocityX": -0.13202296888338985, + "velocityY": -0.03589219483504766, + "timestamp": 2.6358877679845065 + }, + { + "x": 2.5820812048170954, + "y": 5.546779098674632, + "heading": 3.130275233897137, + "angularVelocity": -0.21611918124764368, + "velocityX": -0.26363379643671836, + "velocityY": -0.07351719337009978, + "timestamp": 2.6658427736623853 + }, + { + "x": 2.5702568313825034, + "y": 5.5433888083370855, + "heading": 3.1206149262957497, + "angularVelocity": -0.32249393324338926, + "velocityX": -0.3947378131636905, + "velocityY": -0.1131794256360211, + "timestamp": 2.695797779340264 + }, + { + "x": 2.554524436678559, + "y": 5.5387376635070815, + "heading": 3.107814789143658, + "angularVelocity": -0.42731212571741595, + "velocityX": -0.5252008586852795, + "velocityY": -0.15527103817038457, + "timestamp": 2.725752785018143 + }, + { + "x": 2.5349090755324424, + "y": 5.532737501107672, + "heading": 3.0919335422287366, + "angularVelocity": -0.5301700519005096, + "velocityX": -0.6548274888361012, + "velocityY": -0.20030583415450579, + "timestamp": 2.7557077906960217 + }, + { + "x": 2.511444606178007, + "y": 5.52527960958652, + "heading": 3.0730442108839475, + "angularVelocity": -0.6305901440285391, + "velocityX": -0.7833238159511905, + "velocityY": -0.24896979160511046, + "timestamp": 2.7856627963739005 + }, + { + "x": 2.4841786133457258, + "y": 5.5162272720643095, + "heading": 3.0512365155339536, + "angularVelocity": -0.7280150631418102, + "velocityX": -0.9102316028741883, + "velocityY": -0.3021978236143635, + "timestamp": 2.8156178020517793 + }, + { + "x": 2.4531809940870857, + "y": 5.505404760057258, + "heading": 3.026619340133933, + "angularVelocity": -0.8218050653951327, + "velocityX": -1.03480598842053, + "velocityY": -0.3612922702613114, + "timestamp": 2.845572807729658 + }, + { + "x": 2.4185596883958564, + "y": 5.492580836854246, + "heading": 2.999323661656212, + "angularVelocity": -0.9112226107130541, + "velocityX": -1.1557769697502154, + "velocityY": -0.4281061850200961, + "timestamp": 2.875527813407537 + }, + { + "x": 2.380491125275952, + "y": 5.477444069032361, + "heading": 2.9695096535475827, + "angularVelocity": -0.9952930214480606, + "velocityX": -1.27085815069692, + "velocityY": -0.5053168069690293, + "timestamp": 2.9054828190854156 + }, + { + "x": 2.3392828051052046, + "y": 5.459567907623698, + "heading": 2.9373981254397274, + "angularVelocity": -1.0719920554570026, + "velocityX": -1.3756739228789079, + "velocityY": -0.5967670846367005, + "timestamp": 2.9354378247632944 + }, + { + "x": 2.2955095509891357, + "y": 5.438376261665861, + "heading": 2.9034010779477137, + "angularVelocity": -1.1349371072601777, + "velocityX": -1.4613001441823985, + "velocityY": -0.7074492385586875, + "timestamp": 2.965392830441173 + }, + { + "x": 2.2502877957375587, + "y": 5.413212847808716, + "heading": 2.8684201412995205, + "angularVelocity": -1.1677826746007212, + "velocityX": -1.5096560400578645, + "velocityY": -0.8400403634617765, + "timestamp": 2.995347836119052 + }, + { + "x": 2.205434968136323, + "y": 5.383827174903616, + "heading": 2.8338778225381454, + "angularVelocity": -1.15314011730883, + "velocityX": -1.4973399799539564, + "velocityY": -0.9809937351072376, + "timestamp": 3.0253028417969308 + }, + { + "x": 2.1627117665288242, + "y": 5.35082401983714, + "heading": 2.8010931886625263, + "angularVelocity": -1.0944626159703914, + "velocityX": -1.426245819043514, + "velocityY": -1.1017575967561364, + "timestamp": 3.0552578474748096 + }, + { + "x": 2.1231203758060415, + "y": 5.315102619215835, + "heading": 2.770927863402835, + "angularVelocity": -1.0070211831729985, + "velocityX": -1.3216953169206167, + "velocityY": -1.1925018811692434, + "timestamp": 3.0852128531526883 + }, + { + "x": 2.0871120973907478, + "y": 5.277368588566571, + "heading": 2.743899688375806, + "angularVelocity": -0.9022924354505801, + "velocityX": -1.2020788379247493, + "velocityY": -1.2596903187079023, + "timestamp": 3.115167858830567 + }, + { + "x": 2.0548974336097707, + "y": 5.238102066889186, + "heading": 2.7202319811811844, + "angularVelocity": -0.7901085864957776, + "velocityX": -1.0754350751055555, + "velocityY": -1.310850082942305, + "timestamp": 3.145122864508446 + }, + { + "x": 2.026587153392112, + "y": 5.197634903381892, + "heading": 2.6999923980627787, + "angularVelocity": -0.6756661419481035, + "velocityX": -0.9450934685873016, + "velocityY": -1.350931591950202, + "timestamp": 3.1750778701863247 + }, + { + "x": 2.002245678733676, + "y": 5.1562068665429, + "heading": 2.6831832138201195, + "angularVelocity": -0.5611477568529534, + "velocityX": -0.812601236674494, + "velocityY": -1.3830088127670148, + "timestamp": 3.2050328758642035 + }, + { + "x": 1.9819131544340292, + "y": 5.113998389763002, + "heading": 2.66978325115483, + "angularVelocity": -0.44733634202529504, + "velocityX": -0.6787688347748394, + "velocityY": -1.4090625531434064, + "timestamp": 3.2349878815420823 + }, + { + "x": 1.9656157831702816, + "y": 5.071149791653941, + "heading": 2.659765787935313, + "angularVelocity": -0.33441700286222414, + "velocityX": -0.5440616983685925, + "velocityY": -1.4304319808793762, + "timestamp": 3.264942887219961 + }, + { + "x": 1.9533711923999129, + "y": 5.027773084636261, + "heading": 2.6531059130951715, + "angularVelocity": -0.22232927984587103, + "velocityX": -0.4087660974610013, + "velocityY": -1.4480620529380386, + "timestamp": 3.29489789289784 + }, + { + "x": 1.9451914498185667, + "y": 4.983959581247479, + "heading": 2.649783348569472, + "angularVelocity": -0.11091850762536054, + "velocityX": -0.27306763581709725, + "velocityY": -1.4626438018383596, + "timestamp": 3.3248528985757186 + }, + { + "x": 1.941084861755371, + "y": 4.939785003662109, + "heading": 2.649783348569472, + "angularVelocity": 6.132292113202646e-25, + "velocityX": -0.13709188064779135, + "velocityY": -1.4746976869375825, + "timestamp": 3.3548079042535974 + }, + { + "x": 1.9419022015392158, + "y": 4.889062683793002, + "heading": 2.649783348569472, + "angularVelocity": 4.614715107153485e-26, + "velocityX": 0.023936675164732843, + "velocityY": -1.4854577230996109, + "timestamp": 3.3889538237774923 + }, + { + "x": 1.948223657679148, + "y": 4.838070468392074, + "heading": 2.649783348569472, + "angularVelocity": 4.653945144562686e-26, + "velocityX": 0.18513064600613907, + "velocityY": -1.4933619042018718, + "timestamp": 3.423099743301387 + }, + { + "x": 1.9600538072506943, + "y": 4.786928471515363, + "heading": 2.649783348569472, + "angularVelocity": 4.629261284078221e-26, + "velocityX": 0.3464586614300294, + "velocityY": -1.4977484159101515, + "timestamp": 3.457245662825282 + }, + { + "x": 1.977394685635232, + "y": 4.735788039854744, + "heading": 2.649783348569472, + "angularVelocity": 4.640038416252255e-26, + "velocityX": 0.5078462851879747, + "velocityY": -1.4977025768725385, + "timestamp": 3.491391582349177 + }, + { + "x": 2.0002427480647933, + "y": 4.684845229382865, + "heading": 2.649783348569472, + "angularVelocity": 4.6490332666751425e-26, + "velocityX": 0.6691300966012094, + "velocityY": -1.4919150276866968, + "timestamp": 3.525537501873072 + }, + { + "x": 2.0285822715254445, + "y": 4.634362832128866, + "heading": 2.649783348569472, + "angularVelocity": 4.622795940866032e-26, + "velocityX": 0.8299534426308102, + "velocityY": -1.478431331119124, + "timestamp": 3.559683421396967 + }, + { + "x": 2.0623699922411927, + "y": 4.5847082412928, + "heading": 2.649783348569472, + "angularVelocity": 4.634016030607817e-26, + "velocityX": 0.9895097624213414, + "velocityY": -1.454188129311252, + "timestamp": 3.5938293409208617 + }, + { + "x": 2.101495965027088, + "y": 4.536421557771227, + "heading": 2.649783348569472, + "angularVelocity": 4.645177261974479e-26, + "velocityX": 1.1458462191511727, + "velocityY": -1.4141274915083855, + "timestamp": 3.6279752604447566 + }, + { + "x": 2.145673252803998, + "y": 4.4903376692314145, + "heading": 2.649783348569472, + "angularVelocity": 4.6233288536561975e-26, + "velocityX": 1.2937794147261192, + "velocityY": -1.3496162698902718, + "timestamp": 3.6621211799686515 + }, + { + "x": 2.1941116699379357, + "y": 4.447748182132556, + "heading": 2.649783348569472, + "angularVelocity": 4.6479494614855375e-26, + "velocityX": 1.4185711736373225, + "velocityY": -1.2472789631292271, + "timestamp": 3.6962670994925464 + }, + { + "x": 2.2448201323304646, + "y": 4.410180142680831, + "heading": 2.649783348569472, + "angularVelocity": 4.6329904581670297e-26, + "velocityX": 1.485051891985029, + "velocityY": -1.1002204648621148, + "timestamp": 3.7304130190164413 + }, + { + "x": 2.294852293597042, + "y": 4.3780811675391975, + "heading": 2.649783348569472, + "angularVelocity": 4.6446166642422895e-26, + "velocityX": 1.4652456856979683, + "velocityY": -0.9400530309096446, + "timestamp": 3.764558938540336 + }, + { + "x": 2.342149827765447, + "y": 4.350766530180341, + "heading": 2.649783348569472, + "angularVelocity": 4.633506801188826e-26, + "velocityX": 1.3851591882100787, + "velocityY": -0.7999385501902068, + "timestamp": 3.798704858064231 + }, + { + "x": 2.3856944951859207, + "y": 4.3274872473595005, + "heading": 2.649783348569472, + "angularVelocity": 4.6381778689211406e-26, + "velocityX": 1.2752524467821442, + "velocityY": -0.6817588498253565, + "timestamp": 3.832850777588126 + }, + { + "x": 2.4249725857215654, + "y": 4.307695719032298, + "heading": 2.649783348569472, + "angularVelocity": 4.6432377599415885e-26, + "velocityX": 1.150301151156826, + "velocityY": -0.5796162060697082, + "timestamp": 3.866996697112021 + }, + { + "x": 2.4596964858202144, + "y": 4.291007007021927, + "heading": 2.649783348569472, + "angularVelocity": 4.6331179794053255e-26, + "velocityX": 1.0169267831358226, + "velocityY": -0.4887468910800007, + "timestamp": 3.9011426166359158 + }, + { + "x": 2.4896890256889828, + "y": 4.277143601542489, + "heading": 2.649783348569472, + "angularVelocity": 4.6467164999430504e-26, + "velocityX": 0.8783638070657234, + "velocityY": -0.4060047488173824, + "timestamp": 3.9352885361598107 + }, + { + "x": 2.514832571254234, + "y": 4.265898475268687, + "heading": 2.649783348569472, + "angularVelocity": 4.632856845272505e-26, + "velocityX": 0.7363557905551701, + "velocityY": -0.3293256245723021, + "timestamp": 3.9694344556837056 + }, + { + "x": 2.5350443768209585, + "y": 4.257112226026017, + "heading": 2.649783348569472, + "angularVelocity": 4.629729705599566e-26, + "velocityX": 0.5919244773180152, + "velocityY": -0.2573147645510306, + "timestamp": 4.0035803752076005 + }, + { + "x": 2.550263563230007, + "y": 4.2506587632470065, + "heading": 2.649783348569472, + "angularVelocity": 4.62810585566047e-26, + "velocityX": 0.445710252388941, + "velocityY": -0.18899660249284694, + "timestamp": 4.037726294731495 + }, + { + "x": 2.5604437164785643, + "y": 4.246436059535304, + "heading": 2.649783348569472, + "angularVelocity": 4.6519462435401186e-26, + "velocityX": 0.29813674343821717, + "velocityY": -0.12366642253542871, + "timestamp": 4.07187221425539 + }, + { + "x": 2.5655484199523926, + "y": 4.244359970092773, + "heading": 2.649783348569472, + "angularVelocity": 4.6699948971014915e-26, + "velocityX": 0.14949673474910496, + "velocityY": -0.06080051354533503, + "timestamp": 4.106018133779285 + }, + { + "x": 2.5655484199523926, + "y": 4.244359970092773, + "heading": 2.649783348569472, + "angularVelocity": 1.535197914331468e-26, + "velocityX": 1.1963425727073795e-24, + "velocityY": -7.168329076335369e-24, + "timestamp": 4.14016405330318 + } + ], + "constraints": [ + { + "scope": [ + 2, + 3 + ], + "type": "ZeroAngularVelocity" + }, + { + "scope": [ + 4, + 5 + ], + "type": "ZeroAngularVelocity" + }, + { + "scope": [ + 3 + ], + "type": "WptZeroVelocity" + }, + { + "scope": [ + 3 + ], + "type": "StopPoint" + }, + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + }, + "Test2": { + "waypoints": [ + { + "x": 0.3137660622596741, + "y": 6.865104675292969, + "heading": 3.135568639860519, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 27 + }, + { + "x": 1.7816849946975708, + "y": 6.948645114898682, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 25 + }, + { + "x": 2.6290202140808105, + "y": 6.972513675689697, + "heading": 3.286405336491994, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 23 + }, + { + "x": 7.999455451965332, + "y": 7.473754405975342, + "heading": 3.153293227446569, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 32 + }, + { + "x": 2.4533867835998535, + "y": 5.739410400390625, + "heading": 5.008312945270705, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 0.3137660622596754, + "y": 6.865104675292969, + "heading": 3.135568639860517, + "angularVelocity": -8.5693301650697e-28, + "velocityX": -3.4362695197914885e-17, + "velocityY": -4.743570524232907e-17, + "timestamp": 0 + }, + { + "x": 0.3176438425782737, + "y": 6.865425616765028, + "heading": 3.1355686398605154, + "angularVelocity": -5.1868461530169816e-17, + "velocityX": 0.13515003367410133, + "velocityY": 0.011185587421808195, + "timestamp": 0.028692411042588198 + }, + { + "x": 0.3253999903453479, + "y": 6.866060324505518, + "heading": 3.1355686398605136, + "angularVelocity": -5.186846153227154e-17, + "velocityX": 0.2703205302461453, + "velocityY": 0.022121101623313478, + "timestamp": 0.057384822085176396 + }, + { + "x": 0.3370351106624937, + "y": 6.867001226604551, + "heading": 3.135568639860512, + "angularVelocity": -5.186846153153239e-17, + "velocityX": 0.40551211607398097, + "velocityY": 0.032792716430754286, + "timestamp": 0.0860772331277646 + }, + { + "x": 0.352549826865152, + "y": 6.86824032070545, + "heading": 3.13556863986051, + "angularVelocity": -5.186846153220684e-17, + "velocityX": 0.5407254266512643, + "velocityY": 0.04318542973123231, + "timestamp": 0.11476964417035279 + }, + { + "x": 0.3719447806630506, + "y": 6.869769136338039, + "heading": 3.1355686398605083, + "angularVelocity": -5.1868461532349994e-17, + "velocityX": 0.6759611023663795, + "velocityY": 0.053282926635904025, + "timestamp": 0.14346205521294098 + }, + { + "x": 0.3952206321171861, + "y": 6.871578692739439, + "heading": 3.135568639860506, + "angularVelocity": -5.1868461531715824e-17, + "velocityX": 0.8112197828054859, + "velocityY": 0.06306742220841621, + "timestamp": 0.17215446625552916 + }, + { + "x": 0.42237805939934603, + "y": 6.8736594514681, + "heading": 3.1355686398605043, + "angularVelocity": -5.1868461532243226e-17, + "velocityX": 0.9465020991734979, + "velocityY": 0.07251947999676765, + "timestamp": 0.20084687729811734 + }, + { + "x": 0.45341775826418784, + "y": 6.876001262985059, + "heading": 3.1355686398605025, + "angularVelocity": -5.1868461531961763e-17, + "velocityX": 1.081808664276005, + "velocityY": 0.08161780177636822, + "timestamp": 0.22953928834070553 + }, + { + "x": 0.4883404411428946, + "y": 6.878593306214626, + "heading": 3.1355686398605007, + "angularVelocity": -5.186846153181617e-17, + "velocityX": 1.2171400593303083, + "velocityY": 0.09033898286621334, + "timestamp": 0.2582316993832937 + }, + { + "x": 0.5271468357396778, + "y": 6.881424019896891, + "heading": 3.135568639860499, + "angularVelocity": -5.186846153207198e-17, + "velocityX": 1.3524968166384297, + "velocityY": 0.09865722605409555, + "timestamp": 0.2869241104258819 + }, + { + "x": 0.5698376829754908, + "y": 6.884481024296066, + "heading": 3.135568639860497, + "angularVelocity": -5.18684615321628e-17, + "velocityX": 1.4878793968358233, + "velocityY": 0.10654400547371551, + "timestamp": 0.3156165214684701 + }, + { + "x": 0.6164137340738928, + "y": 6.887751031517755, + "heading": 3.1355686398604954, + "angularVelocity": -5.186846153204876e-17, + "velocityX": 1.6232881589932495, + "velocityY": 0.11396766959864212, + "timestamp": 0.34430893251105826 + }, + { + "x": 0.666875746517318, + "y": 6.891219742296381, + "heading": 3.1355686398604936, + "angularVelocity": -5.186846153160267e-17, + "velocityX": 1.758723321247663, + "velocityY": 0.12089296969422078, + "timestamp": 0.37300134355364645 + }, + { + "x": 0.7212244785112947, + "y": 6.894871726616115, + "heading": 3.135568639860492, + "angularVelocity": -5.186846153215831e-17, + "velocityX": 1.894184908800638, + "velocityY": 0.1272804963763128, + "timestamp": 0.40169375459623463 + }, + { + "x": 0.7794606814696723, + "y": 6.898690284891088, + "heading": 3.13556863986049, + "angularVelocity": -5.186846153213201e-17, + "velocityX": 2.02967268494572, + "velocityY": 0.1330860020548787, + "timestamp": 0.4303861656388228 + }, + { + "x": 0.8415850898614354, + "y": 6.9026572856068995, + "heading": 3.1355686398604883, + "angularVelocity": -5.186846153171342e-17, + "velocityX": 2.165186059113343, + "velocityY": 0.13825958055332968, + "timestamp": 0.459078576681411 + }, + { + "x": 0.9075984075182041, + "y": 6.906752974251311, + "heading": 3.1355686398604865, + "angularVelocity": -5.1868461532457525e-17, + "velocityX": 2.3007239635171715, + "velocityY": 0.14274466646709222, + "timestamp": 0.4877709877239992 + }, + { + "x": 0.9775012891595924, + "y": 6.910955746947304, + "heading": 3.1355686398604847, + "angularVelocity": -5.186846153166985e-17, + "velocityX": 2.43628468648487, + "velocityY": 0.14647680495568935, + "timestamp": 0.5164633987665874 + }, + { + "x": 1.051294315403438, + "y": 6.915241880318482, + "heading": 3.1355686398604825, + "angularVelocity": -5.1868461532007456e-17, + "velocityX": 2.571865645390099, + "velocityY": 0.1493821263335398, + "timestamp": 0.5451558098091757 + }, + { + "x": 1.1289779588157673, + "y": 6.919585206580955, + "heading": 3.1355686398604807, + "angularVelocity": -5.186846153206435e-17, + "velocityX": 2.7074630743656276, + "velocityY": 0.15137543708073178, + "timestamp": 0.5738482208517639 + }, + { + "x": 1.210552537505391, + "y": 6.923956719399065, + "heading": 3.135568639860479, + "angularVelocity": -5.1868461531968826e-17, + "velocityX": 2.843071590203452, + "velocityY": 0.1523578067950584, + "timestamp": 0.6025406318943521 + }, + { + "x": 1.29601815119489, + "y": 6.9283240912670765, + "heading": 3.135568639860477, + "angularVelocity": -5.186846153216223e-17, + "velocityX": 2.978683581614699, + "velocityY": 0.15221348465724693, + "timestamp": 0.6312330429369404 + }, + { + "x": 1.3853745923008893, + "y": 6.932651076487761, + "heading": 3.1355686398604754, + "angularVelocity": -5.1868461531841376e-17, + "velocityX": 3.1142883382426785, + "velocityY": 0.15080591220658612, + "timestamp": 0.6599254539795286 + }, + { + "x": 1.478621220826743, + "y": 6.936896764298285, + "heading": 3.1355686398604736, + "angularVelocity": -5.186846153215644e-17, + "velocityX": 3.2498707894378844, + "velocityY": 0.1479725006107481, + "timestamp": 0.6886178650221169 + }, + { + "x": 1.5757567859462194, + "y": 6.941014632922591, + "heading": 3.135568639860472, + "angularVelocity": -5.186846153173054e-17, + "velocityX": 3.3854096463101833, + "velocityY": 0.14351769247253987, + "timestamp": 0.7173102760647051 + }, + { + "x": 1.676779167520274, + "y": 6.94495133505747, + "heading": 3.13556863986047, + "angularVelocity": -5.186846153388186e-17, + "velocityX": 3.52087461120313, + "velocityY": 0.13720360164351214, + "timestamp": 0.7460026871072933 + }, + { + "x": 1.7816849946975712, + "y": 6.948645114898683, + "heading": 3.1355686398604683, + "angularVelocity": -1.6596882408000968e-16, + "velocityX": 3.6562220937649346, + "velocityY": 0.12873717150261788, + "timestamp": 0.7746950981498816 + }, + { + "x": 1.8121461784699802, + "y": 6.949692765946351, + "heading": 3.1362037838100756, + "angularVelocity": 0.07689012502825784, + "velocityX": 3.687611651202223, + "velocityY": 0.12682797354934053, + "timestamp": 0.7829555080448543 + }, + { + "x": 1.8428693964358074, + "y": 6.950724930078729, + "heading": 3.1374589452138397, + "angularVelocity": 0.15194904607940982, + "velocityX": 3.7193333450103565, + "velocityY": 0.12495313737441759, + "timestamp": 0.7912159179398269 + }, + { + "x": 1.8738574968140793, + "y": 6.95174197296817, + "heading": 3.139318096432083, + "angularVelocity": 0.22506767120320992, + "velocityX": 3.751399842413374, + "velocityY": 0.1231225692636009, + "timestamp": 0.7994763278347996 + }, + { + "x": 1.9051134303883683, + "y": 6.952744343319344, + "heading": 3.1417642609142864, + "angularVelocity": 0.2961311258529596, + "velocityX": 3.783823559810237, + "velocityY": 0.12134632105632295, + "timestamp": 0.8077367377297723 + }, + { + "x": 1.9366402470506743, + "y": 6.953732574884012, + "heading": 3.1447794649491048, + "angularVelocity": 0.36501869436959006, + "velocityX": 3.8166164952046153, + "velocityY": 0.11963468849991928, + "timestamp": 0.815997147624745 + }, + { + "x": 1.96844109067084, + "y": 6.9547072895390265, + "heading": 3.148344689757031, + "angularVelocity": 0.43160386146187707, + "velocityX": 3.849790025494764, + "velocityY": 0.117998339961998, + "timestamp": 0.8242575575197176 + }, + { + "x": 2.0005191919654135, + "y": 6.955669201722795, + "heading": 3.152439824908413, + "angularVelocity": 0.495754472653526, + "velocityX": 3.883354664287875, + "velocityY": 0.11644848088619933, + "timestamp": 0.8325179674146903 + }, + { + "x": 2.032877859008433, + "y": 6.9566191245667985, + "heading": 3.1570436241813726, + "angularVelocity": 0.5573330296555058, + "velocityX": 3.91731977643278, + "velocityY": 0.11499705899340439, + "timestamp": 0.840778377309663 + }, + { + "x": 2.065520465002766, + "y": 6.957557978090485, + "heading": 3.1621336650538163, + "angularVelocity": 0.6161971303075195, + "velocityX": 3.951693246384564, + "velocityY": 0.11365701407324179, + "timestamp": 0.8490387872046357 + }, + { + "x": 2.0984504329177724, + "y": 6.958486799841183, + "heading": 3.1676863130152344, + "angularVelocity": 0.6722000520579139, + "velocityX": 3.9864810988430395, + "velocityY": 0.11244257397721802, + "timestamp": 0.8572991970996083 + }, + { + "x": 2.1316712166001497, + "y": 6.959406758344203, + "heading": 3.1736766917418935, + "angularVelocity": 0.7251914617828771, + "velocityX": 4.021687071799445, + "velocityY": 0.1113695948160438, + "timestamp": 0.865559606994581 + }, + { + "x": 2.165186277981829, + "y": 6.960319169668765, + "heading": 3.1800786598480077, + "angularVelocity": 0.7750182118701426, + "velocityX": 4.057312144046869, + "velocityY": 0.11045593816320934, + "timestamp": 0.8738200168895537 + }, + { + "x": 2.1989990600414004, + "y": 6.961225517299853, + "heading": 3.1868647943382302, + "angularVelocity": 0.8215251514763765, + "velocityX": 4.093354021105941, + "velocityY": 0.10972187126423899, + "timestamp": 0.8820804267845264 + }, + { + "x": 2.2331129552203137, + "y": 6.962127475323903, + "heading": 3.19400637995892, + "angularVelocity": 0.8645558406293681, + "velocityX": 4.1298065849823535, + "velocityY": 0.10919046821108658, + "timestamp": 0.890340836679499 + }, + { + "x": 2.2675312690432237, + "y": 6.963026934685406, + "heading": 3.201473402262516, + "angularVelocity": 0.9039529997346885, + "velocityX": 4.1666593135838506, + "velocityY": 0.10888798170273117, + "timestamp": 0.8986012465744717 + }, + { + "x": 2.302257178726397, + "y": 6.963926031963223, + "heading": 3.2092345401896574, + "angularVelocity": 0.9395584511933502, + "velocityX": 4.203896673978105, + "velocityY": 0.10884414808067534, + "timestamp": 0.9068616564694444 + }, + { + "x": 2.3372936865473584, + "y": 6.964827179782931, + "heading": 3.2172571510612844, + "angularVelocity": 0.9712122005605541, + "velocityX": 4.241497488191654, + "velocityY": 0.10909238538572823, + "timestamp": 0.9151220663644171 + }, + { + "x": 2.372643567633051, + "y": 6.965733097685614, + "heading": 3.225507236598066, + "angularVelocity": 0.9987501397242283, + "velocityX": 4.279434257518503, + "velocityY": 0.10966984861524744, + "timestamp": 0.9233824762593897 + }, + { + "x": 2.4083093114869696, + "y": 6.9666468421059164, + "heading": 3.233949372180996, + "angularVelocity": 1.0219995969049733, + "velocityX": 4.31767240456481, + "velocityY": 0.11061732189065114, + "timestamp": 0.9316428861543624 + }, + { + "x": 2.44429305579275, + "y": 6.967571834188082, + "heading": 3.2425465717794997, + "angularVelocity": 1.0407715486052915, + "velocityX": 4.356169338240552, + "velocityY": 0.11197895672538041, + "timestamp": 0.9399032960493351 + }, + { + "x": 2.480596509367599, + "y": 6.968511884597095, + "heading": 3.251260045781759, + "angularVelocity": 1.054847654420144, + "velocityX": 4.394873140247142, + "velocityY": 0.11380190825438415, + "timestamp": 0.9481637059443078 + }, + { + "x": 2.517220857768171, + "y": 6.969471215284249, + "heading": 3.260048785175124, + "angularVelocity": 1.0639592350877263, + "velocityX": 4.433720464992984, + "velocityY": 0.11613596653794503, + "timestamp": 0.9564241158392804 + }, + { + "x": 2.55416663843719, + "y": 6.970454479070239, + "heading": 3.2688688682505744, + "angularVelocity": 1.0677536814268536, + "velocityX": 4.4726328522150816, + "velocityY": 0.11903329235341958, + "timestamp": 0.9646845257342531 + }, + { + "x": 2.5914335596417093, + "y": 6.971466777824258, + "heading": 3.2776723283732965, + "angularVelocity": 1.065741317277861, + "velocityX": 4.511509922431134, + "velocityY": 0.12254824722845813, + "timestamp": 0.9729449356292258 + }, + { + "x": 2.6290202140808088, + "y": 6.9725136756896955, + "heading": 3.286405336491996, + "angularVelocity": 1.0572124422077387, + "velocityX": 4.550216625687468, + "velocityY": 0.12673679378462502, + "timestamp": 0.9812053455241985 + }, + { + "x": 2.9717276780335036, + "y": 6.984363087388876, + "heading": 3.35108767309623, + "angularVelocity": 0.9203746590757935, + "velocityX": 4.876435853393211, + "velocityY": 0.16860705449782717, + "timestamp": 1.0514836131475132 + }, + { + "x": 3.3366815448761877, + "y": 6.999743371642888, + "heading": 3.3973428408952557, + "angularVelocity": 0.6581717131524953, + "velocityX": 5.192983253355129, + "velocityY": 0.21884836912099448, + "timestamp": 1.1217618807708278 + }, + { + "x": 3.71402485435163, + "y": 7.027526643127482, + "heading": 3.3973428368677423, + "angularVelocity": -5.730809398962461e-8, + "velocityX": 5.369274488921224, + "velocityY": 0.3953323327989187, + "timestamp": 1.1920401483941425 + }, + { + "x": 4.090360227255461, + "y": 7.066661937541599, + "heading": 3.3973428243317834, + "angularVelocity": -1.783760359331945e-7, + "velocityX": 5.354932408421835, + "velocityY": 0.5568619679682045, + "timestamp": 1.2623184160174572 + }, + { + "x": 4.466694898854889, + "y": 7.105803975313799, + "heading": 3.3973428117958613, + "angularVelocity": -1.7837551225317533e-7, + "velocityX": 5.354922429456401, + "velocityY": 0.5569579202208789, + "timestamp": 1.3325966836407719 + }, + { + "x": 4.8430291421579215, + "y": 7.144950130722809, + "heading": 3.397342799228156, + "angularVelocity": -1.7882775854425254e-7, + "velocityX": 5.354916335162799, + "velocityY": 0.5570165106920909, + "timestamp": 1.4028749512640866 + }, + { + "x": 5.214361577079249, + "y": 7.183676617694927, + "heading": 3.378511132398759, + "angularVelocity": -0.26795860891639617, + "velocityX": 5.283744854264688, + "velocityY": 0.55104498562348, + "timestamp": 1.4731532188874013 + }, + { + "x": 5.562524060208366, + "y": 7.219962031584869, + "heading": 3.355414523374288, + "angularVelocity": -0.32864511043819594, + "velocityX": 4.9540561385951625, + "velocityY": 0.5163105909843465, + "timestamp": 1.543431486510716 + }, + { + "x": 5.887470499962918, + "y": 7.253817931255832, + "heading": 3.331666055733143, + "angularVelocity": -0.3379205043646441, + "velocityX": 4.6237115788942855, + "velocityY": 0.48174066914158636, + "timestamp": 1.6137097541340306 + }, + { + "x": 6.189199807150463, + "y": 7.285249171486667, + "heading": 3.308363686841363, + "angularVelocity": -0.33157289841972754, + "velocityX": 4.29335152091094, + "velocityY": 0.4472398266744939, + "timestamp": 1.6839880217573453 + }, + { + "x": 6.467713626267079, + "y": 7.31425833816948, + "heading": 3.2860397702782755, + "angularVelocity": -0.3176503536305448, + "velocityX": 3.963014862708697, + "velocityY": 0.41277577925367004, + "timestamp": 1.75426628938066 + }, + { + "x": 6.723013594809074, + "y": 7.34084703024564, + "heading": 3.265008761402615, + "angularVelocity": -0.29925337642620564, + "velocityX": 3.6327015046867763, + "velocityY": 0.37833448340917136, + "timestamp": 1.8245445570039747 + }, + { + "x": 6.955101065031071, + "y": 7.365016333172441, + "heading": 3.245478308115181, + "angularVelocity": -0.27790174612893576, + "velocityX": 3.302407388098493, + "velocityY": 0.34390863269912847, + "timestamp": 1.8948228246272893 + }, + { + "x": 7.163977127241494, + "y": 7.386767031992689, + "heading": 3.2275957745381954, + "angularVelocity": -0.2544532496565816, + "velocityX": 2.972128785672115, + "velocityY": 0.3094939524808096, + "timestamp": 1.965101092250604 + }, + { + "x": 7.349642665971283, + "y": 7.4060997209757735, + "heading": 3.2114711639593976, + "angularVelocity": -0.22943949992088322, + "velocityX": 2.6418627693690517, + "velocityY": 0.2750877282106925, + "timestamp": 2.035379359873919 + }, + { + "x": 7.512098408217057, + "y": 7.423014865697383, + "heading": 3.1971897253730983, + "angularVelocity": -0.2032127294720464, + "velocityX": 2.3116070976097918, + "velocityY": 0.24068812868678965, + "timestamp": 2.1056576274972336 + }, + { + "x": 7.651344959888608, + "y": 7.43751284079522, + "heading": 3.184819461924971, + "angularVelocity": -0.17601833207433357, + "velocityX": 1.981360047431725, + "velocityY": 0.20629385993890986, + "timestamp": 2.1759358951205483 + }, + { + "x": 7.767382832665261, + "y": 7.449593954241994, + "heading": 3.174415882660127, + "angularVelocity": -0.14803408815663477, + "velocityX": 1.6511202780154728, + "velocityY": 0.1719039733808395, + "timestamp": 2.246214162743863 + }, + { + "x": 7.860212463836391, + "y": 7.459258463658005, + "heading": 3.1660251568616267, + "angularVelocity": -0.11939289459260129, + "velocityX": 1.3208867308552614, + "velocityY": 0.13751775254062693, + "timestamp": 2.3164924303671777 + }, + { + "x": 7.929834231127341, + "y": 7.466506587678373, + "heading": 3.159686290431417, + "angularVelocity": -0.09019668020541635, + "velocityX": 0.9906585584055106, + "velocityY": 0.10313464269233455, + "timestamp": 2.3867706979904924 + }, + { + "x": 7.976248463934981, + "y": 7.471338514116222, + "heading": 3.1554326760982865, + "angularVelocity": -0.06052531567696676, + "velocityX": 0.6604350729932147, + "velocityY": 0.06875420526505516, + "timestamp": 2.457048965613807 + }, + { + "x": 7.999455451965332, + "y": 7.473754405975342, + "heading": 3.153293227446569, + "angularVelocity": -0.03044253542482025, + "velocityX": 0.33021570985126986, + "velocityY": 0.03437608724320182, + "timestamp": 2.5273272332371217 + }, + { + "x": 7.999455451965332, + "y": 7.473754405975342, + "heading": 3.153293227446569, + "angularVelocity": -3.79407884851814e-18, + "velocityX": 2.546987187300363e-16, + "velocityY": -1.0256209789219734e-16, + "timestamp": 2.5976055008604364 + }, + { + "x": 7.9777911181971035, + "y": 7.466979623755239, + "heading": 3.153293227446569, + "angularVelocity": 4.0010921973360205e-29, + "velocityX": -0.31261406582002, + "velocityY": -0.09775939742848679, + "timestamp": 2.6669060734735086 + }, + { + "x": 7.934462450883723, + "y": 7.453430059384792, + "heading": 3.153293227446569, + "angularVelocity": -2.051733815514594e-34, + "velocityX": -0.6252281284210741, + "velocityY": -0.1955187938503514, + "timestamp": 2.736206646086581 + }, + { + "x": 7.869469450281316, + "y": 7.433105712944096, + "heading": 3.153293227446569, + "angularVelocity": -1.98522229630168e-34, + "velocityX": -0.9378421873262779, + "velocityY": -0.29327818911646475, + "timestamp": 2.805507218699653 + }, + { + "x": 7.782812116686986, + "y": 7.40600658452606, + "heading": 3.153293227446569, + "angularVelocity": -1.9190015159804115e-34, + "velocityX": -1.2504562419442953, + "velocityY": -0.39103758304190667, + "timestamp": 2.8748077913127252 + }, + { + "x": 7.6744904504495075, + "y": 7.372132674239751, + "heading": 3.153293227446569, + "angularVelocity": -8.200637720207044e-29, + "velocityX": -1.5630702915295283, + "velocityY": -0.4887969753935167, + "timestamp": 2.9441083639257974 + }, + { + "x": 7.544504451984089, + "y": 7.331483982215012, + "heading": 3.153293227446569, + "angularVelocity": 6.610929004011549e-29, + "velocityX": -1.8756843351233508, + "velocityY": -0.5865563658715173, + "timestamp": 3.0134089365388697 + }, + { + "x": 7.392854121793352, + "y": 7.28406050860902, + "heading": 3.153293227446569, + "angularVelocity": -6.483737543755527e-29, + "velocityX": -2.188298371464413, + "velocityY": -0.6843157540814643, + "timestamp": 3.082709509151942 + }, + { + "x": 7.219539460498179, + "y": 7.229862253615935, + "heading": 3.153293227446569, + "angularVelocity": 8.073376116568391e-29, + "velocityX": -2.5009123988461828, + "velocityY": -0.7820751394896976, + "timestamp": 3.152010081765014 + }, + { + "x": 7.024560468885024, + "y": 7.168889217481695, + "heading": 3.153293227446569, + "angularVelocity": -3.526812750348743e-30, + "velocityX": -2.813526414879514, + "velocityY": -0.8798345213490937, + "timestamp": 3.2213106543780863 + }, + { + "x": 6.807917147982327, + "y": 7.101141400527908, + "heading": 3.153293227446569, + "angularVelocity": -9.438045628849687e-29, + "velocityX": -3.1261404160725776, + "velocityY": -0.9775938985677018, + "timestamp": 3.2906112269911585 + }, + { + "x": 6.569609499192503, + "y": 7.026618803193132, + "heading": 3.153293227446569, + "angularVelocity": 9.790681244582267e-29, + "velocityX": -3.438754397028912, + "velocityY": -1.0753532694579622, + "timestamp": 3.3599117996042307 + }, + { + "x": 6.309637524541267, + "y": 6.94532142611084, + "heading": 3.153293227446569, + "angularVelocity": -1.3895984503554753e-34, + "velocityX": -3.7513683487544087, + "velocityY": -1.1731126312072742, + "timestamp": 3.429212372217303 + }, + { + "x": 6.028001227211883, + "y": 6.85724927027649, + "heading": 3.153293227446569, + "angularVelocity": -1.323503512813964e-34, + "velocityX": -4.063982254545725, + "velocityY": -1.2708719785922367, + "timestamp": 3.498512944830375 + }, + { + "x": 5.724700612934229, + "y": 6.762402337481905, + "heading": 3.153293227446569, + "angularVelocity": -2.1989550257903952e-29, + "velocityX": -4.376596077655505, + "velocityY": -1.3686313001213652, + "timestamp": 3.5678135174434473 + }, + { + "x": 5.399735695078014, + "y": 6.660780631908008, + "heading": 3.153293227446569, + "angularVelocity": 1.8024841235265752e-29, + "velocityX": -4.689209707841794, + "velocityY": -1.4663905613202457, + "timestamp": 3.6371140900565195 + }, + { + "x": 5.053106540487171, + "y": 6.552384174457959, + "heading": 3.153293227446569, + "angularVelocity": 1.896211807861914e-29, + "velocityX": -5.001822373477154, + "velocityY": -1.564149520888688, + "timestamp": 3.7064146626695917 + }, + { + "x": 4.7281416226309565, + "y": 6.4507624688840615, + "heading": 3.153293227446569, + "angularVelocity": -1.4997927313419928e-29, + "velocityX": -4.689209707841795, + "velocityY": -1.4663905613202457, + "timestamp": 3.775715235282664 + }, + { + "x": 4.424841008353304, + "y": 6.355915536089477, + "heading": 3.153293227446569, + "angularVelocity": -4.4185947102012545e-35, + "velocityX": -4.376596077655505, + "velocityY": -1.3686313001213652, + "timestamp": 3.845015807895736 + }, + { + "x": 4.14320471102392, + "y": 6.267843380255127, + "heading": 3.153293227446569, + "angularVelocity": -9.264733448000688e-35, + "velocityX": -4.063982254545726, + "velocityY": -1.2708719785922367, + "timestamp": 3.9143163805088084 + }, + { + "x": 3.883232736372683, + "y": 6.186546003172835, + "heading": 3.153293227446569, + "angularVelocity": 3.5265482976182584e-30, + "velocityX": -3.751368348754409, + "velocityY": -1.1731126312072742, + "timestamp": 3.9836169531218806 + }, + { + "x": 3.644925087582859, + "y": 6.112023405838059, + "heading": 3.153293227446569, + "angularVelocity": 2.156768838744686e-29, + "velocityX": -3.438754397028912, + "velocityY": -1.0753532694579622, + "timestamp": 4.052917525734953 + }, + { + "x": 3.4282817666801613, + "y": 6.044275588884272, + "heading": 3.153293227446569, + "angularVelocity": -2.105657187574439e-28, + "velocityX": -3.1261404160725776, + "velocityY": -0.9775938985677018, + "timestamp": 4.122218098348026 + }, + { + "x": 3.2333027750670063, + "y": 5.983302552750032, + "heading": 3.153293227446569, + "angularVelocity": 1.8547117767579628e-28, + "velocityX": -2.813526414879514, + "velocityY": -0.8798345213490937, + "timestamp": 4.1915186709610985 + }, + { + "x": 3.059988113771833, + "y": 5.929104297756947, + "heading": 3.153293227446569, + "angularVelocity": -5.955568710068408e-35, + "velocityX": -2.5009123988461828, + "velocityY": -0.7820751394896976, + "timestamp": 4.260819243574171 + }, + { + "x": 2.908337783581096, + "y": 5.881680824150955, + "heading": 3.153293227446569, + "angularVelocity": -2.0810713408746708e-29, + "velocityX": -2.1882983714644135, + "velocityY": -0.6843157540814642, + "timestamp": 4.330119816187244 + }, + { + "x": 2.7783517851156776, + "y": 5.841032132126216, + "heading": 3.153293227446569, + "angularVelocity": 2.081061415014159e-29, + "velocityX": -1.875684335123351, + "velocityY": -0.5865563658715173, + "timestamp": 4.3994203888003165 + }, + { + "x": 2.6700301188781994, + "y": 5.807158221839908, + "heading": 3.153293227446569, + "angularVelocity": 7.326893682940581e-29, + "velocityX": -1.5630702915295283, + "velocityY": -0.48879697539351663, + "timestamp": 4.468720961413389 + }, + { + "x": 2.583372785283869, + "y": 5.780059093421871, + "heading": 3.153293227446569, + "angularVelocity": -7.326900961882318e-29, + "velocityX": -1.2504562419442953, + "velocityY": -0.39103758304190656, + "timestamp": 4.538021534026462 + }, + { + "x": 2.5183797846814615, + "y": 5.759734746981175, + "heading": 3.153293227446569, + "angularVelocity": -2.64698564160025e-35, + "velocityX": -0.9378421873262781, + "velocityY": -0.2932781891164647, + "timestamp": 4.6073221066395345 + }, + { + "x": 2.4750511173680816, + "y": 5.746185182610728, + "heading": 3.153293227446569, + "angularVelocity": -4.5099737006589516e-29, + "velocityX": -0.6252281284210743, + "velocityY": -0.1955187938503513, + "timestamp": 4.676622679252607 + }, + { + "x": 2.4533867835998535, + "y": 5.739410400390625, + "heading": 3.153293227446569, + "angularVelocity": 1.9207053764808128e-28, + "velocityX": -0.31261406582002027, + "velocityY": -0.09775939742848665, + "timestamp": 4.74592325186568 + }, + { + "x": 2.4533867835998535, + "y": 5.739410400390625, + "heading": 3.153293227446569, + "angularVelocity": -4.899028012023128e-29, + "velocityX": 1.275108562409782e-22, + "velocityY": 3.987692290669026e-23, + "timestamp": 4.815223824478752 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + }, + { + "scope": [ + 0, + 1 + ], + "type": "ZeroAngularVelocity" + }, + { + "scope": [ + 3 + ], + "type": "StopPoint" + }, + { + "scope": [ + 3, + 4 + ], + "type": "ZeroAngularVelocity" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + } + }, + "splitTrajectoriesAtStopPoints": true, + "usesObstacles": false +} \ No newline at end of file diff --git a/build.gradle b/build.gradle index 03cf3dc8..30e6c78e 100644 --- a/build.gradle +++ b/build.gradle @@ -2,6 +2,7 @@ plugins { id "java" id "edu.wpi.first.GradleRIO" version "2024.3.1" id "org.sonarqube" version "4.4.1.3373" + id "com.peterabeles.gversion" version "1.10" } java { @@ -11,6 +12,27 @@ java { def ROBOT_MAIN_CLASS = "frc.robot.Main" +repositories { + maven { + url = uri("https://maven.pkg.github.com/Mechanical-Advantage/AdvantageKit") + credentials { + username = "Mechanical-Advantage-Bot" + password = "\u0067\u0068\u0070\u005f\u006e\u0056\u0051\u006a\u0055\u004f\u004c\u0061\u0079\u0066\u006e\u0078\u006e\u0037\u0051\u0049\u0054\u0042\u0032\u004c\u004a\u006d\u0055\u0070\u0073\u0031\u006d\u0037\u004c\u005a\u0030\u0076\u0062\u0070\u0063\u0051" + } + } + mavenLocal() +} + +configurations.all { + exclude group: "edu.wpi.first.wpilibj" +} + +task(checkAkitInstall, dependsOn: "classes", type: JavaExec) { + mainClass = "org.littletonrobotics.junction.CheckInstall" + classpath = sourceSets.main.runtimeClasspath +} +compileJava.finalizedBy checkAkitInstall + // Define my targets (RoboRIO) and artifacts (deployable files) // This is added by GradleRIO's backing project DeployUtils. deploy { @@ -29,12 +51,12 @@ deploy { frcJava(getArtifactTypeClass('FRCJavaArtifact')) { // Enable VisualVM connection - jvmArgs.add("-Dcom.sun.management.jmxremote=true") - jvmArgs.add("-Dcom.sun.management.jmxremote.port=1198") - jvmArgs.add("-Dcom.sun.management.jmxremote.local.only=false") - jvmArgs.add("-Dcom.sun.management.jmxremote.ssl=false") - jvmArgs.add("-Dcom.sun.management.jmxremote.authenticate=false") - jvmArgs.add("-Djava.rmi.server.hostname=10.44.67.2") // Replace XX.XX with team number +// jvmArgs.add("-Dcom.sun.management.jmxremote=true") +// jvmArgs.add("-Dcom.sun.management.jmxremote.port=1198") +// jvmArgs.add("-Dcom.sun.management.jmxremote.local.only=false") +// jvmArgs.add("-Dcom.sun.management.jmxremote.ssl=false") +// jvmArgs.add("-Dcom.sun.management.jmxremote.authenticate=false") +// jvmArgs.add("-Djava.rmi.server.hostname=10.44.67.2") // Replace XX.XX with team number } // Static files artifact @@ -76,6 +98,9 @@ dependencies { simulationRelease wpi.sim.enableRelease() implementation 'gov.nist.math:jama:1.0.3' + + def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) + annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version" } test { @@ -115,10 +140,20 @@ deployArtifact.jarTask = jar wpi.java.configureExecutableTasks(jar) wpi.java.configureTestTasks(test) +// Create version file +project.compileJava.dependsOn(createVersionFile) +gversion { + srcDir = "src/main/java/" + classPackage = "frc.robot" + className = "BuildConstants" + dateFormat = "yyyy-MM-dd HH:mm:ss z" + timeZone = "America/New_York" + indent = " " +} + // Configure string concat to always inline compile tasks.withType(JavaCompile) { options.compilerArgs.add '-XDstringConcat=inline' - } diff --git a/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat index f2a66205..333fb9d2 100644 Binary files a/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat and b/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 010 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 010 - 0 - ext.dat index dc553476..3db23480 100644 Binary files a/ctre_sim/Talon FX vers. C - 010 - 0 - ext.dat and b/ctre_sim/Talon FX vers. C - 010 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 016 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 016 - 0 - ext.dat index 235336d0..be834f1e 100644 Binary files a/ctre_sim/Talon FX vers. C - 016 - 0 - ext.dat and b/ctre_sim/Talon FX vers. C - 016 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 017 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 017 - 0 - ext.dat index 2ae33101..bae3be2a 100644 Binary files a/ctre_sim/Talon FX vers. C - 017 - 0 - ext.dat and b/ctre_sim/Talon FX vers. C - 017 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat index f3019bae..81d44a58 100644 Binary files a/ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat and b/ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat index 552f487d..7d179b99 100644 Binary files a/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat and b/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat differ diff --git a/logs/FRC_20240310_203436.wpilog b/logs/FRC_20240310_203436.wpilog new file mode 100644 index 00000000..c9f7a3dd Binary files /dev/null and b/logs/FRC_20240310_203436.wpilog differ diff --git a/logs/FRC_20240310_203649.wpilog b/logs/FRC_20240310_203649.wpilog new file mode 100644 index 00000000..4a0a9c04 Binary files /dev/null and b/logs/FRC_20240310_203649.wpilog differ diff --git a/logs/FRC_20240310_221225.wpilog b/logs/FRC_20240310_221225.wpilog new file mode 100644 index 00000000..869abcd5 Binary files /dev/null and b/logs/FRC_20240310_221225.wpilog differ diff --git a/logs/FRC_20240310_221327.wpilog b/logs/FRC_20240310_221327.wpilog new file mode 100644 index 00000000..71056d22 Binary files /dev/null and b/logs/FRC_20240310_221327.wpilog differ diff --git a/logs/FRC_20240310_221513.wpilog b/logs/FRC_20240310_221513.wpilog new file mode 100644 index 00000000..d9f00e84 Binary files /dev/null and b/logs/FRC_20240310_221513.wpilog differ diff --git a/logs/FRC_20240310_221546.wpilog b/logs/FRC_20240310_221546.wpilog new file mode 100644 index 00000000..4e24f1de Binary files /dev/null and b/logs/FRC_20240310_221546.wpilog differ diff --git a/logs/FRC_20240310_221717.wpilog b/logs/FRC_20240310_221717.wpilog new file mode 100644 index 00000000..8b29e3c5 Binary files /dev/null and b/logs/FRC_20240310_221717.wpilog differ diff --git a/logs/FRC_20240310_221825.wpilog b/logs/FRC_20240310_221825.wpilog new file mode 100644 index 00000000..ebe6ce20 Binary files /dev/null and b/logs/FRC_20240310_221825.wpilog differ diff --git a/logs/FRC_20240310_221848.wpilog b/logs/FRC_20240310_221848.wpilog new file mode 100644 index 00000000..d9f596e9 Binary files /dev/null and b/logs/FRC_20240310_221848.wpilog differ diff --git a/logs/FRC_20240310_222046.wpilog b/logs/FRC_20240310_222046.wpilog new file mode 100644 index 00000000..e43651d5 Binary files /dev/null and b/logs/FRC_20240310_222046.wpilog differ diff --git a/logs/FRC_20240310_222123.wpilog b/logs/FRC_20240310_222123.wpilog new file mode 100644 index 00000000..75d29f57 Binary files /dev/null and b/logs/FRC_20240310_222123.wpilog differ diff --git a/logs/FRC_20240310_222300.wpilog b/logs/FRC_20240310_222300.wpilog new file mode 100644 index 00000000..098b959e Binary files /dev/null and b/logs/FRC_20240310_222300.wpilog differ diff --git a/logs/FRC_20240310_222316.wpilog b/logs/FRC_20240310_222316.wpilog new file mode 100644 index 00000000..e5f5b985 Binary files /dev/null and b/logs/FRC_20240310_222316.wpilog differ diff --git a/logs/FRC_20240310_222414.wpilog b/logs/FRC_20240310_222414.wpilog new file mode 100644 index 00000000..b3404005 Binary files /dev/null and b/logs/FRC_20240310_222414.wpilog differ diff --git a/logs/FRC_20240310_222606.wpilog b/logs/FRC_20240310_222606.wpilog new file mode 100644 index 00000000..51eedb5e Binary files /dev/null and b/logs/FRC_20240310_222606.wpilog differ diff --git a/logs/FRC_20240310_222655.wpilog b/logs/FRC_20240310_222655.wpilog new file mode 100644 index 00000000..63d0a10e Binary files /dev/null and b/logs/FRC_20240310_222655.wpilog differ diff --git a/logs/FRC_20240310_222707.wpilog b/logs/FRC_20240310_222707.wpilog new file mode 100644 index 00000000..3aa97de1 Binary files /dev/null and b/logs/FRC_20240310_222707.wpilog differ diff --git a/logs/FRC_20240310_222808.wpilog b/logs/FRC_20240310_222808.wpilog new file mode 100644 index 00000000..0d51f938 Binary files /dev/null and b/logs/FRC_20240310_222808.wpilog differ diff --git a/logs/FRC_20240310_222820.wpilog b/logs/FRC_20240310_222820.wpilog new file mode 100644 index 00000000..9ee78198 Binary files /dev/null and b/logs/FRC_20240310_222820.wpilog differ diff --git a/logs/FRC_20240310_222837.wpilog b/logs/FRC_20240310_222837.wpilog new file mode 100644 index 00000000..fc8d96a5 Binary files /dev/null and b/logs/FRC_20240310_222837.wpilog differ diff --git a/logs/FRC_20240310_222944.wpilog b/logs/FRC_20240310_222944.wpilog new file mode 100644 index 00000000..2bdb48ae Binary files /dev/null and b/logs/FRC_20240310_222944.wpilog differ diff --git a/logs/FRC_20240310_223012.wpilog b/logs/FRC_20240310_223012.wpilog new file mode 100644 index 00000000..cbaa85fa Binary files /dev/null and b/logs/FRC_20240310_223012.wpilog differ diff --git a/logs/FRC_20240310_223030.wpilog b/logs/FRC_20240310_223030.wpilog new file mode 100644 index 00000000..3b1bc118 Binary files /dev/null and b/logs/FRC_20240310_223030.wpilog differ diff --git a/logs/FRC_20240310_223043.wpilog b/logs/FRC_20240310_223043.wpilog new file mode 100644 index 00000000..bdf1ecaa Binary files /dev/null and b/logs/FRC_20240310_223043.wpilog differ diff --git a/logs/FRC_20240310_223255.wpilog b/logs/FRC_20240310_223255.wpilog new file mode 100644 index 00000000..a5e708b0 Binary files /dev/null and b/logs/FRC_20240310_223255.wpilog differ diff --git a/logs/FRC_20240310_223342.wpilog b/logs/FRC_20240310_223342.wpilog new file mode 100644 index 00000000..45479602 Binary files /dev/null and b/logs/FRC_20240310_223342.wpilog differ diff --git a/logs/FRC_20240310_223413.wpilog b/logs/FRC_20240310_223413.wpilog new file mode 100644 index 00000000..30cf3c74 Binary files /dev/null and b/logs/FRC_20240310_223413.wpilog differ diff --git a/logs/FRC_20240310_223523.wpilog b/logs/FRC_20240310_223523.wpilog new file mode 100644 index 00000000..36edc473 Binary files /dev/null and b/logs/FRC_20240310_223523.wpilog differ diff --git a/logs/FRC_20240310_223729.wpilog b/logs/FRC_20240310_223729.wpilog new file mode 100644 index 00000000..5bac5fc8 Binary files /dev/null and b/logs/FRC_20240310_223729.wpilog differ diff --git a/logs/FRC_20240310_223759.wpilog b/logs/FRC_20240310_223759.wpilog new file mode 100644 index 00000000..f8c615ce Binary files /dev/null and b/logs/FRC_20240310_223759.wpilog differ diff --git a/logs/FRC_20240310_223939.wpilog b/logs/FRC_20240310_223939.wpilog new file mode 100644 index 00000000..223b84cf Binary files /dev/null and b/logs/FRC_20240310_223939.wpilog differ diff --git a/logs/FRC_20240310_224020.wpilog b/logs/FRC_20240310_224020.wpilog new file mode 100644 index 00000000..c9328c7d Binary files /dev/null and b/logs/FRC_20240310_224020.wpilog differ diff --git a/logs/FRC_20240310_233720.wpilog b/logs/FRC_20240310_233720.wpilog new file mode 100644 index 00000000..0d6eba21 Binary files /dev/null and b/logs/FRC_20240310_233720.wpilog differ diff --git a/logs/FRC_20240310_233831.wpilog b/logs/FRC_20240310_233831.wpilog new file mode 100644 index 00000000..c2e3fc21 Binary files /dev/null and b/logs/FRC_20240310_233831.wpilog differ diff --git a/logs/FRC_20240310_234038.wpilog b/logs/FRC_20240310_234038.wpilog new file mode 100644 index 00000000..d8ca42bd Binary files /dev/null and b/logs/FRC_20240310_234038.wpilog differ diff --git a/logs/FRC_20240311_001406.wpilog b/logs/FRC_20240311_001406.wpilog new file mode 100644 index 00000000..3bc82ee1 Binary files /dev/null and b/logs/FRC_20240311_001406.wpilog differ diff --git a/logs/FRC_20240311_001514.wpilog b/logs/FRC_20240311_001514.wpilog new file mode 100644 index 00000000..c5f62212 Binary files /dev/null and b/logs/FRC_20240311_001514.wpilog differ diff --git a/logs/FRC_20240311_001544.wpilog b/logs/FRC_20240311_001544.wpilog new file mode 100644 index 00000000..99badd68 Binary files /dev/null and b/logs/FRC_20240311_001544.wpilog differ diff --git a/logs/FRC_20240311_001637.wpilog b/logs/FRC_20240311_001637.wpilog new file mode 100644 index 00000000..5d8540b1 Binary files /dev/null and b/logs/FRC_20240311_001637.wpilog differ diff --git a/logs/FRC_20240311_001653.wpilog b/logs/FRC_20240311_001653.wpilog new file mode 100644 index 00000000..4ff2eccf Binary files /dev/null and b/logs/FRC_20240311_001653.wpilog differ diff --git a/logs/FRC_20240311_001744.wpilog b/logs/FRC_20240311_001744.wpilog new file mode 100644 index 00000000..d76c1a36 Binary files /dev/null and b/logs/FRC_20240311_001744.wpilog differ diff --git a/logs/FRC_20240311_001807.wpilog b/logs/FRC_20240311_001807.wpilog new file mode 100644 index 00000000..8e61ec15 Binary files /dev/null and b/logs/FRC_20240311_001807.wpilog differ diff --git a/logs/FRC_20240311_001902.wpilog b/logs/FRC_20240311_001902.wpilog new file mode 100644 index 00000000..8b9265c8 Binary files /dev/null and b/logs/FRC_20240311_001902.wpilog differ diff --git a/logs/sim_2024-03-10_16-34-27.hoot b/logs/sim_2024-03-10_16-34-27.hoot new file mode 100644 index 00000000..1fc1cdbe Binary files /dev/null and b/logs/sim_2024-03-10_16-34-27.hoot differ diff --git a/networktables.json b/networktables.json index de79f40f..d14632ec 100644 --- a/networktables.json +++ b/networktables.json @@ -531,5 +531,325 @@ "properties": { "persistent": true } + }, + { + "name": "/Preferences/Shooter/Left PID.mm.kp", + "type": "double", + "value": 0.19623, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Shooter/Left PID.mm.kD", + "type": "double", + "value": 0.005, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Shooter/Left PID.mm.kS", + "type": "double", + "value": 0.21963, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Shooter/Left PID.mm.kV", + "type": "double", + "value": 0.13041, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Shooter/Right PID.mm.kp", + "type": "double", + "value": 0.19623, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Shooter/Right PID.mm.kD", + "type": "double", + "value": 0.005, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Shooter/Right PID.mm.kS", + "type": "double", + "value": 0.21963, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Shooter/Right PID.mm.kV", + "type": "double", + "value": 0.13041, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Shooter/Left RPM", + "type": "double", + "value": 3600.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Shooter/Right RPM", + "type": "double", + "value": 3600.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Arm PID.mm.kp", + "type": "double", + "value": 72.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Arm PID.mm.ki", + "type": "double", + "value": 6.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Arm PID.mm.kD", + "type": "double", + "value": 3.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Arm PID.mm.kS", + "type": "double", + "value": 0.375, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Arm PID.mm.kV", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Arm PID.mm.kG", + "type": "double", + "value": 0.375, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Wrist PID.mm.kp", + "type": "double", + "value": 108.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Wrist PID.mm.ki", + "type": "double", + "value": 3.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Wrist PID.mm.kD", + "type": "double", + "value": 3.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Wrist PID.mm.kS", + "type": "double", + "value": 0.375, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Wrist PID.mm.kV", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Wrist PID.mm.kG", + "type": "double", + "value": 0.35, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Arm Max Vel DegsS", + "type": "double", + "value": 120.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Wrist Max Vel DegsS", + "type": "double", + "value": 120.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Acceleration Time Secs", + "type": "double", + "value": 1.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Climber/Left Climber.mm.kp", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Climber/Left Climber.mm.ki", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Climber/Left Climber.mm.kD", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Climber/Left Climber.mm.kS", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Climber/Left Climber.mm.kV", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Climber/Left Climber.mm.kG", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Climber/Right Climber.mm.kp", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Climber/Right Climber.mm.ki", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Climber/Right Climber.mm.kD", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Climber/Right Climber.mm.kS", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Climber/Right Climber.mm.kV", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Climber/Right Climber.mm.kG", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/ArmPoses/Amp IntermediateArm Angle", + "type": "double", + "value": 60.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/ArmPoses/Amp IntermediateWrist Angle", + "type": "double", + "value": 145.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Wrist Angle", + "type": "double", + "value": 45.0, + "properties": { + "persistent": true + } } ] diff --git a/networktables.json.bck b/networktables.json.bck index c2f36cad..4168d9b3 100644 --- a/networktables.json.bck +++ b/networktables.json.bck @@ -106,9 +106,10 @@ { "name": "/Preferences/Drive/Module3/Turn Pid Property.mm.kp", "type": "double", - "value": 7.0, + "value": 43.982, "properties": { - "persistent": true + "persistent": true, + "retained": true } }, { @@ -530,5 +531,317 @@ "properties": { "persistent": true } + }, + { + "name": "/Preferences/Shooter/Left PID.mm.kp", + "type": "double", + "value": 0.19623, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Shooter/Left PID.mm.kD", + "type": "double", + "value": 0.005, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Shooter/Left PID.mm.kS", + "type": "double", + "value": 0.21963, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Shooter/Left PID.mm.kV", + "type": "double", + "value": 0.13041, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Shooter/Right PID.mm.kp", + "type": "double", + "value": 0.19623, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Shooter/Right PID.mm.kD", + "type": "double", + "value": 0.005, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Shooter/Right PID.mm.kS", + "type": "double", + "value": 0.21963, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Shooter/Right PID.mm.kV", + "type": "double", + "value": 0.13041, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Shooter/Left RPM", + "type": "double", + "value": 3600.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Shooter/Right RPM", + "type": "double", + "value": 3600.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Arm PID.mm.kp", + "type": "double", + "value": 72.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Arm PID.mm.ki", + "type": "double", + "value": 6.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Arm PID.mm.kD", + "type": "double", + "value": 3.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Arm PID.mm.kS", + "type": "double", + "value": 0.375, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Arm PID.mm.kV", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Arm PID.mm.kG", + "type": "double", + "value": 0.375, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Wrist PID.mm.kp", + "type": "double", + "value": 108.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Wrist PID.mm.ki", + "type": "double", + "value": 3.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Wrist PID.mm.kD", + "type": "double", + "value": 3.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Wrist PID.mm.kS", + "type": "double", + "value": 0.375, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Wrist PID.mm.kV", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Wrist PID.mm.kG", + "type": "double", + "value": 0.35, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Arm Max Vel DegsS", + "type": "double", + "value": 120.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Wrist Max Vel DegsS", + "type": "double", + "value": 120.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Acceleration Time Secs", + "type": "double", + "value": 1.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Climber/Left Climber.mm.kp", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Climber/Left Climber.mm.ki", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Climber/Left Climber.mm.kD", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Climber/Left Climber.mm.kS", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Climber/Left Climber.mm.kV", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Climber/Left Climber.mm.kG", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Climber/Right Climber.mm.kp", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Climber/Right Climber.mm.ki", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Climber/Right Climber.mm.kD", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Climber/Right Climber.mm.kS", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Climber/Right Climber.mm.kV", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Climber/Right Climber.mm.kG", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/ArmPoses/Amp IntermediateArm Angle", + "type": "double", + "value": 60.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/ArmPoses/Amp IntermediateWrist Angle", + "type": "double", + "value": 145.0, + "properties": { + "persistent": true + } } ] diff --git a/src/main/deploy/choreo/Test1.1.traj b/src/main/deploy/choreo/Test1.1.traj new file mode 100644 index 00000000..35cbe979 --- /dev/null +++ b/src/main/deploy/choreo/Test1.1.traj @@ -0,0 +1,634 @@ +{ + "samples": [ + { + "x": 1.4869295358657837, + "y": 5.564249038696289, + "heading": 3.141, + "angularVelocity": 8.029589971503427e-22, + "velocityX": 1.7351458241819956e-21, + "velocityY": 6.201477276822387e-22, + "timestamp": 0 + }, + { + "x": 1.506153170031305, + "y": 5.579314508687984, + "heading": 3.1569331073832556, + "angularVelocity": 0.2189381700102546, + "velocityX": 0.2641535755648618, + "velocityY": 0.20701589156378014, + "timestamp": 0.07277446131256805 + }, + { + "x": 1.5445753142190985, + "y": 5.609481391551554, + "heading": 3.1887853931444132, + "angularVelocity": 0.4376849403852184, + "velocityX": 0.5279619181620503, + "velocityY": 0.41452567727023953, + "timestamp": 0.1455489226251361 + }, + { + "x": 1.6021601427736685, + "y": 5.654796415146542, + "heading": 3.2365776272564357, + "angularVelocity": 0.6567171127073494, + "velocityX": 0.7912779774108067, + "velocityY": 0.6226775544288833, + "timestamp": 0.21832338393770417 + }, + { + "x": 1.6788531458253628, + "y": 5.715327275578862, + "heading": 3.3003664786278266, + "angularVelocity": 0.8765279772723583, + "velocityX": 1.0538450119513225, + "velocityY": 0.8317596494783837, + "timestamp": 0.2910978452502722 + }, + { + "x": 1.7745670450726352, + "y": 5.791182757948707, + "heading": 3.380219753358853, + "angularVelocity": 1.097270571169936, + "velocityX": 1.3152127480020601, + "velocityY": 1.0423365697486087, + "timestamp": 0.36387230656284025 + }, + { + "x": 1.8891522077377947, + "y": 5.8825573533612445, + "heading": 3.4761247222373646, + "angularVelocity": 1.317838251891654, + "velocityX": 1.5745243674565124, + "velocityY": 1.255586008669736, + "timestamp": 0.4366467678754083 + }, + { + "x": 2.0223089309237166, + "y": 5.989857408352276, + "heading": 3.5877133538349852, + "angularVelocity": 1.5333487817703626, + "velocityX": 1.829717744168665, + "velocityY": 1.4744190895508729, + "timestamp": 0.5094212291879764 + }, + { + "x": 2.1730917573654285, + "y": 6.114310741146903, + "heading": 3.713064984762661, + "angularVelocity": 1.7224673143135665, + "velocityX": 2.0719195130019052, + "velocityY": 1.7101237240368758, + "timestamp": 0.5821956905005444 + }, + { + "x": 2.304244374220996, + "y": 6.249503988496409, + "heading": 3.8044927424626787, + "angularVelocity": 1.2563165161379026, + "velocityX": 1.8021791503514413, + "velocityY": 1.8577017941616667, + "timestamp": 0.6549701518131125 + }, + { + "x": 2.414916720860036, + "y": 6.372175349075486, + "heading": 3.8745670686179303, + "angularVelocity": 0.9628972154706921, + "velocityX": 1.5207580330096784, + "velocityY": 1.6856374937933305, + "timestamp": 0.7277446131256805 + }, + { + "x": 2.505647288657887, + "y": 6.481209512574969, + "heading": 3.9250064522902046, + "angularVelocity": 0.6930918176863726, + "velocityX": 1.2467363709936818, + "velocityY": 1.4982476205653745, + "timestamp": 0.8005190744382485 + }, + { + "x": 2.576640808380743, + "y": 6.576219510667492, + "heading": 3.9564510372491593, + "angularVelocity": 0.432082689336569, + "velocityX": 0.975527931672858, + "velocityY": 1.305540382971075, + "timestamp": 0.8732935357508166 + }, + { + "x": 2.628005259573955, + "y": 6.657012046349283, + "heading": 3.969203722939461, + "angularVelocity": 0.17523572775796842, + "velocityX": 0.7058032483758355, + "velocityY": 1.1101770349736413, + "timestamp": 0.9460679970633846 + }, + { + "x": 2.6598078437532253, + "y": 6.723475709345355, + "heading": 3.963391945850161, + "angularVelocity": -0.07986011829532402, + "velocityX": 0.4370019867639682, + "velocityY": 0.9132827890076579, + "timestamp": 1.0188424583759528 + }, + { + "x": 2.6720941279928496, + "y": 6.775543377099927, + "heading": 3.9390315825966313, + "angularVelocity": -0.3347378024400806, + "velocityX": 0.16882686615644266, + "velocityY": 0.7154662063514367, + "timestamp": 1.0916169196885208 + }, + { + "x": 2.6648950576782227, + "y": 6.813176155090332, + "heading": 3.896073037200959, + "angularVelocity": -0.5902969890929726, + "velocityX": -0.09892303130498589, + "velocityY": 0.5171151707845827, + "timestamp": 1.1643913810010889 + }, + { + "x": 2.659637231596543, + "y": 6.825031823463049, + "heading": 3.877816032306183, + "angularVelocity": -0.6840416135896586, + "velocityX": -0.19699681615987222, + "velocityY": 0.44420049020077496, + "timestamp": 1.191081284849143 + }, + { + "x": 2.6517927610129908, + "y": 6.834893701518374, + "heading": 3.857106562184998, + "angularVelocity": -0.775928989444263, + "velocityX": -0.29391153404714393, + "velocityY": 0.36949844823228756, + "timestamp": 1.2177711886971971 + }, + { + "x": 2.641397527843252, + "y": 6.842708311525597, + "heading": 3.834005474054525, + "angularVelocity": -0.865536581247652, + "velocityX": -0.38948185159896215, + "velocityY": 0.2927927373478789, + "timestamp": 1.2444610925452513 + }, + { + "x": 2.628493660297315, + "y": 6.848415370045661, + "heading": 3.808587288042731, + "angularVelocity": -0.9523521012476956, + "velocityX": -0.4834737367133058, + "velocityY": 0.2138283656829084, + "timestamp": 1.2711509963933054 + }, + { + "x": 2.6131312860336138, + "y": 6.85194659507741, + "heading": 3.7809426101756807, + "angularVelocity": -1.0357728534516963, + "velocityX": -0.5755874712460319, + "velocityY": 0.13230564830254915, + "timestamp": 1.2978409002413596 + }, + { + "x": 2.5953708985643273, + "y": 6.853224371337149, + "heading": 3.7511803744930092, + "angularVelocity": -1.1151121357389708, + "velocityX": -0.6654346741148351, + "velocityY": 0.0478748918322195, + "timestamp": 1.3245308040894137 + }, + { + "x": 2.5752865454465885, + "y": 6.852160328589509, + "heading": 3.7194297145938124, + "angularVelocity": -1.1896131241218921, + "velocityX": -0.752507511157751, + "velocityY": -0.03986686327897337, + "timestamp": 1.3512207079374678 + }, + { + "x": 2.5529701073140734, + "y": 6.848653945190506, + "heading": 3.6858413880737997, + "angularVelocity": -1.2584656247257915, + "velocityX": -0.8361378242335784, + "velocityY": -0.1313748981249787, + "timestamp": 1.377910611785522 + }, + { + "x": 2.5285370169793717, + "y": 6.842591376045072, + "heading": 3.650589102058915, + "angularVelocity": -1.3208097794422922, + "velocityX": -0.9154431755842751, + "velocityY": -0.2271484071260898, + "timestamp": 1.4046005156335761 + }, + { + "x": 2.502133887928051, + "y": 6.833844846711091, + "heading": 3.613872024847483, + "angularVelocity": -1.375691625584805, + "velocityX": -0.9892553079859069, + "velocityY": -0.32770928601974636, + "timestamp": 1.4312904194816303 + }, + { + "x": 2.4739487064187284, + "y": 6.822273243464229, + "heading": 3.5759212160148097, + "angularVelocity": -1.4219162814796094, + "velocityX": -1.0560240932219476, + "velocityY": -0.43355732237695227, + "timestamp": 1.4579803233296844 + }, + { + "x": 2.4442244144939402, + "y": 6.807725217915028, + "heading": 3.5370136666138436, + "angularVelocity": -1.4577628163243674, + "velocityX": -1.113690483637882, + "velocityY": -0.54507598199017, + "timestamp": 1.4846702271777386 + }, + { + "x": 2.4132762941421024, + "y": 6.79004764808573, + "heading": 3.497494895227568, + "angularVelocity": -1.4806636850869153, + "velocityX": -1.1595440930782523, + "velocityY": -0.6623317165148433, + "timestamp": 1.5113601310257927 + }, + { + "x": 2.3815108616317744, + "y": 6.769104478274178, + "heading": 3.45780088591702, + "angularVelocity": -1.4872293859328474, + "velocityX": -1.1901666147307672, + "velocityY": -0.7846850978100616, + "timestamp": 1.5380500348738468 + }, + { + "x": 2.3494374003667597, + "y": 6.744810785756249, + "heading": 3.4184626098399313, + "angularVelocity": -1.4739010039542162, + "velocityX": -1.2017076362510986, + "velocityY": -0.910220308631834, + "timestamp": 1.564739938721901 + }, + { + "x": 2.317656105280672, + "y": 6.717179467626764, + "heading": 3.3800987305060795, + "angularVelocity": -1.4373929390026206, + "velocityX": -1.190760943427104, + "velocityY": -1.0352722994728694, + "timestamp": 1.5914298425699551 + }, + { + "x": 2.2868057964945567, + "y": 6.686361396326711, + "heading": 3.343389515038899, + "angularVelocity": -1.3753970668521929, + "velocityX": -1.1558793527974576, + "velocityY": -1.1546714995865741, + "timestamp": 1.6181197464180093 + }, + { + "x": 2.25748075399746, + "y": 6.652639268574606, + "heading": 3.308925014353095, + "angularVelocity": -1.291293549876024, + "velocityX": -1.0987316651286732, + "velocityY": -1.263478802474685, + "timestamp": 1.6448096502660634 + }, + { + "x": 2.2301787041350396, + "y": 6.616374519308364, + "heading": 3.2770382136187797, + "angularVelocity": -1.194713960599006, + "velocityX": -1.022935489683713, + "velocityY": -1.3587440956212415, + "timestamp": 1.6714995541141175 + }, + { + "x": 2.2052911587380817, + "y": 6.577965167741144, + "heading": 3.2479010535236887, + "angularVelocity": -1.0916922091952401, + "velocityX": -0.9324704029899484, + "velocityY": -1.4390966631383846, + "timestamp": 1.6981894579621717 + }, + { + "x": 2.183095873519221, + "y": 6.537812097974725, + "heading": 3.2217255041499464, + "angularVelocity": -0.9807285002883435, + "velocityX": -0.8315985454731583, + "velocityY": -1.504429165238335, + "timestamp": 1.7248793618102258 + }, + { + "x": 2.163764936777705, + "y": 6.4962772278935255, + "heading": 3.198795874429834, + "angularVelocity": -0.8591124887766933, + "velocityX": -0.72427899521733, + "velocityY": -1.5562015628702863, + "timestamp": 1.75156926565828 + }, + { + "x": 2.1473967460074728, + "y": 6.453660043976164, + "heading": 3.1793849811459, + "angularVelocity": -0.7272747550699427, + "velocityX": -0.61327275150245, + "velocityY": -1.596752995439064, + "timestamp": 1.7782591695063341 + }, + { + "x": 2.134047988560311, + "y": 6.410199882298709, + "heading": 3.163697789085614, + "angularVelocity": -0.5877575336948715, + "velocityX": -0.5001425828716374, + "velocityY": -1.628337139199698, + "timestamp": 1.8049490733543883 + }, + { + "x": 2.1237534679344243, + "y": 6.366088523528368, + "heading": 3.151864532890396, + "angularVelocity": -0.443360765276075, + "velocityX": -0.38570841935190137, + "velocityY": -1.6527357693556206, + "timestamp": 1.8316389772024424 + }, + { + "x": 2.116536197487218, + "y": 6.321482513089734, + "heading": 3.143955647543306, + "angularVelocity": -0.29632498461271933, + "velocityX": -0.27041200628876155, + "velocityY": -1.671269057115243, + "timestamp": 1.8583288810504965 + }, + { + "x": 2.1124122616124033, + "y": 6.276512434450225, + "heading": 3.14, + "angularVelocity": -0.1482076355848078, + "velocityX": -0.15451295359818015, + "velocityY": -1.6849097282449244, + "timestamp": 1.8850187848985507 + }, + { + "x": 2.1113932132720947, + "y": 6.231289386749268, + "heading": 3.14, + "angularVelocity": 1.1480104978612554e-22, + "velocityX": -0.038181042019109025, + "velocityY": -1.6943878089038094, + "timestamp": 1.9117086887466048 + }, + { + "x": 2.1139722457961088, + "y": 6.184041017313733, + "heading": 3.14, + "angularVelocity": 1.326030822415817e-23, + "velocityX": 0.09287464315336291, + "velocityY": -1.7014812376514836, + "timestamp": 1.9394776516890055 + }, + { + "x": 2.1201947185383916, + "y": 6.136700429098736, + "heading": 3.14, + "angularVelocity": 1.3260179507784046e-23, + "velocityX": 0.224080126981686, + "velocityY": -1.7048021675563763, + "timestamp": 1.9672466146314063 + }, + { + "x": 2.13006161842343, + "y": 6.089396035622716, + "heading": 3.14, + "angularVelocity": 1.326007041519161e-23, + "velocityX": 0.35532115136976367, + "velocityY": -1.7034987433322955, + "timestamp": 1.995015577573807 + }, + { + "x": 2.1435678181376194, + "y": 6.042288248235824, + "heading": 3.14, + "angularVelocity": 1.3260437690465338e-23, + "velocityX": 0.4863775338749469, + "velocityY": -1.6964186773774967, + "timestamp": 2.022784540516208 + }, + { + "x": 2.1606964661173613, + "y": 5.995581737791835, + "heading": 3.14, + "angularVelocity": 1.325999769616951e-23, + "velocityX": 0.6168270675167464, + "velocityY": -1.6819681217793025, + "timestamp": 2.050553503458609 + }, + { + "x": 2.181407907708738, + "y": 5.949543614413858, + "heading": 3.14, + "angularVelocity": 1.3260185229673763e-23, + "velocityX": 0.7458485804578618, + "velocityY": -1.6578985493073877, + "timestamp": 2.07832246640101 + }, + { + "x": 2.2056170475643233, + "y": 5.904529822011414, + "heading": 3.14, + "angularVelocity": 1.3260157963898354e-23, + "velocityX": 0.871805688451557, + "velocityY": -1.6210109284892367, + "timestamp": 2.1060914293434108 + }, + { + "x": 2.2331463169007804, + "y": 5.861019319967013, + "heading": 3.14, + "angularVelocity": 1.326020172336188e-23, + "velocityX": 0.9913682910506778, + "velocityY": -1.566875296519094, + "timestamp": 2.1338603922858117 + }, + { + "x": 2.2636325105277852, + "y": 5.819639483049468, + "heading": 3.14, + "angularVelocity": 1.32601511090322e-23, + "velocityX": 1.0978513562152263, + "velocityY": -1.4901470034504147, + "timestamp": 2.1616293552282126 + }, + { + "x": 2.2963834267173127, + "y": 5.781115195207252, + "heading": 3.14, + "angularVelocity": 1.3260107576996925e-23, + "velocityX": 1.1794072489296905, + "velocityY": -1.387314604514559, + "timestamp": 2.1893983181706136 + }, + { + "x": 2.3303148904385163, + "y": 5.746039013141612, + "heading": 3.14, + "angularVelocity": 1.3260240855176863e-23, + "velocityX": 1.221920450957617, + "velocityY": -1.2631433928013844, + "timestamp": 2.2171672811130145 + }, + { + "x": 2.3642062447518297, + "y": 5.714607213055508, + "heading": 3.14, + "angularVelocity": 1.326023555417506e-23, + "velocityX": 1.2204760539171806, + "velocityY": -1.1319039948052778, + "timestamp": 2.2449362440554155 + }, + { + "x": 2.397064742551735, + "y": 5.68667060145179, + "heading": 3.14, + "angularVelocity": 1.3260077501265654e-23, + "velocityX": 1.1832814163085987, + "velocityY": -1.0060372676381828, + "timestamp": 2.2727052069978164 + }, + { + "x": 2.4282178000518853, + "y": 5.661954954768778, + "heading": 3.14, + "angularVelocity": 1.3260249833501151e-23, + "velocityX": 1.1218660763374204, + "velocityY": -0.8900457224231765, + "timestamp": 2.3004741699402174 + }, + { + "x": 2.457231034537111, + "y": 5.640189613922927, + "heading": 3.14, + "angularVelocity": 1.3260141947431602e-23, + "velocityX": 1.0448079946451803, + "velocityY": -0.7838009972139435, + "timestamp": 2.3282431328826183 + }, + { + "x": 2.483818861573163, + "y": 5.621144668272307, + "heading": 3.14, + "angularVelocity": 1.3260165607649488e-23, + "velocityX": 0.9574656097601072, + "velocityY": -0.6858356824532662, + "timestamp": 2.3560120958250192 + }, + { + "x": 2.507786212179532, + "y": 5.604632755958765, + "heading": 3.14, + "angularVelocity": 1.3260126001429812e-23, + "velocityX": 0.8630985123961222, + "velocityY": -0.5946175356923091, + "timestamp": 2.38378105876742 + }, + { + "x": 2.5289943781295285, + "y": 5.590502031840209, + "heading": 3.14, + "angularVelocity": 1.3260018717887446e-23, + "velocityX": 0.763736333761809, + "velocityY": -0.5088675492802951, + "timestamp": 2.411550021709821 + }, + { + "x": 2.547341071809568, + "y": 5.578628548447832, + "heading": 3.14, + "angularVelocity": 1.3260377381509874e-23, + "velocityX": 0.6606906321310845, + "velocityY": -0.4275810881740915, + "timestamp": 2.439318984652222 + }, + { + "x": 2.562748457639991, + "y": 5.568909973472095, + "heading": 3.14, + "angularVelocity": 1.3260175762620676e-23, + "velocityX": 0.5548419601546171, + "velocityY": -0.3499797596293578, + "timestamp": 2.467087947594623 + }, + { + "x": 2.575155705150767, + "y": 5.561260778200535, + "heading": 3.14, + "angularVelocity": 1.3260285574981282e-23, + "velocityX": 0.4468026961075851, + "velocityY": -0.2754584421256751, + "timestamp": 2.494856910537024 + }, + { + "x": 2.5845141850851863, + "y": 5.555608625671519, + "heading": 3.14, + "angularVelocity": 1.326006595026007e-23, + "velocityX": 0.33701222310069334, + "velocityY": -0.2035420818825477, + "timestamp": 2.522625873479425 + }, + { + "x": 2.5907842648688533, + "y": 5.55189166019714, + "heading": 3.14, + "angularVelocity": 1.3260302513246253e-23, + "velocityX": 0.22579452450827125, + "velocityY": -0.1338532332694335, + "timestamp": 2.550394836421826 + }, + { + "x": 2.59393310546875, + "y": 5.550056457519531, + "heading": 3.14, + "angularVelocity": 1.3260089047889746e-23, + "velocityX": 0.11339424545409957, + "velocityY": -0.06608826845335893, + "timestamp": 2.578163799364227 + }, + { + "x": 2.59393310546875, + "y": 5.550056457519531, + "heading": 3.14, + "angularVelocity": 4.554749859713047e-24, + "velocityX": -4.596278509340535e-25, + "velocityY": -1.5195442763313795e-23, + "timestamp": 2.6059327623066277 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Test1.2.traj b/src/main/deploy/choreo/Test1.2.traj new file mode 100644 index 00000000..714f9e0e --- /dev/null +++ b/src/main/deploy/choreo/Test1.2.traj @@ -0,0 +1,445 @@ +{ + "samples": [ + { + "x": 2.59393310546875, + "y": 5.550056457519531, + "heading": 3.14, + "angularVelocity": 4.554749859713047e-24, + "velocityX": -4.596278509340535e-25, + "velocityY": -1.5195442763313795e-23, + "timestamp": 0 + }, + { + "x": 2.5899783566862378, + "y": 5.5489813066194555, + "heading": 3.1367490851985087, + "angularVelocity": -0.1085265960704612, + "velocityX": -0.13202296888338985, + "velocityY": -0.03589219483504766, + "timestamp": 0.029955005677878788 + }, + { + "x": 2.5820812048170954, + "y": 5.546779098674632, + "heading": 3.130275233897137, + "angularVelocity": -0.21611918124764368, + "velocityX": -0.26363379643671836, + "velocityY": -0.07351719337009978, + "timestamp": 0.059910011355757575 + }, + { + "x": 2.5702568313825034, + "y": 5.5433888083370855, + "heading": 3.1206149262957497, + "angularVelocity": -0.32249393324338926, + "velocityX": -0.3947378131636905, + "velocityY": -0.1131794256360211, + "timestamp": 0.08986501703363636 + }, + { + "x": 2.554524436678559, + "y": 5.5387376635070815, + "heading": 3.107814789143658, + "angularVelocity": -0.42731212571741595, + "velocityX": -0.5252008586852795, + "velocityY": -0.15527103817038457, + "timestamp": 0.11982002271151515 + }, + { + "x": 2.5349090755324424, + "y": 5.532737501107672, + "heading": 3.0919335422287366, + "angularVelocity": -0.5301700519005096, + "velocityX": -0.6548274888361012, + "velocityY": -0.20030583415450579, + "timestamp": 0.14977502838939394 + }, + { + "x": 2.511444606178007, + "y": 5.52527960958652, + "heading": 3.0730442108839475, + "angularVelocity": -0.6305901440285391, + "velocityX": -0.7833238159511905, + "velocityY": -0.24896979160511046, + "timestamp": 0.17973003406727273 + }, + { + "x": 2.4841786133457258, + "y": 5.5162272720643095, + "heading": 3.0512365155339536, + "angularVelocity": -0.7280150631418102, + "velocityX": -0.9102316028741883, + "velocityY": -0.3021978236143635, + "timestamp": 0.2096850397451515 + }, + { + "x": 2.4531809940870857, + "y": 5.505404760057258, + "heading": 3.026619340133933, + "angularVelocity": -0.8218050653951327, + "velocityX": -1.03480598842053, + "velocityY": -0.3612922702613114, + "timestamp": 0.2396400454230303 + }, + { + "x": 2.4185596883958564, + "y": 5.492580836854246, + "heading": 2.999323661656212, + "angularVelocity": -0.9112226107130541, + "velocityX": -1.1557769697502154, + "velocityY": -0.4281061850200961, + "timestamp": 0.2695950511009091 + }, + { + "x": 2.380491125275952, + "y": 5.477444069032361, + "heading": 2.9695096535475827, + "angularVelocity": -0.9952930214480606, + "velocityX": -1.27085815069692, + "velocityY": -0.5053168069690293, + "timestamp": 0.2995500567787879 + }, + { + "x": 2.3392828051052046, + "y": 5.459567907623698, + "heading": 2.9373981254397274, + "angularVelocity": -1.0719920554570026, + "velocityX": -1.3756739228789079, + "velocityY": -0.5967670846367005, + "timestamp": 0.32950506245666666 + }, + { + "x": 2.2955095509891357, + "y": 5.438376261665861, + "heading": 2.9034010779477137, + "angularVelocity": -1.1349371072601777, + "velocityX": -1.4613001441823985, + "velocityY": -0.7074492385586875, + "timestamp": 0.35946006813454545 + }, + { + "x": 2.2502877957375587, + "y": 5.413212847808716, + "heading": 2.8684201412995205, + "angularVelocity": -1.1677826746007212, + "velocityX": -1.5096560400578645, + "velocityY": -0.8400403634617765, + "timestamp": 0.38941507381242424 + }, + { + "x": 2.205434968136323, + "y": 5.383827174903616, + "heading": 2.8338778225381454, + "angularVelocity": -1.15314011730883, + "velocityX": -1.4973399799539564, + "velocityY": -0.9809937351072376, + "timestamp": 0.419370079490303 + }, + { + "x": 2.1627117665288242, + "y": 5.35082401983714, + "heading": 2.8010931886625263, + "angularVelocity": -1.0944626159703914, + "velocityX": -1.426245819043514, + "velocityY": -1.1017575967561364, + "timestamp": 0.4493250851681818 + }, + { + "x": 2.1231203758060415, + "y": 5.315102619215835, + "heading": 2.770927863402835, + "angularVelocity": -1.0070211831729985, + "velocityX": -1.3216953169206167, + "velocityY": -1.1925018811692434, + "timestamp": 0.4792800908460606 + }, + { + "x": 2.0871120973907478, + "y": 5.277368588566571, + "heading": 2.743899688375806, + "angularVelocity": -0.9022924354505801, + "velocityX": -1.2020788379247493, + "velocityY": -1.2596903187079023, + "timestamp": 0.5092350965239394 + }, + { + "x": 2.0548974336097707, + "y": 5.238102066889186, + "heading": 2.7202319811811844, + "angularVelocity": -0.7901085864957776, + "velocityX": -1.0754350751055555, + "velocityY": -1.310850082942305, + "timestamp": 0.5391901022018182 + }, + { + "x": 2.026587153392112, + "y": 5.197634903381892, + "heading": 2.6999923980627787, + "angularVelocity": -0.6756661419481035, + "velocityX": -0.9450934685873016, + "velocityY": -1.350931591950202, + "timestamp": 0.569145107879697 + }, + { + "x": 2.002245678733676, + "y": 5.1562068665429, + "heading": 2.6831832138201195, + "angularVelocity": -0.5611477568529534, + "velocityX": -0.812601236674494, + "velocityY": -1.3830088127670148, + "timestamp": 0.5991001135575758 + }, + { + "x": 1.9819131544340292, + "y": 5.113998389763002, + "heading": 2.66978325115483, + "angularVelocity": -0.44733634202529504, + "velocityX": -0.6787688347748394, + "velocityY": -1.4090625531434064, + "timestamp": 0.6290551192354545 + }, + { + "x": 1.9656157831702816, + "y": 5.071149791653941, + "heading": 2.659765787935313, + "angularVelocity": -0.33441700286222414, + "velocityX": -0.5440616983685925, + "velocityY": -1.4304319808793762, + "timestamp": 0.6590101249133333 + }, + { + "x": 1.9533711923999129, + "y": 5.027773084636261, + "heading": 2.6531059130951715, + "angularVelocity": -0.22232927984587103, + "velocityX": -0.4087660974610013, + "velocityY": -1.4480620529380386, + "timestamp": 0.6889651305912121 + }, + { + "x": 1.9451914498185667, + "y": 4.983959581247479, + "heading": 2.649783348569472, + "angularVelocity": -0.11091850762536054, + "velocityX": -0.27306763581709725, + "velocityY": -1.4626438018383596, + "timestamp": 0.7189201362690909 + }, + { + "x": 1.941084861755371, + "y": 4.939785003662109, + "heading": 2.649783348569472, + "angularVelocity": 6.132292113202646e-25, + "velocityX": -0.13709188064779135, + "velocityY": -1.4746976869375825, + "timestamp": 0.7488751419469697 + }, + { + "x": 1.9419022015392158, + "y": 4.889062683793002, + "heading": 2.649783348569472, + "angularVelocity": 4.614715107153485e-26, + "velocityX": 0.023936675164732843, + "velocityY": -1.4854577230996109, + "timestamp": 0.7830210614708646 + }, + { + "x": 1.948223657679148, + "y": 4.838070468392074, + "heading": 2.649783348569472, + "angularVelocity": 4.653945144562686e-26, + "velocityX": 0.18513064600613907, + "velocityY": -1.4933619042018718, + "timestamp": 0.8171669809947595 + }, + { + "x": 1.9600538072506943, + "y": 4.786928471515363, + "heading": 2.649783348569472, + "angularVelocity": 4.629261284078221e-26, + "velocityX": 0.3464586614300294, + "velocityY": -1.4977484159101515, + "timestamp": 0.8513129005186544 + }, + { + "x": 1.977394685635232, + "y": 4.735788039854744, + "heading": 2.649783348569472, + "angularVelocity": 4.640038416252255e-26, + "velocityX": 0.5078462851879747, + "velocityY": -1.4977025768725385, + "timestamp": 0.8854588200425493 + }, + { + "x": 2.0002427480647933, + "y": 4.684845229382865, + "heading": 2.649783348569472, + "angularVelocity": 4.6490332666751425e-26, + "velocityX": 0.6691300966012094, + "velocityY": -1.4919150276866968, + "timestamp": 0.9196047395664442 + }, + { + "x": 2.0285822715254445, + "y": 4.634362832128866, + "heading": 2.649783348569472, + "angularVelocity": 4.622795940866032e-26, + "velocityX": 0.8299534426308102, + "velocityY": -1.478431331119124, + "timestamp": 0.9537506590903391 + }, + { + "x": 2.0623699922411927, + "y": 4.5847082412928, + "heading": 2.649783348569472, + "angularVelocity": 4.634016030607817e-26, + "velocityX": 0.9895097624213414, + "velocityY": -1.454188129311252, + "timestamp": 0.987896578614234 + }, + { + "x": 2.101495965027088, + "y": 4.536421557771227, + "heading": 2.649783348569472, + "angularVelocity": 4.645177261974479e-26, + "velocityX": 1.1458462191511727, + "velocityY": -1.4141274915083855, + "timestamp": 1.0220424981381289 + }, + { + "x": 2.145673252803998, + "y": 4.4903376692314145, + "heading": 2.649783348569472, + "angularVelocity": 4.6233288536561975e-26, + "velocityX": 1.2937794147261192, + "velocityY": -1.3496162698902718, + "timestamp": 1.0561884176620238 + }, + { + "x": 2.1941116699379357, + "y": 4.447748182132556, + "heading": 2.649783348569472, + "angularVelocity": 4.6479494614855375e-26, + "velocityX": 1.4185711736373225, + "velocityY": -1.2472789631292271, + "timestamp": 1.0903343371859187 + }, + { + "x": 2.2448201323304646, + "y": 4.410180142680831, + "heading": 2.649783348569472, + "angularVelocity": 4.6329904581670297e-26, + "velocityX": 1.485051891985029, + "velocityY": -1.1002204648621148, + "timestamp": 1.1244802567098136 + }, + { + "x": 2.294852293597042, + "y": 4.3780811675391975, + "heading": 2.649783348569472, + "angularVelocity": 4.6446166642422895e-26, + "velocityX": 1.4652456856979683, + "velocityY": -0.9400530309096446, + "timestamp": 1.1586261762337084 + }, + { + "x": 2.342149827765447, + "y": 4.350766530180341, + "heading": 2.649783348569472, + "angularVelocity": 4.633506801188826e-26, + "velocityX": 1.3851591882100787, + "velocityY": -0.7999385501902068, + "timestamp": 1.1927720957576033 + }, + { + "x": 2.3856944951859207, + "y": 4.3274872473595005, + "heading": 2.649783348569472, + "angularVelocity": 4.6381778689211406e-26, + "velocityX": 1.2752524467821442, + "velocityY": -0.6817588498253565, + "timestamp": 1.2269180152814982 + }, + { + "x": 2.4249725857215654, + "y": 4.307695719032298, + "heading": 2.649783348569472, + "angularVelocity": 4.6432377599415885e-26, + "velocityX": 1.150301151156826, + "velocityY": -0.5796162060697082, + "timestamp": 1.2610639348053931 + }, + { + "x": 2.4596964858202144, + "y": 4.291007007021927, + "heading": 2.649783348569472, + "angularVelocity": 4.6331179794053255e-26, + "velocityX": 1.0169267831358226, + "velocityY": -0.4887468910800007, + "timestamp": 1.295209854329288 + }, + { + "x": 2.4896890256889828, + "y": 4.277143601542489, + "heading": 2.649783348569472, + "angularVelocity": 4.6467164999430504e-26, + "velocityX": 0.8783638070657234, + "velocityY": -0.4060047488173824, + "timestamp": 1.329355773853183 + }, + { + "x": 2.514832571254234, + "y": 4.265898475268687, + "heading": 2.649783348569472, + "angularVelocity": 4.632856845272505e-26, + "velocityX": 0.7363557905551701, + "velocityY": -0.3293256245723021, + "timestamp": 1.3635016933770778 + }, + { + "x": 2.5350443768209585, + "y": 4.257112226026017, + "heading": 2.649783348569472, + "angularVelocity": 4.629729705599566e-26, + "velocityX": 0.5919244773180152, + "velocityY": -0.2573147645510306, + "timestamp": 1.3976476129009727 + }, + { + "x": 2.550263563230007, + "y": 4.2506587632470065, + "heading": 2.649783348569472, + "angularVelocity": 4.62810585566047e-26, + "velocityX": 0.445710252388941, + "velocityY": -0.18899660249284694, + "timestamp": 1.4317935324248676 + }, + { + "x": 2.5604437164785643, + "y": 4.246436059535304, + "heading": 2.649783348569472, + "angularVelocity": 4.6519462435401186e-26, + "velocityX": 0.29813674343821717, + "velocityY": -0.12366642253542871, + "timestamp": 1.4659394519487625 + }, + { + "x": 2.5655484199523926, + "y": 4.244359970092773, + "heading": 2.649783348569472, + "angularVelocity": 4.6699948971014915e-26, + "velocityX": 0.14949673474910496, + "velocityY": -0.06080051354533503, + "timestamp": 1.5000853714726574 + }, + { + "x": 2.5655484199523926, + "y": 4.244359970092773, + "heading": 2.649783348569472, + "angularVelocity": 1.535197914331468e-26, + "velocityX": 1.1963425727073795e-24, + "velocityY": -7.168329076335369e-24, + "timestamp": 1.5342312909965523 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Test1.traj b/src/main/deploy/choreo/Test1.traj new file mode 100644 index 00000000..e709ba2f --- /dev/null +++ b/src/main/deploy/choreo/Test1.traj @@ -0,0 +1,1066 @@ +{ + "samples": [ + { + "x": 1.4869295358657837, + "y": 5.564249038696289, + "heading": 3.141, + "angularVelocity": 8.029589971503427e-22, + "velocityX": 1.7351458241819956e-21, + "velocityY": 6.201477276822387e-22, + "timestamp": 0 + }, + { + "x": 1.506153170031305, + "y": 5.579314508687984, + "heading": 3.1569331073832556, + "angularVelocity": 0.2189381700102546, + "velocityX": 0.2641535755648618, + "velocityY": 0.20701589156378014, + "timestamp": 0.07277446131256805 + }, + { + "x": 1.5445753142190985, + "y": 5.609481391551554, + "heading": 3.1887853931444132, + "angularVelocity": 0.4376849403852184, + "velocityX": 0.5279619181620503, + "velocityY": 0.41452567727023953, + "timestamp": 0.1455489226251361 + }, + { + "x": 1.6021601427736685, + "y": 5.654796415146542, + "heading": 3.2365776272564357, + "angularVelocity": 0.6567171127073494, + "velocityX": 0.7912779774108067, + "velocityY": 0.6226775544288833, + "timestamp": 0.21832338393770417 + }, + { + "x": 1.6788531458253628, + "y": 5.715327275578862, + "heading": 3.3003664786278266, + "angularVelocity": 0.8765279772723583, + "velocityX": 1.0538450119513225, + "velocityY": 0.8317596494783837, + "timestamp": 0.2910978452502722 + }, + { + "x": 1.7745670450726352, + "y": 5.791182757948707, + "heading": 3.380219753358853, + "angularVelocity": 1.097270571169936, + "velocityX": 1.3152127480020601, + "velocityY": 1.0423365697486087, + "timestamp": 0.36387230656284025 + }, + { + "x": 1.8891522077377947, + "y": 5.8825573533612445, + "heading": 3.4761247222373646, + "angularVelocity": 1.317838251891654, + "velocityX": 1.5745243674565124, + "velocityY": 1.255586008669736, + "timestamp": 0.4366467678754083 + }, + { + "x": 2.0223089309237166, + "y": 5.989857408352276, + "heading": 3.5877133538349852, + "angularVelocity": 1.5333487817703626, + "velocityX": 1.829717744168665, + "velocityY": 1.4744190895508729, + "timestamp": 0.5094212291879764 + }, + { + "x": 2.1730917573654285, + "y": 6.114310741146903, + "heading": 3.713064984762661, + "angularVelocity": 1.7224673143135665, + "velocityX": 2.0719195130019052, + "velocityY": 1.7101237240368758, + "timestamp": 0.5821956905005444 + }, + { + "x": 2.304244374220996, + "y": 6.249503988496409, + "heading": 3.8044927424626787, + "angularVelocity": 1.2563165161379026, + "velocityX": 1.8021791503514413, + "velocityY": 1.8577017941616667, + "timestamp": 0.6549701518131125 + }, + { + "x": 2.414916720860036, + "y": 6.372175349075486, + "heading": 3.8745670686179303, + "angularVelocity": 0.9628972154706921, + "velocityX": 1.5207580330096784, + "velocityY": 1.6856374937933305, + "timestamp": 0.7277446131256805 + }, + { + "x": 2.505647288657887, + "y": 6.481209512574969, + "heading": 3.9250064522902046, + "angularVelocity": 0.6930918176863726, + "velocityX": 1.2467363709936818, + "velocityY": 1.4982476205653745, + "timestamp": 0.8005190744382485 + }, + { + "x": 2.576640808380743, + "y": 6.576219510667492, + "heading": 3.9564510372491593, + "angularVelocity": 0.432082689336569, + "velocityX": 0.975527931672858, + "velocityY": 1.305540382971075, + "timestamp": 0.8732935357508166 + }, + { + "x": 2.628005259573955, + "y": 6.657012046349283, + "heading": 3.969203722939461, + "angularVelocity": 0.17523572775796842, + "velocityX": 0.7058032483758355, + "velocityY": 1.1101770349736413, + "timestamp": 0.9460679970633846 + }, + { + "x": 2.6598078437532253, + "y": 6.723475709345355, + "heading": 3.963391945850161, + "angularVelocity": -0.07986011829532402, + "velocityX": 0.4370019867639682, + "velocityY": 0.9132827890076579, + "timestamp": 1.0188424583759528 + }, + { + "x": 2.6720941279928496, + "y": 6.775543377099927, + "heading": 3.9390315825966313, + "angularVelocity": -0.3347378024400806, + "velocityX": 0.16882686615644266, + "velocityY": 0.7154662063514367, + "timestamp": 1.0916169196885208 + }, + { + "x": 2.6648950576782227, + "y": 6.813176155090332, + "heading": 3.896073037200959, + "angularVelocity": -0.5902969890929726, + "velocityX": -0.09892303130498589, + "velocityY": 0.5171151707845827, + "timestamp": 1.1643913810010889 + }, + { + "x": 2.659637231596543, + "y": 6.825031823463049, + "heading": 3.877816032306183, + "angularVelocity": -0.6840416135896586, + "velocityX": -0.19699681615987222, + "velocityY": 0.44420049020077496, + "timestamp": 1.191081284849143 + }, + { + "x": 2.6517927610129908, + "y": 6.834893701518374, + "heading": 3.857106562184998, + "angularVelocity": -0.775928989444263, + "velocityX": -0.29391153404714393, + "velocityY": 0.36949844823228756, + "timestamp": 1.2177711886971971 + }, + { + "x": 2.641397527843252, + "y": 6.842708311525597, + "heading": 3.834005474054525, + "angularVelocity": -0.865536581247652, + "velocityX": -0.38948185159896215, + "velocityY": 0.2927927373478789, + "timestamp": 1.2444610925452513 + }, + { + "x": 2.628493660297315, + "y": 6.848415370045661, + "heading": 3.808587288042731, + "angularVelocity": -0.9523521012476956, + "velocityX": -0.4834737367133058, + "velocityY": 0.2138283656829084, + "timestamp": 1.2711509963933054 + }, + { + "x": 2.6131312860336138, + "y": 6.85194659507741, + "heading": 3.7809426101756807, + "angularVelocity": -1.0357728534516963, + "velocityX": -0.5755874712460319, + "velocityY": 0.13230564830254915, + "timestamp": 1.2978409002413596 + }, + { + "x": 2.5953708985643273, + "y": 6.853224371337149, + "heading": 3.7511803744930092, + "angularVelocity": -1.1151121357389708, + "velocityX": -0.6654346741148351, + "velocityY": 0.0478748918322195, + "timestamp": 1.3245308040894137 + }, + { + "x": 2.5752865454465885, + "y": 6.852160328589509, + "heading": 3.7194297145938124, + "angularVelocity": -1.1896131241218921, + "velocityX": -0.752507511157751, + "velocityY": -0.03986686327897337, + "timestamp": 1.3512207079374678 + }, + { + "x": 2.5529701073140734, + "y": 6.848653945190506, + "heading": 3.6858413880737997, + "angularVelocity": -1.2584656247257915, + "velocityX": -0.8361378242335784, + "velocityY": -0.1313748981249787, + "timestamp": 1.377910611785522 + }, + { + "x": 2.5285370169793717, + "y": 6.842591376045072, + "heading": 3.650589102058915, + "angularVelocity": -1.3208097794422922, + "velocityX": -0.9154431755842751, + "velocityY": -0.2271484071260898, + "timestamp": 1.4046005156335761 + }, + { + "x": 2.502133887928051, + "y": 6.833844846711091, + "heading": 3.613872024847483, + "angularVelocity": -1.375691625584805, + "velocityX": -0.9892553079859069, + "velocityY": -0.32770928601974636, + "timestamp": 1.4312904194816303 + }, + { + "x": 2.4739487064187284, + "y": 6.822273243464229, + "heading": 3.5759212160148097, + "angularVelocity": -1.4219162814796094, + "velocityX": -1.0560240932219476, + "velocityY": -0.43355732237695227, + "timestamp": 1.4579803233296844 + }, + { + "x": 2.4442244144939402, + "y": 6.807725217915028, + "heading": 3.5370136666138436, + "angularVelocity": -1.4577628163243674, + "velocityX": -1.113690483637882, + "velocityY": -0.54507598199017, + "timestamp": 1.4846702271777386 + }, + { + "x": 2.4132762941421024, + "y": 6.79004764808573, + "heading": 3.497494895227568, + "angularVelocity": -1.4806636850869153, + "velocityX": -1.1595440930782523, + "velocityY": -0.6623317165148433, + "timestamp": 1.5113601310257927 + }, + { + "x": 2.3815108616317744, + "y": 6.769104478274178, + "heading": 3.45780088591702, + "angularVelocity": -1.4872293859328474, + "velocityX": -1.1901666147307672, + "velocityY": -0.7846850978100616, + "timestamp": 1.5380500348738468 + }, + { + "x": 2.3494374003667597, + "y": 6.744810785756249, + "heading": 3.4184626098399313, + "angularVelocity": -1.4739010039542162, + "velocityX": -1.2017076362510986, + "velocityY": -0.910220308631834, + "timestamp": 1.564739938721901 + }, + { + "x": 2.317656105280672, + "y": 6.717179467626764, + "heading": 3.3800987305060795, + "angularVelocity": -1.4373929390026206, + "velocityX": -1.190760943427104, + "velocityY": -1.0352722994728694, + "timestamp": 1.5914298425699551 + }, + { + "x": 2.2868057964945567, + "y": 6.686361396326711, + "heading": 3.343389515038899, + "angularVelocity": -1.3753970668521929, + "velocityX": -1.1558793527974576, + "velocityY": -1.1546714995865741, + "timestamp": 1.6181197464180093 + }, + { + "x": 2.25748075399746, + "y": 6.652639268574606, + "heading": 3.308925014353095, + "angularVelocity": -1.291293549876024, + "velocityX": -1.0987316651286732, + "velocityY": -1.263478802474685, + "timestamp": 1.6448096502660634 + }, + { + "x": 2.2301787041350396, + "y": 6.616374519308364, + "heading": 3.2770382136187797, + "angularVelocity": -1.194713960599006, + "velocityX": -1.022935489683713, + "velocityY": -1.3587440956212415, + "timestamp": 1.6714995541141175 + }, + { + "x": 2.2052911587380817, + "y": 6.577965167741144, + "heading": 3.2479010535236887, + "angularVelocity": -1.0916922091952401, + "velocityX": -0.9324704029899484, + "velocityY": -1.4390966631383846, + "timestamp": 1.6981894579621717 + }, + { + "x": 2.183095873519221, + "y": 6.537812097974725, + "heading": 3.2217255041499464, + "angularVelocity": -0.9807285002883435, + "velocityX": -0.8315985454731583, + "velocityY": -1.504429165238335, + "timestamp": 1.7248793618102258 + }, + { + "x": 2.163764936777705, + "y": 6.4962772278935255, + "heading": 3.198795874429834, + "angularVelocity": -0.8591124887766933, + "velocityX": -0.72427899521733, + "velocityY": -1.5562015628702863, + "timestamp": 1.75156926565828 + }, + { + "x": 2.1473967460074728, + "y": 6.453660043976164, + "heading": 3.1793849811459, + "angularVelocity": -0.7272747550699427, + "velocityX": -0.61327275150245, + "velocityY": -1.596752995439064, + "timestamp": 1.7782591695063341 + }, + { + "x": 2.134047988560311, + "y": 6.410199882298709, + "heading": 3.163697789085614, + "angularVelocity": -0.5877575336948715, + "velocityX": -0.5001425828716374, + "velocityY": -1.628337139199698, + "timestamp": 1.8049490733543883 + }, + { + "x": 2.1237534679344243, + "y": 6.366088523528368, + "heading": 3.151864532890396, + "angularVelocity": -0.443360765276075, + "velocityX": -0.38570841935190137, + "velocityY": -1.6527357693556206, + "timestamp": 1.8316389772024424 + }, + { + "x": 2.116536197487218, + "y": 6.321482513089734, + "heading": 3.143955647543306, + "angularVelocity": -0.29632498461271933, + "velocityX": -0.27041200628876155, + "velocityY": -1.671269057115243, + "timestamp": 1.8583288810504965 + }, + { + "x": 2.1124122616124033, + "y": 6.276512434450225, + "heading": 3.14, + "angularVelocity": -0.1482076355848078, + "velocityX": -0.15451295359818015, + "velocityY": -1.6849097282449244, + "timestamp": 1.8850187848985507 + }, + { + "x": 2.1113932132720947, + "y": 6.231289386749268, + "heading": 3.14, + "angularVelocity": 1.1480104978612554e-22, + "velocityX": -0.038181042019109025, + "velocityY": -1.6943878089038094, + "timestamp": 1.9117086887466048 + }, + { + "x": 2.1139722457961088, + "y": 6.184041017313733, + "heading": 3.14, + "angularVelocity": 1.326030822415817e-23, + "velocityX": 0.09287464315336291, + "velocityY": -1.7014812376514836, + "timestamp": 1.9394776516890055 + }, + { + "x": 2.1201947185383916, + "y": 6.136700429098736, + "heading": 3.14, + "angularVelocity": 1.3260179507784046e-23, + "velocityX": 0.224080126981686, + "velocityY": -1.7048021675563763, + "timestamp": 1.9672466146314063 + }, + { + "x": 2.13006161842343, + "y": 6.089396035622716, + "heading": 3.14, + "angularVelocity": 1.326007041519161e-23, + "velocityX": 0.35532115136976367, + "velocityY": -1.7034987433322955, + "timestamp": 1.995015577573807 + }, + { + "x": 2.1435678181376194, + "y": 6.042288248235824, + "heading": 3.14, + "angularVelocity": 1.3260437690465338e-23, + "velocityX": 0.4863775338749469, + "velocityY": -1.6964186773774967, + "timestamp": 2.022784540516208 + }, + { + "x": 2.1606964661173613, + "y": 5.995581737791835, + "heading": 3.14, + "angularVelocity": 1.325999769616951e-23, + "velocityX": 0.6168270675167464, + "velocityY": -1.6819681217793025, + "timestamp": 2.050553503458609 + }, + { + "x": 2.181407907708738, + "y": 5.949543614413858, + "heading": 3.14, + "angularVelocity": 1.3260185229673763e-23, + "velocityX": 0.7458485804578618, + "velocityY": -1.6578985493073877, + "timestamp": 2.07832246640101 + }, + { + "x": 2.2056170475643233, + "y": 5.904529822011414, + "heading": 3.14, + "angularVelocity": 1.3260157963898354e-23, + "velocityX": 0.871805688451557, + "velocityY": -1.6210109284892367, + "timestamp": 2.1060914293434108 + }, + { + "x": 2.2331463169007804, + "y": 5.861019319967013, + "heading": 3.14, + "angularVelocity": 1.326020172336188e-23, + "velocityX": 0.9913682910506778, + "velocityY": -1.566875296519094, + "timestamp": 2.1338603922858117 + }, + { + "x": 2.2636325105277852, + "y": 5.819639483049468, + "heading": 3.14, + "angularVelocity": 1.32601511090322e-23, + "velocityX": 1.0978513562152263, + "velocityY": -1.4901470034504147, + "timestamp": 2.1616293552282126 + }, + { + "x": 2.2963834267173127, + "y": 5.781115195207252, + "heading": 3.14, + "angularVelocity": 1.3260107576996925e-23, + "velocityX": 1.1794072489296905, + "velocityY": -1.387314604514559, + "timestamp": 2.1893983181706136 + }, + { + "x": 2.3303148904385163, + "y": 5.746039013141612, + "heading": 3.14, + "angularVelocity": 1.3260240855176863e-23, + "velocityX": 1.221920450957617, + "velocityY": -1.2631433928013844, + "timestamp": 2.2171672811130145 + }, + { + "x": 2.3642062447518297, + "y": 5.714607213055508, + "heading": 3.14, + "angularVelocity": 1.326023555417506e-23, + "velocityX": 1.2204760539171806, + "velocityY": -1.1319039948052778, + "timestamp": 2.2449362440554155 + }, + { + "x": 2.397064742551735, + "y": 5.68667060145179, + "heading": 3.14, + "angularVelocity": 1.3260077501265654e-23, + "velocityX": 1.1832814163085987, + "velocityY": -1.0060372676381828, + "timestamp": 2.2727052069978164 + }, + { + "x": 2.4282178000518853, + "y": 5.661954954768778, + "heading": 3.14, + "angularVelocity": 1.3260249833501151e-23, + "velocityX": 1.1218660763374204, + "velocityY": -0.8900457224231765, + "timestamp": 2.3004741699402174 + }, + { + "x": 2.457231034537111, + "y": 5.640189613922927, + "heading": 3.14, + "angularVelocity": 1.3260141947431602e-23, + "velocityX": 1.0448079946451803, + "velocityY": -0.7838009972139435, + "timestamp": 2.3282431328826183 + }, + { + "x": 2.483818861573163, + "y": 5.621144668272307, + "heading": 3.14, + "angularVelocity": 1.3260165607649488e-23, + "velocityX": 0.9574656097601072, + "velocityY": -0.6858356824532662, + "timestamp": 2.3560120958250192 + }, + { + "x": 2.507786212179532, + "y": 5.604632755958765, + "heading": 3.14, + "angularVelocity": 1.3260126001429812e-23, + "velocityX": 0.8630985123961222, + "velocityY": -0.5946175356923091, + "timestamp": 2.38378105876742 + }, + { + "x": 2.5289943781295285, + "y": 5.590502031840209, + "heading": 3.14, + "angularVelocity": 1.3260018717887446e-23, + "velocityX": 0.763736333761809, + "velocityY": -0.5088675492802951, + "timestamp": 2.411550021709821 + }, + { + "x": 2.547341071809568, + "y": 5.578628548447832, + "heading": 3.14, + "angularVelocity": 1.3260377381509874e-23, + "velocityX": 0.6606906321310845, + "velocityY": -0.4275810881740915, + "timestamp": 2.439318984652222 + }, + { + "x": 2.562748457639991, + "y": 5.568909973472095, + "heading": 3.14, + "angularVelocity": 1.3260175762620676e-23, + "velocityX": 0.5548419601546171, + "velocityY": -0.3499797596293578, + "timestamp": 2.467087947594623 + }, + { + "x": 2.575155705150767, + "y": 5.561260778200535, + "heading": 3.14, + "angularVelocity": 1.3260285574981282e-23, + "velocityX": 0.4468026961075851, + "velocityY": -0.2754584421256751, + "timestamp": 2.494856910537024 + }, + { + "x": 2.5845141850851863, + "y": 5.555608625671519, + "heading": 3.14, + "angularVelocity": 1.326006595026007e-23, + "velocityX": 0.33701222310069334, + "velocityY": -0.2035420818825477, + "timestamp": 2.522625873479425 + }, + { + "x": 2.5907842648688533, + "y": 5.55189166019714, + "heading": 3.14, + "angularVelocity": 1.3260302513246253e-23, + "velocityX": 0.22579452450827125, + "velocityY": -0.1338532332694335, + "timestamp": 2.550394836421826 + }, + { + "x": 2.59393310546875, + "y": 5.550056457519531, + "heading": 3.14, + "angularVelocity": 1.3260089047889746e-23, + "velocityX": 0.11339424545409957, + "velocityY": -0.06608826845335893, + "timestamp": 2.578163799364227 + }, + { + "x": 2.59393310546875, + "y": 5.550056457519531, + "heading": 3.14, + "angularVelocity": 4.554749859713047e-24, + "velocityX": -4.596278509340535e-25, + "velocityY": -1.5195442763313795e-23, + "timestamp": 2.6059327623066277 + }, + { + "x": 2.5899783566862378, + "y": 5.5489813066194555, + "heading": 3.1367490851985087, + "angularVelocity": -0.1085265960704612, + "velocityX": -0.13202296888338985, + "velocityY": -0.03589219483504766, + "timestamp": 2.6358877679845065 + }, + { + "x": 2.5820812048170954, + "y": 5.546779098674632, + "heading": 3.130275233897137, + "angularVelocity": -0.21611918124764368, + "velocityX": -0.26363379643671836, + "velocityY": -0.07351719337009978, + "timestamp": 2.6658427736623853 + }, + { + "x": 2.5702568313825034, + "y": 5.5433888083370855, + "heading": 3.1206149262957497, + "angularVelocity": -0.32249393324338926, + "velocityX": -0.3947378131636905, + "velocityY": -0.1131794256360211, + "timestamp": 2.695797779340264 + }, + { + "x": 2.554524436678559, + "y": 5.5387376635070815, + "heading": 3.107814789143658, + "angularVelocity": -0.42731212571741595, + "velocityX": -0.5252008586852795, + "velocityY": -0.15527103817038457, + "timestamp": 2.725752785018143 + }, + { + "x": 2.5349090755324424, + "y": 5.532737501107672, + "heading": 3.0919335422287366, + "angularVelocity": -0.5301700519005096, + "velocityX": -0.6548274888361012, + "velocityY": -0.20030583415450579, + "timestamp": 2.7557077906960217 + }, + { + "x": 2.511444606178007, + "y": 5.52527960958652, + "heading": 3.0730442108839475, + "angularVelocity": -0.6305901440285391, + "velocityX": -0.7833238159511905, + "velocityY": -0.24896979160511046, + "timestamp": 2.7856627963739005 + }, + { + "x": 2.4841786133457258, + "y": 5.5162272720643095, + "heading": 3.0512365155339536, + "angularVelocity": -0.7280150631418102, + "velocityX": -0.9102316028741883, + "velocityY": -0.3021978236143635, + "timestamp": 2.8156178020517793 + }, + { + "x": 2.4531809940870857, + "y": 5.505404760057258, + "heading": 3.026619340133933, + "angularVelocity": -0.8218050653951327, + "velocityX": -1.03480598842053, + "velocityY": -0.3612922702613114, + "timestamp": 2.845572807729658 + }, + { + "x": 2.4185596883958564, + "y": 5.492580836854246, + "heading": 2.999323661656212, + "angularVelocity": -0.9112226107130541, + "velocityX": -1.1557769697502154, + "velocityY": -0.4281061850200961, + "timestamp": 2.875527813407537 + }, + { + "x": 2.380491125275952, + "y": 5.477444069032361, + "heading": 2.9695096535475827, + "angularVelocity": -0.9952930214480606, + "velocityX": -1.27085815069692, + "velocityY": -0.5053168069690293, + "timestamp": 2.9054828190854156 + }, + { + "x": 2.3392828051052046, + "y": 5.459567907623698, + "heading": 2.9373981254397274, + "angularVelocity": -1.0719920554570026, + "velocityX": -1.3756739228789079, + "velocityY": -0.5967670846367005, + "timestamp": 2.9354378247632944 + }, + { + "x": 2.2955095509891357, + "y": 5.438376261665861, + "heading": 2.9034010779477137, + "angularVelocity": -1.1349371072601777, + "velocityX": -1.4613001441823985, + "velocityY": -0.7074492385586875, + "timestamp": 2.965392830441173 + }, + { + "x": 2.2502877957375587, + "y": 5.413212847808716, + "heading": 2.8684201412995205, + "angularVelocity": -1.1677826746007212, + "velocityX": -1.5096560400578645, + "velocityY": -0.8400403634617765, + "timestamp": 2.995347836119052 + }, + { + "x": 2.205434968136323, + "y": 5.383827174903616, + "heading": 2.8338778225381454, + "angularVelocity": -1.15314011730883, + "velocityX": -1.4973399799539564, + "velocityY": -0.9809937351072376, + "timestamp": 3.0253028417969308 + }, + { + "x": 2.1627117665288242, + "y": 5.35082401983714, + "heading": 2.8010931886625263, + "angularVelocity": -1.0944626159703914, + "velocityX": -1.426245819043514, + "velocityY": -1.1017575967561364, + "timestamp": 3.0552578474748096 + }, + { + "x": 2.1231203758060415, + "y": 5.315102619215835, + "heading": 2.770927863402835, + "angularVelocity": -1.0070211831729985, + "velocityX": -1.3216953169206167, + "velocityY": -1.1925018811692434, + "timestamp": 3.0852128531526883 + }, + { + "x": 2.0871120973907478, + "y": 5.277368588566571, + "heading": 2.743899688375806, + "angularVelocity": -0.9022924354505801, + "velocityX": -1.2020788379247493, + "velocityY": -1.2596903187079023, + "timestamp": 3.115167858830567 + }, + { + "x": 2.0548974336097707, + "y": 5.238102066889186, + "heading": 2.7202319811811844, + "angularVelocity": -0.7901085864957776, + "velocityX": -1.0754350751055555, + "velocityY": -1.310850082942305, + "timestamp": 3.145122864508446 + }, + { + "x": 2.026587153392112, + "y": 5.197634903381892, + "heading": 2.6999923980627787, + "angularVelocity": -0.6756661419481035, + "velocityX": -0.9450934685873016, + "velocityY": -1.350931591950202, + "timestamp": 3.1750778701863247 + }, + { + "x": 2.002245678733676, + "y": 5.1562068665429, + "heading": 2.6831832138201195, + "angularVelocity": -0.5611477568529534, + "velocityX": -0.812601236674494, + "velocityY": -1.3830088127670148, + "timestamp": 3.2050328758642035 + }, + { + "x": 1.9819131544340292, + "y": 5.113998389763002, + "heading": 2.66978325115483, + "angularVelocity": -0.44733634202529504, + "velocityX": -0.6787688347748394, + "velocityY": -1.4090625531434064, + "timestamp": 3.2349878815420823 + }, + { + "x": 1.9656157831702816, + "y": 5.071149791653941, + "heading": 2.659765787935313, + "angularVelocity": -0.33441700286222414, + "velocityX": -0.5440616983685925, + "velocityY": -1.4304319808793762, + "timestamp": 3.264942887219961 + }, + { + "x": 1.9533711923999129, + "y": 5.027773084636261, + "heading": 2.6531059130951715, + "angularVelocity": -0.22232927984587103, + "velocityX": -0.4087660974610013, + "velocityY": -1.4480620529380386, + "timestamp": 3.29489789289784 + }, + { + "x": 1.9451914498185667, + "y": 4.983959581247479, + "heading": 2.649783348569472, + "angularVelocity": -0.11091850762536054, + "velocityX": -0.27306763581709725, + "velocityY": -1.4626438018383596, + "timestamp": 3.3248528985757186 + }, + { + "x": 1.941084861755371, + "y": 4.939785003662109, + "heading": 2.649783348569472, + "angularVelocity": 6.132292113202646e-25, + "velocityX": -0.13709188064779135, + "velocityY": -1.4746976869375825, + "timestamp": 3.3548079042535974 + }, + { + "x": 1.9419022015392158, + "y": 4.889062683793002, + "heading": 2.649783348569472, + "angularVelocity": 4.614715107153485e-26, + "velocityX": 0.023936675164732843, + "velocityY": -1.4854577230996109, + "timestamp": 3.3889538237774923 + }, + { + "x": 1.948223657679148, + "y": 4.838070468392074, + "heading": 2.649783348569472, + "angularVelocity": 4.653945144562686e-26, + "velocityX": 0.18513064600613907, + "velocityY": -1.4933619042018718, + "timestamp": 3.423099743301387 + }, + { + "x": 1.9600538072506943, + "y": 4.786928471515363, + "heading": 2.649783348569472, + "angularVelocity": 4.629261284078221e-26, + "velocityX": 0.3464586614300294, + "velocityY": -1.4977484159101515, + "timestamp": 3.457245662825282 + }, + { + "x": 1.977394685635232, + "y": 4.735788039854744, + "heading": 2.649783348569472, + "angularVelocity": 4.640038416252255e-26, + "velocityX": 0.5078462851879747, + "velocityY": -1.4977025768725385, + "timestamp": 3.491391582349177 + }, + { + "x": 2.0002427480647933, + "y": 4.684845229382865, + "heading": 2.649783348569472, + "angularVelocity": 4.6490332666751425e-26, + "velocityX": 0.6691300966012094, + "velocityY": -1.4919150276866968, + "timestamp": 3.525537501873072 + }, + { + "x": 2.0285822715254445, + "y": 4.634362832128866, + "heading": 2.649783348569472, + "angularVelocity": 4.622795940866032e-26, + "velocityX": 0.8299534426308102, + "velocityY": -1.478431331119124, + "timestamp": 3.559683421396967 + }, + { + "x": 2.0623699922411927, + "y": 4.5847082412928, + "heading": 2.649783348569472, + "angularVelocity": 4.634016030607817e-26, + "velocityX": 0.9895097624213414, + "velocityY": -1.454188129311252, + "timestamp": 3.5938293409208617 + }, + { + "x": 2.101495965027088, + "y": 4.536421557771227, + "heading": 2.649783348569472, + "angularVelocity": 4.645177261974479e-26, + "velocityX": 1.1458462191511727, + "velocityY": -1.4141274915083855, + "timestamp": 3.6279752604447566 + }, + { + "x": 2.145673252803998, + "y": 4.4903376692314145, + "heading": 2.649783348569472, + "angularVelocity": 4.6233288536561975e-26, + "velocityX": 1.2937794147261192, + "velocityY": -1.3496162698902718, + "timestamp": 3.6621211799686515 + }, + { + "x": 2.1941116699379357, + "y": 4.447748182132556, + "heading": 2.649783348569472, + "angularVelocity": 4.6479494614855375e-26, + "velocityX": 1.4185711736373225, + "velocityY": -1.2472789631292271, + "timestamp": 3.6962670994925464 + }, + { + "x": 2.2448201323304646, + "y": 4.410180142680831, + "heading": 2.649783348569472, + "angularVelocity": 4.6329904581670297e-26, + "velocityX": 1.485051891985029, + "velocityY": -1.1002204648621148, + "timestamp": 3.7304130190164413 + }, + { + "x": 2.294852293597042, + "y": 4.3780811675391975, + "heading": 2.649783348569472, + "angularVelocity": 4.6446166642422895e-26, + "velocityX": 1.4652456856979683, + "velocityY": -0.9400530309096446, + "timestamp": 3.764558938540336 + }, + { + "x": 2.342149827765447, + "y": 4.350766530180341, + "heading": 2.649783348569472, + "angularVelocity": 4.633506801188826e-26, + "velocityX": 1.3851591882100787, + "velocityY": -0.7999385501902068, + "timestamp": 3.798704858064231 + }, + { + "x": 2.3856944951859207, + "y": 4.3274872473595005, + "heading": 2.649783348569472, + "angularVelocity": 4.6381778689211406e-26, + "velocityX": 1.2752524467821442, + "velocityY": -0.6817588498253565, + "timestamp": 3.832850777588126 + }, + { + "x": 2.4249725857215654, + "y": 4.307695719032298, + "heading": 2.649783348569472, + "angularVelocity": 4.6432377599415885e-26, + "velocityX": 1.150301151156826, + "velocityY": -0.5796162060697082, + "timestamp": 3.866996697112021 + }, + { + "x": 2.4596964858202144, + "y": 4.291007007021927, + "heading": 2.649783348569472, + "angularVelocity": 4.6331179794053255e-26, + "velocityX": 1.0169267831358226, + "velocityY": -0.4887468910800007, + "timestamp": 3.9011426166359158 + }, + { + "x": 2.4896890256889828, + "y": 4.277143601542489, + "heading": 2.649783348569472, + "angularVelocity": 4.6467164999430504e-26, + "velocityX": 0.8783638070657234, + "velocityY": -0.4060047488173824, + "timestamp": 3.9352885361598107 + }, + { + "x": 2.514832571254234, + "y": 4.265898475268687, + "heading": 2.649783348569472, + "angularVelocity": 4.632856845272505e-26, + "velocityX": 0.7363557905551701, + "velocityY": -0.3293256245723021, + "timestamp": 3.9694344556837056 + }, + { + "x": 2.5350443768209585, + "y": 4.257112226026017, + "heading": 2.649783348569472, + "angularVelocity": 4.629729705599566e-26, + "velocityX": 0.5919244773180152, + "velocityY": -0.2573147645510306, + "timestamp": 4.0035803752076005 + }, + { + "x": 2.550263563230007, + "y": 4.2506587632470065, + "heading": 2.649783348569472, + "angularVelocity": 4.62810585566047e-26, + "velocityX": 0.445710252388941, + "velocityY": -0.18899660249284694, + "timestamp": 4.037726294731495 + }, + { + "x": 2.5604437164785643, + "y": 4.246436059535304, + "heading": 2.649783348569472, + "angularVelocity": 4.6519462435401186e-26, + "velocityX": 0.29813674343821717, + "velocityY": -0.12366642253542871, + "timestamp": 4.07187221425539 + }, + { + "x": 2.5655484199523926, + "y": 4.244359970092773, + "heading": 2.649783348569472, + "angularVelocity": 4.6699948971014915e-26, + "velocityX": 0.14949673474910496, + "velocityY": -0.06080051354533503, + "timestamp": 4.106018133779285 + }, + { + "x": 2.5655484199523926, + "y": 4.244359970092773, + "heading": 2.649783348569472, + "angularVelocity": 1.535197914331468e-26, + "velocityX": 1.1963425727073795e-24, + "velocityY": -7.168329076335369e-24, + "timestamp": 4.14016405330318 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Test2.1.traj b/src/main/deploy/choreo/Test2.1.traj new file mode 100644 index 00000000..6858b3e4 --- /dev/null +++ b/src/main/deploy/choreo/Test2.1.traj @@ -0,0 +1,688 @@ +{ + "samples": [ + { + "x": 0.3137660622596754, + "y": 6.865104675292969, + "heading": 3.135568639860517, + "angularVelocity": -8.5693301650697e-28, + "velocityX": -3.4362695197914885e-17, + "velocityY": -4.743570524232907e-17, + "timestamp": 0 + }, + { + "x": 0.3176438425782737, + "y": 6.865425616765028, + "heading": 3.1355686398605154, + "angularVelocity": -5.1868461530169816e-17, + "velocityX": 0.13515003367410133, + "velocityY": 0.011185587421808195, + "timestamp": 0.028692411042588198 + }, + { + "x": 0.3253999903453479, + "y": 6.866060324505518, + "heading": 3.1355686398605136, + "angularVelocity": -5.186846153227154e-17, + "velocityX": 0.2703205302461453, + "velocityY": 0.022121101623313478, + "timestamp": 0.057384822085176396 + }, + { + "x": 0.3370351106624937, + "y": 6.867001226604551, + "heading": 3.135568639860512, + "angularVelocity": -5.186846153153239e-17, + "velocityX": 0.40551211607398097, + "velocityY": 0.032792716430754286, + "timestamp": 0.0860772331277646 + }, + { + "x": 0.352549826865152, + "y": 6.86824032070545, + "heading": 3.13556863986051, + "angularVelocity": -5.186846153220684e-17, + "velocityX": 0.5407254266512643, + "velocityY": 0.04318542973123231, + "timestamp": 0.11476964417035279 + }, + { + "x": 0.3719447806630506, + "y": 6.869769136338039, + "heading": 3.1355686398605083, + "angularVelocity": -5.1868461532349994e-17, + "velocityX": 0.6759611023663795, + "velocityY": 0.053282926635904025, + "timestamp": 0.14346205521294098 + }, + { + "x": 0.3952206321171861, + "y": 6.871578692739439, + "heading": 3.135568639860506, + "angularVelocity": -5.1868461531715824e-17, + "velocityX": 0.8112197828054859, + "velocityY": 0.06306742220841621, + "timestamp": 0.17215446625552916 + }, + { + "x": 0.42237805939934603, + "y": 6.8736594514681, + "heading": 3.1355686398605043, + "angularVelocity": -5.1868461532243226e-17, + "velocityX": 0.9465020991734979, + "velocityY": 0.07251947999676765, + "timestamp": 0.20084687729811734 + }, + { + "x": 0.45341775826418784, + "y": 6.876001262985059, + "heading": 3.1355686398605025, + "angularVelocity": -5.1868461531961763e-17, + "velocityX": 1.081808664276005, + "velocityY": 0.08161780177636822, + "timestamp": 0.22953928834070553 + }, + { + "x": 0.4883404411428946, + "y": 6.878593306214626, + "heading": 3.1355686398605007, + "angularVelocity": -5.186846153181617e-17, + "velocityX": 1.2171400593303083, + "velocityY": 0.09033898286621334, + "timestamp": 0.2582316993832937 + }, + { + "x": 0.5271468357396778, + "y": 6.881424019896891, + "heading": 3.135568639860499, + "angularVelocity": -5.186846153207198e-17, + "velocityX": 1.3524968166384297, + "velocityY": 0.09865722605409555, + "timestamp": 0.2869241104258819 + }, + { + "x": 0.5698376829754908, + "y": 6.884481024296066, + "heading": 3.135568639860497, + "angularVelocity": -5.18684615321628e-17, + "velocityX": 1.4878793968358233, + "velocityY": 0.10654400547371551, + "timestamp": 0.3156165214684701 + }, + { + "x": 0.6164137340738928, + "y": 6.887751031517755, + "heading": 3.1355686398604954, + "angularVelocity": -5.186846153204876e-17, + "velocityX": 1.6232881589932495, + "velocityY": 0.11396766959864212, + "timestamp": 0.34430893251105826 + }, + { + "x": 0.666875746517318, + "y": 6.891219742296381, + "heading": 3.1355686398604936, + "angularVelocity": -5.186846153160267e-17, + "velocityX": 1.758723321247663, + "velocityY": 0.12089296969422078, + "timestamp": 0.37300134355364645 + }, + { + "x": 0.7212244785112947, + "y": 6.894871726616115, + "heading": 3.135568639860492, + "angularVelocity": -5.186846153215831e-17, + "velocityX": 1.894184908800638, + "velocityY": 0.1272804963763128, + "timestamp": 0.40169375459623463 + }, + { + "x": 0.7794606814696723, + "y": 6.898690284891088, + "heading": 3.13556863986049, + "angularVelocity": -5.186846153213201e-17, + "velocityX": 2.02967268494572, + "velocityY": 0.1330860020548787, + "timestamp": 0.4303861656388228 + }, + { + "x": 0.8415850898614354, + "y": 6.9026572856068995, + "heading": 3.1355686398604883, + "angularVelocity": -5.186846153171342e-17, + "velocityX": 2.165186059113343, + "velocityY": 0.13825958055332968, + "timestamp": 0.459078576681411 + }, + { + "x": 0.9075984075182041, + "y": 6.906752974251311, + "heading": 3.1355686398604865, + "angularVelocity": -5.1868461532457525e-17, + "velocityX": 2.3007239635171715, + "velocityY": 0.14274466646709222, + "timestamp": 0.4877709877239992 + }, + { + "x": 0.9775012891595924, + "y": 6.910955746947304, + "heading": 3.1355686398604847, + "angularVelocity": -5.186846153166985e-17, + "velocityX": 2.43628468648487, + "velocityY": 0.14647680495568935, + "timestamp": 0.5164633987665874 + }, + { + "x": 1.051294315403438, + "y": 6.915241880318482, + "heading": 3.1355686398604825, + "angularVelocity": -5.1868461532007456e-17, + "velocityX": 2.571865645390099, + "velocityY": 0.1493821263335398, + "timestamp": 0.5451558098091757 + }, + { + "x": 1.1289779588157673, + "y": 6.919585206580955, + "heading": 3.1355686398604807, + "angularVelocity": -5.186846153206435e-17, + "velocityX": 2.7074630743656276, + "velocityY": 0.15137543708073178, + "timestamp": 0.5738482208517639 + }, + { + "x": 1.210552537505391, + "y": 6.923956719399065, + "heading": 3.135568639860479, + "angularVelocity": -5.1868461531968826e-17, + "velocityX": 2.843071590203452, + "velocityY": 0.1523578067950584, + "timestamp": 0.6025406318943521 + }, + { + "x": 1.29601815119489, + "y": 6.9283240912670765, + "heading": 3.135568639860477, + "angularVelocity": -5.186846153216223e-17, + "velocityX": 2.978683581614699, + "velocityY": 0.15221348465724693, + "timestamp": 0.6312330429369404 + }, + { + "x": 1.3853745923008893, + "y": 6.932651076487761, + "heading": 3.1355686398604754, + "angularVelocity": -5.1868461531841376e-17, + "velocityX": 3.1142883382426785, + "velocityY": 0.15080591220658612, + "timestamp": 0.6599254539795286 + }, + { + "x": 1.478621220826743, + "y": 6.936896764298285, + "heading": 3.1355686398604736, + "angularVelocity": -5.186846153215644e-17, + "velocityX": 3.2498707894378844, + "velocityY": 0.1479725006107481, + "timestamp": 0.6886178650221169 + }, + { + "x": 1.5757567859462194, + "y": 6.941014632922591, + "heading": 3.135568639860472, + "angularVelocity": -5.186846153173054e-17, + "velocityX": 3.3854096463101833, + "velocityY": 0.14351769247253987, + "timestamp": 0.7173102760647051 + }, + { + "x": 1.676779167520274, + "y": 6.94495133505747, + "heading": 3.13556863986047, + "angularVelocity": -5.186846153388186e-17, + "velocityX": 3.52087461120313, + "velocityY": 0.13720360164351214, + "timestamp": 0.7460026871072933 + }, + { + "x": 1.7816849946975712, + "y": 6.948645114898683, + "heading": 3.1355686398604683, + "angularVelocity": -1.6596882408000968e-16, + "velocityX": 3.6562220937649346, + "velocityY": 0.12873717150261788, + "timestamp": 0.7746950981498816 + }, + { + "x": 1.8121461784699802, + "y": 6.949692765946351, + "heading": 3.1362037838100756, + "angularVelocity": 0.07689012502825784, + "velocityX": 3.687611651202223, + "velocityY": 0.12682797354934053, + "timestamp": 0.7829555080448543 + }, + { + "x": 1.8428693964358074, + "y": 6.950724930078729, + "heading": 3.1374589452138397, + "angularVelocity": 0.15194904607940982, + "velocityX": 3.7193333450103565, + "velocityY": 0.12495313737441759, + "timestamp": 0.7912159179398269 + }, + { + "x": 1.8738574968140793, + "y": 6.95174197296817, + "heading": 3.139318096432083, + "angularVelocity": 0.22506767120320992, + "velocityX": 3.751399842413374, + "velocityY": 0.1231225692636009, + "timestamp": 0.7994763278347996 + }, + { + "x": 1.9051134303883683, + "y": 6.952744343319344, + "heading": 3.1417642609142864, + "angularVelocity": 0.2961311258529596, + "velocityX": 3.783823559810237, + "velocityY": 0.12134632105632295, + "timestamp": 0.8077367377297723 + }, + { + "x": 1.9366402470506743, + "y": 6.953732574884012, + "heading": 3.1447794649491048, + "angularVelocity": 0.36501869436959006, + "velocityX": 3.8166164952046153, + "velocityY": 0.11963468849991928, + "timestamp": 0.815997147624745 + }, + { + "x": 1.96844109067084, + "y": 6.9547072895390265, + "heading": 3.148344689757031, + "angularVelocity": 0.43160386146187707, + "velocityX": 3.849790025494764, + "velocityY": 0.117998339961998, + "timestamp": 0.8242575575197176 + }, + { + "x": 2.0005191919654135, + "y": 6.955669201722795, + "heading": 3.152439824908413, + "angularVelocity": 0.495754472653526, + "velocityX": 3.883354664287875, + "velocityY": 0.11644848088619933, + "timestamp": 0.8325179674146903 + }, + { + "x": 2.032877859008433, + "y": 6.9566191245667985, + "heading": 3.1570436241813726, + "angularVelocity": 0.5573330296555058, + "velocityX": 3.91731977643278, + "velocityY": 0.11499705899340439, + "timestamp": 0.840778377309663 + }, + { + "x": 2.065520465002766, + "y": 6.957557978090485, + "heading": 3.1621336650538163, + "angularVelocity": 0.6161971303075195, + "velocityX": 3.951693246384564, + "velocityY": 0.11365701407324179, + "timestamp": 0.8490387872046357 + }, + { + "x": 2.0984504329177724, + "y": 6.958486799841183, + "heading": 3.1676863130152344, + "angularVelocity": 0.6722000520579139, + "velocityX": 3.9864810988430395, + "velocityY": 0.11244257397721802, + "timestamp": 0.8572991970996083 + }, + { + "x": 2.1316712166001497, + "y": 6.959406758344203, + "heading": 3.1736766917418935, + "angularVelocity": 0.7251914617828771, + "velocityX": 4.021687071799445, + "velocityY": 0.1113695948160438, + "timestamp": 0.865559606994581 + }, + { + "x": 2.165186277981829, + "y": 6.960319169668765, + "heading": 3.1800786598480077, + "angularVelocity": 0.7750182118701426, + "velocityX": 4.057312144046869, + "velocityY": 0.11045593816320934, + "timestamp": 0.8738200168895537 + }, + { + "x": 2.1989990600414004, + "y": 6.961225517299853, + "heading": 3.1868647943382302, + "angularVelocity": 0.8215251514763765, + "velocityX": 4.093354021105941, + "velocityY": 0.10972187126423899, + "timestamp": 0.8820804267845264 + }, + { + "x": 2.2331129552203137, + "y": 6.962127475323903, + "heading": 3.19400637995892, + "angularVelocity": 0.8645558406293681, + "velocityX": 4.1298065849823535, + "velocityY": 0.10919046821108658, + "timestamp": 0.890340836679499 + }, + { + "x": 2.2675312690432237, + "y": 6.963026934685406, + "heading": 3.201473402262516, + "angularVelocity": 0.9039529997346885, + "velocityX": 4.1666593135838506, + "velocityY": 0.10888798170273117, + "timestamp": 0.8986012465744717 + }, + { + "x": 2.302257178726397, + "y": 6.963926031963223, + "heading": 3.2092345401896574, + "angularVelocity": 0.9395584511933502, + "velocityX": 4.203896673978105, + "velocityY": 0.10884414808067534, + "timestamp": 0.9068616564694444 + }, + { + "x": 2.3372936865473584, + "y": 6.964827179782931, + "heading": 3.2172571510612844, + "angularVelocity": 0.9712122005605541, + "velocityX": 4.241497488191654, + "velocityY": 0.10909238538572823, + "timestamp": 0.9151220663644171 + }, + { + "x": 2.372643567633051, + "y": 6.965733097685614, + "heading": 3.225507236598066, + "angularVelocity": 0.9987501397242283, + "velocityX": 4.279434257518503, + "velocityY": 0.10966984861524744, + "timestamp": 0.9233824762593897 + }, + { + "x": 2.4083093114869696, + "y": 6.9666468421059164, + "heading": 3.233949372180996, + "angularVelocity": 1.0219995969049733, + "velocityX": 4.31767240456481, + "velocityY": 0.11061732189065114, + "timestamp": 0.9316428861543624 + }, + { + "x": 2.44429305579275, + "y": 6.967571834188082, + "heading": 3.2425465717794997, + "angularVelocity": 1.0407715486052915, + "velocityX": 4.356169338240552, + "velocityY": 0.11197895672538041, + "timestamp": 0.9399032960493351 + }, + { + "x": 2.480596509367599, + "y": 6.968511884597095, + "heading": 3.251260045781759, + "angularVelocity": 1.054847654420144, + "velocityX": 4.394873140247142, + "velocityY": 0.11380190825438415, + "timestamp": 0.9481637059443078 + }, + { + "x": 2.517220857768171, + "y": 6.969471215284249, + "heading": 3.260048785175124, + "angularVelocity": 1.0639592350877263, + "velocityX": 4.433720464992984, + "velocityY": 0.11613596653794503, + "timestamp": 0.9564241158392804 + }, + { + "x": 2.55416663843719, + "y": 6.970454479070239, + "heading": 3.2688688682505744, + "angularVelocity": 1.0677536814268536, + "velocityX": 4.4726328522150816, + "velocityY": 0.11903329235341958, + "timestamp": 0.9646845257342531 + }, + { + "x": 2.5914335596417093, + "y": 6.971466777824258, + "heading": 3.2776723283732965, + "angularVelocity": 1.065741317277861, + "velocityX": 4.511509922431134, + "velocityY": 0.12254824722845813, + "timestamp": 0.9729449356292258 + }, + { + "x": 2.6290202140808088, + "y": 6.9725136756896955, + "heading": 3.286405336491996, + "angularVelocity": 1.0572124422077387, + "velocityX": 4.550216625687468, + "velocityY": 0.12673679378462502, + "timestamp": 0.9812053455241985 + }, + { + "x": 2.9717276780335036, + "y": 6.984363087388876, + "heading": 3.35108767309623, + "angularVelocity": 0.9203746590757935, + "velocityX": 4.876435853393211, + "velocityY": 0.16860705449782717, + "timestamp": 1.0514836131475132 + }, + { + "x": 3.3366815448761877, + "y": 6.999743371642888, + "heading": 3.3973428408952557, + "angularVelocity": 0.6581717131524953, + "velocityX": 5.192983253355129, + "velocityY": 0.21884836912099448, + "timestamp": 1.1217618807708278 + }, + { + "x": 3.71402485435163, + "y": 7.027526643127482, + "heading": 3.3973428368677423, + "angularVelocity": -5.730809398962461e-8, + "velocityX": 5.369274488921224, + "velocityY": 0.3953323327989187, + "timestamp": 1.1920401483941425 + }, + { + "x": 4.090360227255461, + "y": 7.066661937541599, + "heading": 3.3973428243317834, + "angularVelocity": -1.783760359331945e-7, + "velocityX": 5.354932408421835, + "velocityY": 0.5568619679682045, + "timestamp": 1.2623184160174572 + }, + { + "x": 4.466694898854889, + "y": 7.105803975313799, + "heading": 3.3973428117958613, + "angularVelocity": -1.7837551225317533e-7, + "velocityX": 5.354922429456401, + "velocityY": 0.5569579202208789, + "timestamp": 1.3325966836407719 + }, + { + "x": 4.8430291421579215, + "y": 7.144950130722809, + "heading": 3.397342799228156, + "angularVelocity": -1.7882775854425254e-7, + "velocityX": 5.354916335162799, + "velocityY": 0.5570165106920909, + "timestamp": 1.4028749512640866 + }, + { + "x": 5.214361577079249, + "y": 7.183676617694927, + "heading": 3.378511132398759, + "angularVelocity": -0.26795860891639617, + "velocityX": 5.283744854264688, + "velocityY": 0.55104498562348, + "timestamp": 1.4731532188874013 + }, + { + "x": 5.562524060208366, + "y": 7.219962031584869, + "heading": 3.355414523374288, + "angularVelocity": -0.32864511043819594, + "velocityX": 4.9540561385951625, + "velocityY": 0.5163105909843465, + "timestamp": 1.543431486510716 + }, + { + "x": 5.887470499962918, + "y": 7.253817931255832, + "heading": 3.331666055733143, + "angularVelocity": -0.3379205043646441, + "velocityX": 4.6237115788942855, + "velocityY": 0.48174066914158636, + "timestamp": 1.6137097541340306 + }, + { + "x": 6.189199807150463, + "y": 7.285249171486667, + "heading": 3.308363686841363, + "angularVelocity": -0.33157289841972754, + "velocityX": 4.29335152091094, + "velocityY": 0.4472398266744939, + "timestamp": 1.6839880217573453 + }, + { + "x": 6.467713626267079, + "y": 7.31425833816948, + "heading": 3.2860397702782755, + "angularVelocity": -0.3176503536305448, + "velocityX": 3.963014862708697, + "velocityY": 0.41277577925367004, + "timestamp": 1.75426628938066 + }, + { + "x": 6.723013594809074, + "y": 7.34084703024564, + "heading": 3.265008761402615, + "angularVelocity": -0.29925337642620564, + "velocityX": 3.6327015046867763, + "velocityY": 0.37833448340917136, + "timestamp": 1.8245445570039747 + }, + { + "x": 6.955101065031071, + "y": 7.365016333172441, + "heading": 3.245478308115181, + "angularVelocity": -0.27790174612893576, + "velocityX": 3.302407388098493, + "velocityY": 0.34390863269912847, + "timestamp": 1.8948228246272893 + }, + { + "x": 7.163977127241494, + "y": 7.386767031992689, + "heading": 3.2275957745381954, + "angularVelocity": -0.2544532496565816, + "velocityX": 2.972128785672115, + "velocityY": 0.3094939524808096, + "timestamp": 1.965101092250604 + }, + { + "x": 7.349642665971283, + "y": 7.4060997209757735, + "heading": 3.2114711639593976, + "angularVelocity": -0.22943949992088322, + "velocityX": 2.6418627693690517, + "velocityY": 0.2750877282106925, + "timestamp": 2.035379359873919 + }, + { + "x": 7.512098408217057, + "y": 7.423014865697383, + "heading": 3.1971897253730983, + "angularVelocity": -0.2032127294720464, + "velocityX": 2.3116070976097918, + "velocityY": 0.24068812868678965, + "timestamp": 2.1056576274972336 + }, + { + "x": 7.651344959888608, + "y": 7.43751284079522, + "heading": 3.184819461924971, + "angularVelocity": -0.17601833207433357, + "velocityX": 1.981360047431725, + "velocityY": 0.20629385993890986, + "timestamp": 2.1759358951205483 + }, + { + "x": 7.767382832665261, + "y": 7.449593954241994, + "heading": 3.174415882660127, + "angularVelocity": -0.14803408815663477, + "velocityX": 1.6511202780154728, + "velocityY": 0.1719039733808395, + "timestamp": 2.246214162743863 + }, + { + "x": 7.860212463836391, + "y": 7.459258463658005, + "heading": 3.1660251568616267, + "angularVelocity": -0.11939289459260129, + "velocityX": 1.3208867308552614, + "velocityY": 0.13751775254062693, + "timestamp": 2.3164924303671777 + }, + { + "x": 7.929834231127341, + "y": 7.466506587678373, + "heading": 3.159686290431417, + "angularVelocity": -0.09019668020541635, + "velocityX": 0.9906585584055106, + "velocityY": 0.10313464269233455, + "timestamp": 2.3867706979904924 + }, + { + "x": 7.976248463934981, + "y": 7.471338514116222, + "heading": 3.1554326760982865, + "angularVelocity": -0.06052531567696676, + "velocityX": 0.6604350729932147, + "velocityY": 0.06875420526505516, + "timestamp": 2.457048965613807 + }, + { + "x": 7.999455451965332, + "y": 7.473754405975342, + "heading": 3.153293227446569, + "angularVelocity": -0.03044253542482025, + "velocityX": 0.33021570985126986, + "velocityY": 0.03437608724320182, + "timestamp": 2.5273272332371217 + }, + { + "x": 7.999455451965332, + "y": 7.473754405975342, + "heading": 3.153293227446569, + "angularVelocity": -3.79407884851814e-18, + "velocityX": 2.546987187300363e-16, + "velocityY": -1.0256209789219734e-16, + "timestamp": 2.5976055008604364 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Test2.2.traj b/src/main/deploy/choreo/Test2.2.traj new file mode 100644 index 00000000..a0cb7c71 --- /dev/null +++ b/src/main/deploy/choreo/Test2.2.traj @@ -0,0 +1,301 @@ +{ + "samples": [ + { + "x": 7.999455451965332, + "y": 7.473754405975342, + "heading": 3.153293227446569, + "angularVelocity": -3.79407884851814e-18, + "velocityX": 2.546987187300363e-16, + "velocityY": -1.0256209789219734e-16, + "timestamp": 0 + }, + { + "x": 7.9777911181971035, + "y": 7.466979623755239, + "heading": 3.153293227446569, + "angularVelocity": 4.0010921973360205e-29, + "velocityX": -0.31261406582002, + "velocityY": -0.09775939742848679, + "timestamp": 0.06930057261307221 + }, + { + "x": 7.934462450883723, + "y": 7.453430059384792, + "heading": 3.153293227446569, + "angularVelocity": -2.051733815514594e-34, + "velocityX": -0.6252281284210741, + "velocityY": -0.1955187938503514, + "timestamp": 0.13860114522614442 + }, + { + "x": 7.869469450281316, + "y": 7.433105712944096, + "heading": 3.153293227446569, + "angularVelocity": -1.98522229630168e-34, + "velocityX": -0.9378421873262779, + "velocityY": -0.29327818911646475, + "timestamp": 0.20790171783921663 + }, + { + "x": 7.782812116686986, + "y": 7.40600658452606, + "heading": 3.153293227446569, + "angularVelocity": -1.9190015159804115e-34, + "velocityX": -1.2504562419442953, + "velocityY": -0.39103758304190667, + "timestamp": 0.27720229045228884 + }, + { + "x": 7.6744904504495075, + "y": 7.372132674239751, + "heading": 3.153293227446569, + "angularVelocity": -8.200637720207044e-29, + "velocityX": -1.5630702915295283, + "velocityY": -0.4887969753935167, + "timestamp": 0.34650286306536104 + }, + { + "x": 7.544504451984089, + "y": 7.331483982215012, + "heading": 3.153293227446569, + "angularVelocity": 6.610929004011549e-29, + "velocityX": -1.8756843351233508, + "velocityY": -0.5865563658715173, + "timestamp": 0.41580343567843325 + }, + { + "x": 7.392854121793352, + "y": 7.28406050860902, + "heading": 3.153293227446569, + "angularVelocity": -6.483737543755527e-29, + "velocityX": -2.188298371464413, + "velocityY": -0.6843157540814643, + "timestamp": 0.48510400829150546 + }, + { + "x": 7.219539460498179, + "y": 7.229862253615935, + "heading": 3.153293227446569, + "angularVelocity": 8.073376116568391e-29, + "velocityX": -2.5009123988461828, + "velocityY": -0.7820751394896976, + "timestamp": 0.5544045809045777 + }, + { + "x": 7.024560468885024, + "y": 7.168889217481695, + "heading": 3.153293227446569, + "angularVelocity": -3.526812750348743e-30, + "velocityX": -2.813526414879514, + "velocityY": -0.8798345213490937, + "timestamp": 0.6237051535176499 + }, + { + "x": 6.807917147982327, + "y": 7.101141400527908, + "heading": 3.153293227446569, + "angularVelocity": -9.438045628849687e-29, + "velocityX": -3.1261404160725776, + "velocityY": -0.9775938985677018, + "timestamp": 0.6930057261307221 + }, + { + "x": 6.569609499192503, + "y": 7.026618803193132, + "heading": 3.153293227446569, + "angularVelocity": 9.790681244582267e-29, + "velocityX": -3.438754397028912, + "velocityY": -1.0753532694579622, + "timestamp": 0.7623062987437943 + }, + { + "x": 6.309637524541267, + "y": 6.94532142611084, + "heading": 3.153293227446569, + "angularVelocity": -1.3895984503554753e-34, + "velocityX": -3.7513683487544087, + "velocityY": -1.1731126312072742, + "timestamp": 0.8316068713568665 + }, + { + "x": 6.028001227211883, + "y": 6.85724927027649, + "heading": 3.153293227446569, + "angularVelocity": -1.323503512813964e-34, + "velocityX": -4.063982254545725, + "velocityY": -1.2708719785922367, + "timestamp": 0.9009074439699387 + }, + { + "x": 5.724700612934229, + "y": 6.762402337481905, + "heading": 3.153293227446569, + "angularVelocity": -2.1989550257903952e-29, + "velocityX": -4.376596077655505, + "velocityY": -1.3686313001213652, + "timestamp": 0.9702080165830109 + }, + { + "x": 5.399735695078014, + "y": 6.660780631908008, + "heading": 3.153293227446569, + "angularVelocity": 1.8024841235265752e-29, + "velocityX": -4.689209707841794, + "velocityY": -1.4663905613202457, + "timestamp": 1.0395085891960831 + }, + { + "x": 5.053106540487171, + "y": 6.552384174457959, + "heading": 3.153293227446569, + "angularVelocity": 1.896211807861914e-29, + "velocityX": -5.001822373477154, + "velocityY": -1.564149520888688, + "timestamp": 1.1088091618091553 + }, + { + "x": 4.7281416226309565, + "y": 6.4507624688840615, + "heading": 3.153293227446569, + "angularVelocity": -1.4997927313419928e-29, + "velocityX": -4.689209707841795, + "velocityY": -1.4663905613202457, + "timestamp": 1.1781097344222276 + }, + { + "x": 4.424841008353304, + "y": 6.355915536089477, + "heading": 3.153293227446569, + "angularVelocity": -4.4185947102012545e-35, + "velocityX": -4.376596077655505, + "velocityY": -1.3686313001213652, + "timestamp": 1.2474103070352998 + }, + { + "x": 4.14320471102392, + "y": 6.267843380255127, + "heading": 3.153293227446569, + "angularVelocity": -9.264733448000688e-35, + "velocityX": -4.063982254545726, + "velocityY": -1.2708719785922367, + "timestamp": 1.316710879648372 + }, + { + "x": 3.883232736372683, + "y": 6.186546003172835, + "heading": 3.153293227446569, + "angularVelocity": 3.5265482976182584e-30, + "velocityX": -3.751368348754409, + "velocityY": -1.1731126312072742, + "timestamp": 1.3860114522614442 + }, + { + "x": 3.644925087582859, + "y": 6.112023405838059, + "heading": 3.153293227446569, + "angularVelocity": 2.156768838744686e-29, + "velocityX": -3.438754397028912, + "velocityY": -1.0753532694579622, + "timestamp": 1.4553120248745168 + }, + { + "x": 3.4282817666801613, + "y": 6.044275588884272, + "heading": 3.153293227446569, + "angularVelocity": -2.105657187574439e-28, + "velocityX": -3.1261404160725776, + "velocityY": -0.9775938985677018, + "timestamp": 1.5246125974875895 + }, + { + "x": 3.2333027750670063, + "y": 5.983302552750032, + "heading": 3.153293227446569, + "angularVelocity": 1.8547117767579628e-28, + "velocityX": -2.813526414879514, + "velocityY": -0.8798345213490937, + "timestamp": 1.5939131701006621 + }, + { + "x": 3.059988113771833, + "y": 5.929104297756947, + "heading": 3.153293227446569, + "angularVelocity": -5.955568710068408e-35, + "velocityX": -2.5009123988461828, + "velocityY": -0.7820751394896976, + "timestamp": 1.6632137427137348 + }, + { + "x": 2.908337783581096, + "y": 5.881680824150955, + "heading": 3.153293227446569, + "angularVelocity": -2.0810713408746708e-29, + "velocityX": -2.1882983714644135, + "velocityY": -0.6843157540814642, + "timestamp": 1.7325143153268074 + }, + { + "x": 2.7783517851156776, + "y": 5.841032132126216, + "heading": 3.153293227446569, + "angularVelocity": 2.081061415014159e-29, + "velocityX": -1.875684335123351, + "velocityY": -0.5865563658715173, + "timestamp": 1.80181488793988 + }, + { + "x": 2.6700301188781994, + "y": 5.807158221839908, + "heading": 3.153293227446569, + "angularVelocity": 7.326893682940581e-29, + "velocityX": -1.5630702915295283, + "velocityY": -0.48879697539351663, + "timestamp": 1.8711154605529527 + }, + { + "x": 2.583372785283869, + "y": 5.780059093421871, + "heading": 3.153293227446569, + "angularVelocity": -7.326900961882318e-29, + "velocityX": -1.2504562419442953, + "velocityY": -0.39103758304190656, + "timestamp": 1.9404160331660254 + }, + { + "x": 2.5183797846814615, + "y": 5.759734746981175, + "heading": 3.153293227446569, + "angularVelocity": -2.64698564160025e-35, + "velocityX": -0.9378421873262781, + "velocityY": -0.2932781891164647, + "timestamp": 2.009716605779098 + }, + { + "x": 2.4750511173680816, + "y": 5.746185182610728, + "heading": 3.153293227446569, + "angularVelocity": -4.5099737006589516e-29, + "velocityX": -0.6252281284210743, + "velocityY": -0.1955187938503513, + "timestamp": 2.0790171783921707 + }, + { + "x": 2.4533867835998535, + "y": 5.739410400390625, + "heading": 3.153293227446569, + "angularVelocity": 1.9207053764808128e-28, + "velocityX": -0.31261406582002027, + "velocityY": -0.09775939742848665, + "timestamp": 2.1483177510052434 + }, + { + "x": 2.4533867835998535, + "y": 5.739410400390625, + "heading": 3.153293227446569, + "angularVelocity": -4.899028012023128e-29, + "velocityX": 1.275108562409782e-22, + "velocityY": 3.987692290669026e-23, + "timestamp": 2.217618323618316 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Test2.traj b/src/main/deploy/choreo/Test2.traj new file mode 100644 index 00000000..68c886e7 --- /dev/null +++ b/src/main/deploy/choreo/Test2.traj @@ -0,0 +1,976 @@ +{ + "samples": [ + { + "x": 0.3137660622596754, + "y": 6.865104675292969, + "heading": 3.135568639860517, + "angularVelocity": -8.5693301650697e-28, + "velocityX": -3.4362695197914885e-17, + "velocityY": -4.743570524232907e-17, + "timestamp": 0 + }, + { + "x": 0.3176438425782737, + "y": 6.865425616765028, + "heading": 3.1355686398605154, + "angularVelocity": -5.1868461530169816e-17, + "velocityX": 0.13515003367410133, + "velocityY": 0.011185587421808195, + "timestamp": 0.028692411042588198 + }, + { + "x": 0.3253999903453479, + "y": 6.866060324505518, + "heading": 3.1355686398605136, + "angularVelocity": -5.186846153227154e-17, + "velocityX": 0.2703205302461453, + "velocityY": 0.022121101623313478, + "timestamp": 0.057384822085176396 + }, + { + "x": 0.3370351106624937, + "y": 6.867001226604551, + "heading": 3.135568639860512, + "angularVelocity": -5.186846153153239e-17, + "velocityX": 0.40551211607398097, + "velocityY": 0.032792716430754286, + "timestamp": 0.0860772331277646 + }, + { + "x": 0.352549826865152, + "y": 6.86824032070545, + "heading": 3.13556863986051, + "angularVelocity": -5.186846153220684e-17, + "velocityX": 0.5407254266512643, + "velocityY": 0.04318542973123231, + "timestamp": 0.11476964417035279 + }, + { + "x": 0.3719447806630506, + "y": 6.869769136338039, + "heading": 3.1355686398605083, + "angularVelocity": -5.1868461532349994e-17, + "velocityX": 0.6759611023663795, + "velocityY": 0.053282926635904025, + "timestamp": 0.14346205521294098 + }, + { + "x": 0.3952206321171861, + "y": 6.871578692739439, + "heading": 3.135568639860506, + "angularVelocity": -5.1868461531715824e-17, + "velocityX": 0.8112197828054859, + "velocityY": 0.06306742220841621, + "timestamp": 0.17215446625552916 + }, + { + "x": 0.42237805939934603, + "y": 6.8736594514681, + "heading": 3.1355686398605043, + "angularVelocity": -5.1868461532243226e-17, + "velocityX": 0.9465020991734979, + "velocityY": 0.07251947999676765, + "timestamp": 0.20084687729811734 + }, + { + "x": 0.45341775826418784, + "y": 6.876001262985059, + "heading": 3.1355686398605025, + "angularVelocity": -5.1868461531961763e-17, + "velocityX": 1.081808664276005, + "velocityY": 0.08161780177636822, + "timestamp": 0.22953928834070553 + }, + { + "x": 0.4883404411428946, + "y": 6.878593306214626, + "heading": 3.1355686398605007, + "angularVelocity": -5.186846153181617e-17, + "velocityX": 1.2171400593303083, + "velocityY": 0.09033898286621334, + "timestamp": 0.2582316993832937 + }, + { + "x": 0.5271468357396778, + "y": 6.881424019896891, + "heading": 3.135568639860499, + "angularVelocity": -5.186846153207198e-17, + "velocityX": 1.3524968166384297, + "velocityY": 0.09865722605409555, + "timestamp": 0.2869241104258819 + }, + { + "x": 0.5698376829754908, + "y": 6.884481024296066, + "heading": 3.135568639860497, + "angularVelocity": -5.18684615321628e-17, + "velocityX": 1.4878793968358233, + "velocityY": 0.10654400547371551, + "timestamp": 0.3156165214684701 + }, + { + "x": 0.6164137340738928, + "y": 6.887751031517755, + "heading": 3.1355686398604954, + "angularVelocity": -5.186846153204876e-17, + "velocityX": 1.6232881589932495, + "velocityY": 0.11396766959864212, + "timestamp": 0.34430893251105826 + }, + { + "x": 0.666875746517318, + "y": 6.891219742296381, + "heading": 3.1355686398604936, + "angularVelocity": -5.186846153160267e-17, + "velocityX": 1.758723321247663, + "velocityY": 0.12089296969422078, + "timestamp": 0.37300134355364645 + }, + { + "x": 0.7212244785112947, + "y": 6.894871726616115, + "heading": 3.135568639860492, + "angularVelocity": -5.186846153215831e-17, + "velocityX": 1.894184908800638, + "velocityY": 0.1272804963763128, + "timestamp": 0.40169375459623463 + }, + { + "x": 0.7794606814696723, + "y": 6.898690284891088, + "heading": 3.13556863986049, + "angularVelocity": -5.186846153213201e-17, + "velocityX": 2.02967268494572, + "velocityY": 0.1330860020548787, + "timestamp": 0.4303861656388228 + }, + { + "x": 0.8415850898614354, + "y": 6.9026572856068995, + "heading": 3.1355686398604883, + "angularVelocity": -5.186846153171342e-17, + "velocityX": 2.165186059113343, + "velocityY": 0.13825958055332968, + "timestamp": 0.459078576681411 + }, + { + "x": 0.9075984075182041, + "y": 6.906752974251311, + "heading": 3.1355686398604865, + "angularVelocity": -5.1868461532457525e-17, + "velocityX": 2.3007239635171715, + "velocityY": 0.14274466646709222, + "timestamp": 0.4877709877239992 + }, + { + "x": 0.9775012891595924, + "y": 6.910955746947304, + "heading": 3.1355686398604847, + "angularVelocity": -5.186846153166985e-17, + "velocityX": 2.43628468648487, + "velocityY": 0.14647680495568935, + "timestamp": 0.5164633987665874 + }, + { + "x": 1.051294315403438, + "y": 6.915241880318482, + "heading": 3.1355686398604825, + "angularVelocity": -5.1868461532007456e-17, + "velocityX": 2.571865645390099, + "velocityY": 0.1493821263335398, + "timestamp": 0.5451558098091757 + }, + { + "x": 1.1289779588157673, + "y": 6.919585206580955, + "heading": 3.1355686398604807, + "angularVelocity": -5.186846153206435e-17, + "velocityX": 2.7074630743656276, + "velocityY": 0.15137543708073178, + "timestamp": 0.5738482208517639 + }, + { + "x": 1.210552537505391, + "y": 6.923956719399065, + "heading": 3.135568639860479, + "angularVelocity": -5.1868461531968826e-17, + "velocityX": 2.843071590203452, + "velocityY": 0.1523578067950584, + "timestamp": 0.6025406318943521 + }, + { + "x": 1.29601815119489, + "y": 6.9283240912670765, + "heading": 3.135568639860477, + "angularVelocity": -5.186846153216223e-17, + "velocityX": 2.978683581614699, + "velocityY": 0.15221348465724693, + "timestamp": 0.6312330429369404 + }, + { + "x": 1.3853745923008893, + "y": 6.932651076487761, + "heading": 3.1355686398604754, + "angularVelocity": -5.1868461531841376e-17, + "velocityX": 3.1142883382426785, + "velocityY": 0.15080591220658612, + "timestamp": 0.6599254539795286 + }, + { + "x": 1.478621220826743, + "y": 6.936896764298285, + "heading": 3.1355686398604736, + "angularVelocity": -5.186846153215644e-17, + "velocityX": 3.2498707894378844, + "velocityY": 0.1479725006107481, + "timestamp": 0.6886178650221169 + }, + { + "x": 1.5757567859462194, + "y": 6.941014632922591, + "heading": 3.135568639860472, + "angularVelocity": -5.186846153173054e-17, + "velocityX": 3.3854096463101833, + "velocityY": 0.14351769247253987, + "timestamp": 0.7173102760647051 + }, + { + "x": 1.676779167520274, + "y": 6.94495133505747, + "heading": 3.13556863986047, + "angularVelocity": -5.186846153388186e-17, + "velocityX": 3.52087461120313, + "velocityY": 0.13720360164351214, + "timestamp": 0.7460026871072933 + }, + { + "x": 1.7816849946975712, + "y": 6.948645114898683, + "heading": 3.1355686398604683, + "angularVelocity": -1.6596882408000968e-16, + "velocityX": 3.6562220937649346, + "velocityY": 0.12873717150261788, + "timestamp": 0.7746950981498816 + }, + { + "x": 1.8121461784699802, + "y": 6.949692765946351, + "heading": 3.1362037838100756, + "angularVelocity": 0.07689012502825784, + "velocityX": 3.687611651202223, + "velocityY": 0.12682797354934053, + "timestamp": 0.7829555080448543 + }, + { + "x": 1.8428693964358074, + "y": 6.950724930078729, + "heading": 3.1374589452138397, + "angularVelocity": 0.15194904607940982, + "velocityX": 3.7193333450103565, + "velocityY": 0.12495313737441759, + "timestamp": 0.7912159179398269 + }, + { + "x": 1.8738574968140793, + "y": 6.95174197296817, + "heading": 3.139318096432083, + "angularVelocity": 0.22506767120320992, + "velocityX": 3.751399842413374, + "velocityY": 0.1231225692636009, + "timestamp": 0.7994763278347996 + }, + { + "x": 1.9051134303883683, + "y": 6.952744343319344, + "heading": 3.1417642609142864, + "angularVelocity": 0.2961311258529596, + "velocityX": 3.783823559810237, + "velocityY": 0.12134632105632295, + "timestamp": 0.8077367377297723 + }, + { + "x": 1.9366402470506743, + "y": 6.953732574884012, + "heading": 3.1447794649491048, + "angularVelocity": 0.36501869436959006, + "velocityX": 3.8166164952046153, + "velocityY": 0.11963468849991928, + "timestamp": 0.815997147624745 + }, + { + "x": 1.96844109067084, + "y": 6.9547072895390265, + "heading": 3.148344689757031, + "angularVelocity": 0.43160386146187707, + "velocityX": 3.849790025494764, + "velocityY": 0.117998339961998, + "timestamp": 0.8242575575197176 + }, + { + "x": 2.0005191919654135, + "y": 6.955669201722795, + "heading": 3.152439824908413, + "angularVelocity": 0.495754472653526, + "velocityX": 3.883354664287875, + "velocityY": 0.11644848088619933, + "timestamp": 0.8325179674146903 + }, + { + "x": 2.032877859008433, + "y": 6.9566191245667985, + "heading": 3.1570436241813726, + "angularVelocity": 0.5573330296555058, + "velocityX": 3.91731977643278, + "velocityY": 0.11499705899340439, + "timestamp": 0.840778377309663 + }, + { + "x": 2.065520465002766, + "y": 6.957557978090485, + "heading": 3.1621336650538163, + "angularVelocity": 0.6161971303075195, + "velocityX": 3.951693246384564, + "velocityY": 0.11365701407324179, + "timestamp": 0.8490387872046357 + }, + { + "x": 2.0984504329177724, + "y": 6.958486799841183, + "heading": 3.1676863130152344, + "angularVelocity": 0.6722000520579139, + "velocityX": 3.9864810988430395, + "velocityY": 0.11244257397721802, + "timestamp": 0.8572991970996083 + }, + { + "x": 2.1316712166001497, + "y": 6.959406758344203, + "heading": 3.1736766917418935, + "angularVelocity": 0.7251914617828771, + "velocityX": 4.021687071799445, + "velocityY": 0.1113695948160438, + "timestamp": 0.865559606994581 + }, + { + "x": 2.165186277981829, + "y": 6.960319169668765, + "heading": 3.1800786598480077, + "angularVelocity": 0.7750182118701426, + "velocityX": 4.057312144046869, + "velocityY": 0.11045593816320934, + "timestamp": 0.8738200168895537 + }, + { + "x": 2.1989990600414004, + "y": 6.961225517299853, + "heading": 3.1868647943382302, + "angularVelocity": 0.8215251514763765, + "velocityX": 4.093354021105941, + "velocityY": 0.10972187126423899, + "timestamp": 0.8820804267845264 + }, + { + "x": 2.2331129552203137, + "y": 6.962127475323903, + "heading": 3.19400637995892, + "angularVelocity": 0.8645558406293681, + "velocityX": 4.1298065849823535, + "velocityY": 0.10919046821108658, + "timestamp": 0.890340836679499 + }, + { + "x": 2.2675312690432237, + "y": 6.963026934685406, + "heading": 3.201473402262516, + "angularVelocity": 0.9039529997346885, + "velocityX": 4.1666593135838506, + "velocityY": 0.10888798170273117, + "timestamp": 0.8986012465744717 + }, + { + "x": 2.302257178726397, + "y": 6.963926031963223, + "heading": 3.2092345401896574, + "angularVelocity": 0.9395584511933502, + "velocityX": 4.203896673978105, + "velocityY": 0.10884414808067534, + "timestamp": 0.9068616564694444 + }, + { + "x": 2.3372936865473584, + "y": 6.964827179782931, + "heading": 3.2172571510612844, + "angularVelocity": 0.9712122005605541, + "velocityX": 4.241497488191654, + "velocityY": 0.10909238538572823, + "timestamp": 0.9151220663644171 + }, + { + "x": 2.372643567633051, + "y": 6.965733097685614, + "heading": 3.225507236598066, + "angularVelocity": 0.9987501397242283, + "velocityX": 4.279434257518503, + "velocityY": 0.10966984861524744, + "timestamp": 0.9233824762593897 + }, + { + "x": 2.4083093114869696, + "y": 6.9666468421059164, + "heading": 3.233949372180996, + "angularVelocity": 1.0219995969049733, + "velocityX": 4.31767240456481, + "velocityY": 0.11061732189065114, + "timestamp": 0.9316428861543624 + }, + { + "x": 2.44429305579275, + "y": 6.967571834188082, + "heading": 3.2425465717794997, + "angularVelocity": 1.0407715486052915, + "velocityX": 4.356169338240552, + "velocityY": 0.11197895672538041, + "timestamp": 0.9399032960493351 + }, + { + "x": 2.480596509367599, + "y": 6.968511884597095, + "heading": 3.251260045781759, + "angularVelocity": 1.054847654420144, + "velocityX": 4.394873140247142, + "velocityY": 0.11380190825438415, + "timestamp": 0.9481637059443078 + }, + { + "x": 2.517220857768171, + "y": 6.969471215284249, + "heading": 3.260048785175124, + "angularVelocity": 1.0639592350877263, + "velocityX": 4.433720464992984, + "velocityY": 0.11613596653794503, + "timestamp": 0.9564241158392804 + }, + { + "x": 2.55416663843719, + "y": 6.970454479070239, + "heading": 3.2688688682505744, + "angularVelocity": 1.0677536814268536, + "velocityX": 4.4726328522150816, + "velocityY": 0.11903329235341958, + "timestamp": 0.9646845257342531 + }, + { + "x": 2.5914335596417093, + "y": 6.971466777824258, + "heading": 3.2776723283732965, + "angularVelocity": 1.065741317277861, + "velocityX": 4.511509922431134, + "velocityY": 0.12254824722845813, + "timestamp": 0.9729449356292258 + }, + { + "x": 2.6290202140808088, + "y": 6.9725136756896955, + "heading": 3.286405336491996, + "angularVelocity": 1.0572124422077387, + "velocityX": 4.550216625687468, + "velocityY": 0.12673679378462502, + "timestamp": 0.9812053455241985 + }, + { + "x": 2.9717276780335036, + "y": 6.984363087388876, + "heading": 3.35108767309623, + "angularVelocity": 0.9203746590757935, + "velocityX": 4.876435853393211, + "velocityY": 0.16860705449782717, + "timestamp": 1.0514836131475132 + }, + { + "x": 3.3366815448761877, + "y": 6.999743371642888, + "heading": 3.3973428408952557, + "angularVelocity": 0.6581717131524953, + "velocityX": 5.192983253355129, + "velocityY": 0.21884836912099448, + "timestamp": 1.1217618807708278 + }, + { + "x": 3.71402485435163, + "y": 7.027526643127482, + "heading": 3.3973428368677423, + "angularVelocity": -5.730809398962461e-8, + "velocityX": 5.369274488921224, + "velocityY": 0.3953323327989187, + "timestamp": 1.1920401483941425 + }, + { + "x": 4.090360227255461, + "y": 7.066661937541599, + "heading": 3.3973428243317834, + "angularVelocity": -1.783760359331945e-7, + "velocityX": 5.354932408421835, + "velocityY": 0.5568619679682045, + "timestamp": 1.2623184160174572 + }, + { + "x": 4.466694898854889, + "y": 7.105803975313799, + "heading": 3.3973428117958613, + "angularVelocity": -1.7837551225317533e-7, + "velocityX": 5.354922429456401, + "velocityY": 0.5569579202208789, + "timestamp": 1.3325966836407719 + }, + { + "x": 4.8430291421579215, + "y": 7.144950130722809, + "heading": 3.397342799228156, + "angularVelocity": -1.7882775854425254e-7, + "velocityX": 5.354916335162799, + "velocityY": 0.5570165106920909, + "timestamp": 1.4028749512640866 + }, + { + "x": 5.214361577079249, + "y": 7.183676617694927, + "heading": 3.378511132398759, + "angularVelocity": -0.26795860891639617, + "velocityX": 5.283744854264688, + "velocityY": 0.55104498562348, + "timestamp": 1.4731532188874013 + }, + { + "x": 5.562524060208366, + "y": 7.219962031584869, + "heading": 3.355414523374288, + "angularVelocity": -0.32864511043819594, + "velocityX": 4.9540561385951625, + "velocityY": 0.5163105909843465, + "timestamp": 1.543431486510716 + }, + { + "x": 5.887470499962918, + "y": 7.253817931255832, + "heading": 3.331666055733143, + "angularVelocity": -0.3379205043646441, + "velocityX": 4.6237115788942855, + "velocityY": 0.48174066914158636, + "timestamp": 1.6137097541340306 + }, + { + "x": 6.189199807150463, + "y": 7.285249171486667, + "heading": 3.308363686841363, + "angularVelocity": -0.33157289841972754, + "velocityX": 4.29335152091094, + "velocityY": 0.4472398266744939, + "timestamp": 1.6839880217573453 + }, + { + "x": 6.467713626267079, + "y": 7.31425833816948, + "heading": 3.2860397702782755, + "angularVelocity": -0.3176503536305448, + "velocityX": 3.963014862708697, + "velocityY": 0.41277577925367004, + "timestamp": 1.75426628938066 + }, + { + "x": 6.723013594809074, + "y": 7.34084703024564, + "heading": 3.265008761402615, + "angularVelocity": -0.29925337642620564, + "velocityX": 3.6327015046867763, + "velocityY": 0.37833448340917136, + "timestamp": 1.8245445570039747 + }, + { + "x": 6.955101065031071, + "y": 7.365016333172441, + "heading": 3.245478308115181, + "angularVelocity": -0.27790174612893576, + "velocityX": 3.302407388098493, + "velocityY": 0.34390863269912847, + "timestamp": 1.8948228246272893 + }, + { + "x": 7.163977127241494, + "y": 7.386767031992689, + "heading": 3.2275957745381954, + "angularVelocity": -0.2544532496565816, + "velocityX": 2.972128785672115, + "velocityY": 0.3094939524808096, + "timestamp": 1.965101092250604 + }, + { + "x": 7.349642665971283, + "y": 7.4060997209757735, + "heading": 3.2114711639593976, + "angularVelocity": -0.22943949992088322, + "velocityX": 2.6418627693690517, + "velocityY": 0.2750877282106925, + "timestamp": 2.035379359873919 + }, + { + "x": 7.512098408217057, + "y": 7.423014865697383, + "heading": 3.1971897253730983, + "angularVelocity": -0.2032127294720464, + "velocityX": 2.3116070976097918, + "velocityY": 0.24068812868678965, + "timestamp": 2.1056576274972336 + }, + { + "x": 7.651344959888608, + "y": 7.43751284079522, + "heading": 3.184819461924971, + "angularVelocity": -0.17601833207433357, + "velocityX": 1.981360047431725, + "velocityY": 0.20629385993890986, + "timestamp": 2.1759358951205483 + }, + { + "x": 7.767382832665261, + "y": 7.449593954241994, + "heading": 3.174415882660127, + "angularVelocity": -0.14803408815663477, + "velocityX": 1.6511202780154728, + "velocityY": 0.1719039733808395, + "timestamp": 2.246214162743863 + }, + { + "x": 7.860212463836391, + "y": 7.459258463658005, + "heading": 3.1660251568616267, + "angularVelocity": -0.11939289459260129, + "velocityX": 1.3208867308552614, + "velocityY": 0.13751775254062693, + "timestamp": 2.3164924303671777 + }, + { + "x": 7.929834231127341, + "y": 7.466506587678373, + "heading": 3.159686290431417, + "angularVelocity": -0.09019668020541635, + "velocityX": 0.9906585584055106, + "velocityY": 0.10313464269233455, + "timestamp": 2.3867706979904924 + }, + { + "x": 7.976248463934981, + "y": 7.471338514116222, + "heading": 3.1554326760982865, + "angularVelocity": -0.06052531567696676, + "velocityX": 0.6604350729932147, + "velocityY": 0.06875420526505516, + "timestamp": 2.457048965613807 + }, + { + "x": 7.999455451965332, + "y": 7.473754405975342, + "heading": 3.153293227446569, + "angularVelocity": -0.03044253542482025, + "velocityX": 0.33021570985126986, + "velocityY": 0.03437608724320182, + "timestamp": 2.5273272332371217 + }, + { + "x": 7.999455451965332, + "y": 7.473754405975342, + "heading": 3.153293227446569, + "angularVelocity": -3.79407884851814e-18, + "velocityX": 2.546987187300363e-16, + "velocityY": -1.0256209789219734e-16, + "timestamp": 2.5976055008604364 + }, + { + "x": 7.9777911181971035, + "y": 7.466979623755239, + "heading": 3.153293227446569, + "angularVelocity": 4.0010921973360205e-29, + "velocityX": -0.31261406582002, + "velocityY": -0.09775939742848679, + "timestamp": 2.6669060734735086 + }, + { + "x": 7.934462450883723, + "y": 7.453430059384792, + "heading": 3.153293227446569, + "angularVelocity": -2.051733815514594e-34, + "velocityX": -0.6252281284210741, + "velocityY": -0.1955187938503514, + "timestamp": 2.736206646086581 + }, + { + "x": 7.869469450281316, + "y": 7.433105712944096, + "heading": 3.153293227446569, + "angularVelocity": -1.98522229630168e-34, + "velocityX": -0.9378421873262779, + "velocityY": -0.29327818911646475, + "timestamp": 2.805507218699653 + }, + { + "x": 7.782812116686986, + "y": 7.40600658452606, + "heading": 3.153293227446569, + "angularVelocity": -1.9190015159804115e-34, + "velocityX": -1.2504562419442953, + "velocityY": -0.39103758304190667, + "timestamp": 2.8748077913127252 + }, + { + "x": 7.6744904504495075, + "y": 7.372132674239751, + "heading": 3.153293227446569, + "angularVelocity": -8.200637720207044e-29, + "velocityX": -1.5630702915295283, + "velocityY": -0.4887969753935167, + "timestamp": 2.9441083639257974 + }, + { + "x": 7.544504451984089, + "y": 7.331483982215012, + "heading": 3.153293227446569, + "angularVelocity": 6.610929004011549e-29, + "velocityX": -1.8756843351233508, + "velocityY": -0.5865563658715173, + "timestamp": 3.0134089365388697 + }, + { + "x": 7.392854121793352, + "y": 7.28406050860902, + "heading": 3.153293227446569, + "angularVelocity": -6.483737543755527e-29, + "velocityX": -2.188298371464413, + "velocityY": -0.6843157540814643, + "timestamp": 3.082709509151942 + }, + { + "x": 7.219539460498179, + "y": 7.229862253615935, + "heading": 3.153293227446569, + "angularVelocity": 8.073376116568391e-29, + "velocityX": -2.5009123988461828, + "velocityY": -0.7820751394896976, + "timestamp": 3.152010081765014 + }, + { + "x": 7.024560468885024, + "y": 7.168889217481695, + "heading": 3.153293227446569, + "angularVelocity": -3.526812750348743e-30, + "velocityX": -2.813526414879514, + "velocityY": -0.8798345213490937, + "timestamp": 3.2213106543780863 + }, + { + "x": 6.807917147982327, + "y": 7.101141400527908, + "heading": 3.153293227446569, + "angularVelocity": -9.438045628849687e-29, + "velocityX": -3.1261404160725776, + "velocityY": -0.9775938985677018, + "timestamp": 3.2906112269911585 + }, + { + "x": 6.569609499192503, + "y": 7.026618803193132, + "heading": 3.153293227446569, + "angularVelocity": 9.790681244582267e-29, + "velocityX": -3.438754397028912, + "velocityY": -1.0753532694579622, + "timestamp": 3.3599117996042307 + }, + { + "x": 6.309637524541267, + "y": 6.94532142611084, + "heading": 3.153293227446569, + "angularVelocity": -1.3895984503554753e-34, + "velocityX": -3.7513683487544087, + "velocityY": -1.1731126312072742, + "timestamp": 3.429212372217303 + }, + { + "x": 6.028001227211883, + "y": 6.85724927027649, + "heading": 3.153293227446569, + "angularVelocity": -1.323503512813964e-34, + "velocityX": -4.063982254545725, + "velocityY": -1.2708719785922367, + "timestamp": 3.498512944830375 + }, + { + "x": 5.724700612934229, + "y": 6.762402337481905, + "heading": 3.153293227446569, + "angularVelocity": -2.1989550257903952e-29, + "velocityX": -4.376596077655505, + "velocityY": -1.3686313001213652, + "timestamp": 3.5678135174434473 + }, + { + "x": 5.399735695078014, + "y": 6.660780631908008, + "heading": 3.153293227446569, + "angularVelocity": 1.8024841235265752e-29, + "velocityX": -4.689209707841794, + "velocityY": -1.4663905613202457, + "timestamp": 3.6371140900565195 + }, + { + "x": 5.053106540487171, + "y": 6.552384174457959, + "heading": 3.153293227446569, + "angularVelocity": 1.896211807861914e-29, + "velocityX": -5.001822373477154, + "velocityY": -1.564149520888688, + "timestamp": 3.7064146626695917 + }, + { + "x": 4.7281416226309565, + "y": 6.4507624688840615, + "heading": 3.153293227446569, + "angularVelocity": -1.4997927313419928e-29, + "velocityX": -4.689209707841795, + "velocityY": -1.4663905613202457, + "timestamp": 3.775715235282664 + }, + { + "x": 4.424841008353304, + "y": 6.355915536089477, + "heading": 3.153293227446569, + "angularVelocity": -4.4185947102012545e-35, + "velocityX": -4.376596077655505, + "velocityY": -1.3686313001213652, + "timestamp": 3.845015807895736 + }, + { + "x": 4.14320471102392, + "y": 6.267843380255127, + "heading": 3.153293227446569, + "angularVelocity": -9.264733448000688e-35, + "velocityX": -4.063982254545726, + "velocityY": -1.2708719785922367, + "timestamp": 3.9143163805088084 + }, + { + "x": 3.883232736372683, + "y": 6.186546003172835, + "heading": 3.153293227446569, + "angularVelocity": 3.5265482976182584e-30, + "velocityX": -3.751368348754409, + "velocityY": -1.1731126312072742, + "timestamp": 3.9836169531218806 + }, + { + "x": 3.644925087582859, + "y": 6.112023405838059, + "heading": 3.153293227446569, + "angularVelocity": 2.156768838744686e-29, + "velocityX": -3.438754397028912, + "velocityY": -1.0753532694579622, + "timestamp": 4.052917525734953 + }, + { + "x": 3.4282817666801613, + "y": 6.044275588884272, + "heading": 3.153293227446569, + "angularVelocity": -2.105657187574439e-28, + "velocityX": -3.1261404160725776, + "velocityY": -0.9775938985677018, + "timestamp": 4.122218098348026 + }, + { + "x": 3.2333027750670063, + "y": 5.983302552750032, + "heading": 3.153293227446569, + "angularVelocity": 1.8547117767579628e-28, + "velocityX": -2.813526414879514, + "velocityY": -0.8798345213490937, + "timestamp": 4.1915186709610985 + }, + { + "x": 3.059988113771833, + "y": 5.929104297756947, + "heading": 3.153293227446569, + "angularVelocity": -5.955568710068408e-35, + "velocityX": -2.5009123988461828, + "velocityY": -0.7820751394896976, + "timestamp": 4.260819243574171 + }, + { + "x": 2.908337783581096, + "y": 5.881680824150955, + "heading": 3.153293227446569, + "angularVelocity": -2.0810713408746708e-29, + "velocityX": -2.1882983714644135, + "velocityY": -0.6843157540814642, + "timestamp": 4.330119816187244 + }, + { + "x": 2.7783517851156776, + "y": 5.841032132126216, + "heading": 3.153293227446569, + "angularVelocity": 2.081061415014159e-29, + "velocityX": -1.875684335123351, + "velocityY": -0.5865563658715173, + "timestamp": 4.3994203888003165 + }, + { + "x": 2.6700301188781994, + "y": 5.807158221839908, + "heading": 3.153293227446569, + "angularVelocity": 7.326893682940581e-29, + "velocityX": -1.5630702915295283, + "velocityY": -0.48879697539351663, + "timestamp": 4.468720961413389 + }, + { + "x": 2.583372785283869, + "y": 5.780059093421871, + "heading": 3.153293227446569, + "angularVelocity": -7.326900961882318e-29, + "velocityX": -1.2504562419442953, + "velocityY": -0.39103758304190656, + "timestamp": 4.538021534026462 + }, + { + "x": 2.5183797846814615, + "y": 5.759734746981175, + "heading": 3.153293227446569, + "angularVelocity": -2.64698564160025e-35, + "velocityX": -0.9378421873262781, + "velocityY": -0.2932781891164647, + "timestamp": 4.6073221066395345 + }, + { + "x": 2.4750511173680816, + "y": 5.746185182610728, + "heading": 3.153293227446569, + "angularVelocity": -4.5099737006589516e-29, + "velocityX": -0.6252281284210743, + "velocityY": -0.1955187938503513, + "timestamp": 4.676622679252607 + }, + { + "x": 2.4533867835998535, + "y": 5.739410400390625, + "heading": 3.153293227446569, + "angularVelocity": 1.9207053764808128e-28, + "velocityX": -0.31261406582002027, + "velocityY": -0.09775939742848665, + "timestamp": 4.74592325186568 + }, + { + "x": 2.4533867835998535, + "y": 5.739410400390625, + "heading": 3.153293227446569, + "angularVelocity": -4.899028012023128e-29, + "velocityX": 1.275108562409782e-22, + "velocityY": 3.987692290669026e-23, + "timestamp": 4.815223824478752 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/1M Auto.auto b/src/main/deploy/pathplanner/autos/1M Auto.auto deleted file mode 100644 index 77afe835..00000000 --- a/src/main/deploy/pathplanner/autos/1M Auto.auto +++ /dev/null @@ -1,19 +0,0 @@ -{ - "version": 1.0, - "startingPose": null, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "1M Path" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Example Auto.auto b/src/main/deploy/pathplanner/autos/ChoreoTest2.auto similarity index 53% rename from src/main/deploy/pathplanner/autos/Example Auto.auto rename to src/main/deploy/pathplanner/autos/ChoreoTest2.auto index fabd6e60..4f6033f3 100644 --- a/src/main/deploy/pathplanner/autos/Example Auto.auto +++ b/src/main/deploy/pathplanner/autos/ChoreoTest2.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.3842846515935527, - "y": 5.4806715034257465 + "x": 0.3137660622596754, + "y": 6.865104675292969 }, - "rotation": 0.0 + "rotation": 179.6548494375836 }, "command": { "type": "sequential", @@ -14,18 +14,24 @@ { "type": "path", "data": { - "pathName": "Example Path" + "pathName": "Test2.1" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 } }, { "type": "path", "data": { - "pathName": "New Path" + "pathName": "Test2.2" } } ] } }, "folder": null, - "choreoAuto": false + "choreoAuto": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto new file mode 100644 index 00000000..6ce2c5d3 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/New Auto.auto @@ -0,0 +1,37 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.4869295358657837, + "y": 5.564249038696289 + }, + "rotation": 179.96604345059157 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Test1.1" + } + }, + { + "type": "wait", + "data": { + "waitTime": 5.0 + } + }, + { + "type": "path", + "data": { + "pathName": "Test1.2" + } + } + ] + } + }, + "folder": null, + "choreoAuto": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json index 690f5db2..bab0da93 100644 --- a/src/main/deploy/pathplanner/navgrid.json +++ b/src/main/deploy/pathplanner/navgrid.json @@ -1,1633 +1 @@ -{ - "field_size": { - "x": 16.54, - "y": 8.21 - }, - "nodeSizeMeters": 0.3, - "grid": [ - [ - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true - ], - [ - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true - ], - [ - true, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - true, - true - ], - [ - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true - ], - [ - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true - ], - [ - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true - ], - [ - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true - ], - [ - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true - ], - [ - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true - ], - [ - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true - ], - [ - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true - ], - [ - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - false, - false, - false, - false, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - false, - false, - false, - false, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true - ], - [ - true, - true, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true - ], - [ - true, - true, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true - ], - [ - true, - true, - true, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true - ], - [ - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - true, - true, - true, - false, - false, - false, - false, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - false, - false, - false, - false, - true, - true, - true, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true - ], - [ - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - true - ], - [ - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - true - ], - [ - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - true - ], - [ - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - true - ], - [ - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - true - ], - [ - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true - ], - [ - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true - ], - [ - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true - ], - [ - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true - ], - [ - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true - ], - [ - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true - ], - [ - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true - ] - ] -} +{"field_size":{"x":16.54,"y":8.21},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true],[true,true,true,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/1M Path.path b/src/main/deploy/pathplanner/paths/1M Path.path deleted file mode 100644 index 7630c6eb..00000000 --- a/src/main/deploy/pathplanner/paths/1M Path.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.34, - "y": 5.55 - }, - "prevControl": null, - "nextControl": { - "x": 2.34, - "y": 5.55 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.34, - "y": 5.55 - }, - "prevControl": { - "x": 1.3399999999999999, - "y": 5.55 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.10655751049247228, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path deleted file mode 100644 index 25b75972..00000000 --- a/src/main/deploy/pathplanner/paths/Example Path.path +++ /dev/null @@ -1,73 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.3842846515935527, - "y": 5.4806715034257465 - }, - "prevControl": null, - "nextControl": { - "x": 1.9229831326943445, - "y": 5.831996599796616 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.625633325446393, - "y": 6.733731013824367 - }, - "prevControl": { - "x": 2.239175719435603, - "y": 6.2887192250846695 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "TopSpike" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.5, - "rotationDegrees": 48.06970382251976, - "rotateFast": false - } - ], - "constraintZones": [], - "eventMarkers": [ - { - "name": "Run Intake", - "waypointRelativePos": 0.55, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Run Intake" - } - } - ] - } - } - } - ], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0.0, - "rotation": 47.91083782616781, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New New New Path.path b/src/main/deploy/pathplanner/paths/New New New Path.path deleted file mode 100644 index 1d7579f5..00000000 --- a/src/main/deploy/pathplanner/paths/New New New Path.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 0.0, - "y": 0.0 - }, - "prevControl": null, - "nextControl": { - "x": 1.0000000000000013, - "y": 0.0 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.0, - "y": 0.0 - }, - "prevControl": { - "x": 0.0, - "y": 0.0 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 90.05397463575794, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path deleted file mode 100644 index 0f1db65f..00000000 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ /dev/null @@ -1,58 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.625633325446393, - "y": 6.733731013824367 - }, - "prevControl": null, - "nextControl": { - "x": 3.7615844703872, - "y": 7.366116187296568 - }, - "isLocked": false, - "linkedName": "TopSpike" - }, - { - "anchor": { - "x": 7.790112242136038, - "y": 7.471513716208602 - }, - "prevControl": { - "x": 5.401101586796611, - "y": 7.436381206571257 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.7, - "rotationDegrees": 0, - "rotateFast": false - } - ], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 46.97493401088208, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 50676892..500e5429 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -45,7 +45,7 @@ private Constants() { throw new IllegalStateException("Constants class should not be constructed"); } - public static final Mode currentMode = Mode.REAL; + public static final Mode currentMode = Mode.SIM; public enum Mode { /** Running on a real robot. */ @@ -70,7 +70,7 @@ private DriveConstants() { // module constants public static final double WHEEL_RADIUS_METERS = Units.inchesToMeters(2.0); - public static final double MAX_LINEAR_SPEED = Units.feetToMeters(18.0); + public static final double MAX_LINEAR_SPEED = Units.feetToMeters(17.8); public static final double TRACK_WIDTH_X = Units.inchesToMeters(18.6); public static final double TRACK_WIDTH_Y = Units.inchesToMeters(18.6); public static final double DRIVE_BASE_RADIUS = @@ -84,35 +84,38 @@ private DriveConstants() { // kP, kI, kD in order public static final double[] TURN_FB_GAINS = new double[]{43.982, 0.0, 0.0}; -// public static final Transform3d RIGHT_CAMERA_TRANSFORMATION = new Transform3d( -// new Translation3d(Units.inchesToMeters(10.5), Units.inchesToMeters(8.5), Units.inchesToMeters(6)), -// new Rotation3d(0.0, Units.degreesToRadians(50), Units.degreesToRadians(-18)) -// ); -// public static final Transform3d LEFT_CAMERA_TRANSFORMATION = new Transform3d( -// new Translation3d(Units.inchesToMeters(10.5), Units.inchesToMeters(-8.5), Units.inchesToMeters(6)), -// new Rotation3d(0.0, Units.degreesToRadians(50), Units.degreesToRadians(18)) -// ); - - public static final Transform3d RIGHT_CAMERA_TRANSFORMATION = new Transform3d( - new Translation3d(Units.inchesToMeters(12.0), Units.inchesToMeters(6.0), Units.inchesToMeters(7.8)), - new Rotation3d(Units.degreesToRadians(0.0), Units.degreesToRadians(20.0), -Units.degreesToRadians(7.5)) + public static final Transform3d LEFT_CAMERA_TRANSFORMATION = new Transform3d( + new Translation3d( + Units.inchesToMeters(11.0351), // 11.0351 + Units.inchesToMeters(10.023204), // 10.023204 + Units.inchesToMeters(7.1374)), // 4.1374 + new Rotation3d( + Units.degreesToRadians(0.0), + Units.degreesToRadians(-30.0), // -120.0 + 91.0 + Units.degreesToRadians(-14.7)) // 165.3224 + 180 ); - public static final Transform3d LEFT_CAMERA_TRANSFORMATION = new Transform3d( - new Translation3d(Units.inchesToMeters(-11.25), Units.inchesToMeters(-9.0), Units.inchesToMeters(6.0)), - new Rotation3d(Units.degreesToRadians(2.0), Units.degreesToRadians(-26.0), Units.degreesToRadians(-35.0 - 180)) + public static final Transform3d RIGHT_CAMERA_TRANSFORMATION = new Transform3d( + new Translation3d( + Units.inchesToMeters(11.0351), //11.0351 + Units.inchesToMeters(-10.023204), //-10.023204 + Units.inchesToMeters(7.1374)), // 7.1374 + new Rotation3d( + Units.degreesToRadians(0.0), + Units.degreesToRadians(-30.0), // -30.0 - 1 + Units.degreesToRadians(14.7)) // 165.3224) ); public static final PathConstraints DEFAULT_CONSTRAINTS = new PathConstraints( - Units.radiansToDegrees(MAX_LINEAR_SPEED), - Units.radiansToDegrees(MAX_LINEAR_SPEED), - MAX_ANGULAR_SPEED, - MAX_ANGULAR_SPEED + MAX_LINEAR_SPEED * 0.25, + MAX_LINEAR_SPEED * 0.25, + MAX_ANGULAR_SPEED * 0.25, + MAX_ANGULAR_SPEED * 0.25 ); public static final HolonomicPathFollowerConfig HOLONOMIC_CONFIG = new HolonomicPathFollowerConfig( - new PIDConstants(5.0),new PIDConstants(5.0), - DriveConstants.MAX_LINEAR_SPEED, DriveConstants.DRIVE_BASE_RADIUS, new ReplanningConfig()); + new PIDConstants(7.5, 0.75), new PIDConstants(7.5, 0.75), + DriveConstants.MAX_LINEAR_SPEED * 0.25, DriveConstants.DRIVE_BASE_RADIUS, new ReplanningConfig()); public static final ModuleConstants FL_MOD_CONSTANTS = new ModuleConstants( 0, @@ -120,7 +123,7 @@ private DriveConstants() { DRIVE_FF_GAINS, DRIVE_FB_GAINS, TURN_FB_GAINS, - Units.rotationsToDegrees(0.472168), // offset 0.457764 + Units.rotationsToDegrees(-0.017822) + 180, // offset 0.457764 true, // inversion ModuleConstants.GearRatios.L3_KRAKEN ); @@ -131,7 +134,7 @@ private DriveConstants() { DRIVE_FF_GAINS, DRIVE_FB_GAINS, TURN_FB_GAINS, - Units.rotationsToDegrees(0.046143), + Units.rotationsToDegrees(-0.453857) + 180, true, ModuleConstants.GearRatios.L3_KRAKEN ); @@ -142,7 +145,7 @@ private DriveConstants() { DRIVE_FF_GAINS, DRIVE_FB_GAINS, TURN_FB_GAINS, - Units.rotationsToDegrees(-0.073730), + Units.rotationsToDegrees(0.428467) + 180, true, ModuleConstants.GearRatios.L3_KRAKEN ); @@ -153,7 +156,7 @@ private DriveConstants() { DRIVE_FF_GAINS, DRIVE_FB_GAINS, TURN_FB_GAINS, - Units.rotationsToDegrees(0.404297), + Units.rotationsToDegrees(-0.093750) + 180, true, ModuleConstants.GearRatios.L3_KRAKEN ); @@ -174,12 +177,14 @@ private ArmConstants() {} * than the lowest point the arm and wrist will move to , and then compensate for that in our encoder reset code */ public static final double OFFSET_NUDGE = 45; public static final double ARM_OFFSET = -0.123779 + Units.degreesToRotations(OFFSET_NUDGE); - public static final double WRIST_OFFSET = -0.163330 + Units.degreesToRotations(OFFSET_NUDGE); - public static final double ARM_SENSOR_MECHANISM_RATIO = (56.0 / 12.0) * (66.0 / 18.0) * (80.0 / 18.0) * (64.0 / 24.0); + public static final double WRIST_OFFSET = 0.000000 + Units.degreesToRotations(OFFSET_NUDGE); + public static final double ARM_SENSOR_MECHANISM_RATIO = + (56.0 / 12.0) * (66.0 / 18.0) * (80.0 / 18.0) * (64.0 / 24.0); public static final double ARM_CANCODER_MECHANISM_RATIO = (26.0 / 36.0) * (64.0 / 24.0); // the pulley on the encoder is a 1:1 - public static final double WRIST_SENSOR_MECHANISM_RATIO = (56.0 / 12.0) * (66.0 / 18.0) * (80.0 / 18.0) * (48.0 / 24.0); + public static final double WRIST_SENSOR_MECHANISM_RATIO = + (56.0 / 12.0) * (66.0 / 18.0) * (80.0 / 18.0) * (48.0 / 24.0); public static final double WRIST_CANCODER_MECHANISM_RATIO = (48.0 / 24.0); public static final double WRIST_KP = 108.0; @@ -233,10 +238,12 @@ private ArmSetpoints() { public static final ArmPose STOW_SETPOINT = new ArmPose("ArmPoses/Stow", true, 0.0, 45.0); public static final ArmPose INTAKE_SETPOINT = - new ArmPose("ArmPoses/Intake", true, 0.0, 55.0); + new ArmPose("ArmPoses/Intake", true, -7.0, 55.0); public static final ArmPose AMP_SETPOINT = new ArmPose("ArmPoses/Amp", true, 94.0, 145.0); + public static final ArmPose STATIC_SHOOTER = new ArmPose(0.0, 55.0); + public static final GosDoubleProperty WRIST_ANGLE = new GosDoubleProperty(false, "Wrist Angle", 45.0); public static final Trajectory STOW_AMP_TRAJ; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index cc114c87..3f35eda3 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -25,6 +25,12 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import lib.factories.TalonFXFactory; import lib.logger.DataLogUtil; +import org.littletonrobotics.junction.LogFileUtil; +import org.littletonrobotics.junction.LoggedRobot; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.networktables.NT4Publisher; +import org.littletonrobotics.junction.wpilog.WPILOGReader; +import org.littletonrobotics.junction.wpilog.WPILOGWriter; /** * The VM is configured to automatically run this class, and to call the functions corresponding to @@ -32,7 +38,7 @@ * the package after creating this project, you must also update the build.gradle file in the * project. */ -public class Robot extends TimedRobot { +public class Robot extends LoggedRobot { private Command autonomousCommand; private RobotContainer robotContainer; private PowerDistribution pdp; @@ -48,8 +54,8 @@ public void robotInit() { String logPath = "/media/sda1/aoide"; - DataLogManager.start(logPath); - DriverStation.startDataLog(DataLogManager.getLog()); +// DataLogManager.start(logPath); +// DriverStation.startDataLog(DataLogManager.getLog()); SignalLogger.setPath(logPath); SignalLogger.start(); @@ -60,14 +66,43 @@ public void robotInit() { pdp = new PowerDistribution(); - final StringLogEntry entry = new StringLogEntry(DataLogManager.getLog(), "/ntlog"); - NetworkTableInstance.getDefault() - .addLogger( - 0, - 100, - event -> - entry.append( - event.logMessage.filename + ":" + event.logMessage.line + ":" + event.logMessage.message)); + // Record metadata + Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); + Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE); + Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA); + Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE); + Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH); + switch (BuildConstants.DIRTY) { + case 0: + Logger.recordMetadata("GitDirty", "All changes committed"); + break; + case 1: + Logger.recordMetadata("GitDirty", "Uncomitted changes"); + break; + default: + Logger.recordMetadata("GitDirty", "Unknown"); + break; + } + + switch (Constants.currentMode) { + case REAL -> { + Logger.addDataReceiver(new WPILOGWriter(logPath)); // Log to a USB stick ("/U/logs") + Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables + } + case SIM -> { + Logger.addDataReceiver(new NT4Publisher()); + } + case REPLAY -> { + setUseTiming(false); // Run as fast as possible + String replayLog = LogFileUtil.findReplayLog(); // Pull the replay log from AdvantageScope (or prompt the user) + Logger.setReplaySource(new WPILOGReader(replayLog)); // Read replay log + // Save outputs to a new log + Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(replayLog, "_sim"))); + } + } + +// Logger.disableDeterministicTimestamps() // See "Deterministic Timestamps" in the "Understanding Data Flow" page + Logger.start(); // Start logging! No more data receivers, replay sources, or metadata values may be added. } /** This function is called periodically during all modes. */ @@ -80,7 +115,7 @@ public void robotPeriodic() { // the Command-based framework to work. CommandScheduler.getInstance().run(); TalonFXFactory.handleFaults(); - DataLogUtil.updateTables(); +// DataLogUtil.updateTables(); } /** This function is called once when the robot is disabled. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6117e9ab..d216dec6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,10 +13,11 @@ package frc.robot; +import com.ctre.phoenix6.SignalLogger; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; +import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; @@ -26,13 +27,17 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; 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.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants.DriveConstants; import frc.robot.commands.DriveCommands; import frc.robot.commands.FeedForwardCharacterization; +import frc.robot.commands.IntakeControlCommand; +import frc.robot.commands.ShooterAutoCommand; import frc.robot.subsystems.arm.*; import frc.robot.subsystems.climber.ClimberIO; import frc.robot.subsystems.climber.ClimberIOKraken; @@ -84,9 +89,11 @@ public RobotContainer() { new ModuleIOTalonFX(Constants.DriveConstants.BL_MOD_CONSTANTS), new ModuleIOTalonFX(Constants.DriveConstants.BR_MOD_CONSTANTS), new VisionSubsystem[]{ - new VisionSubsystem("RightCamera", DriveConstants.RIGHT_CAMERA_TRANSFORMATION) - }); - m_shooter = new ShooterSubsystem(new ShooterIOKraken()); + new VisionSubsystem("RightCamera", DriveConstants.RIGHT_CAMERA_TRANSFORMATION), + new VisionSubsystem("LeftCamera", DriveConstants.LEFT_CAMERA_TRANSFORMATION) + } + ); + m_shooter = new ShooterSubsystem(new ShooterIOKraken(), m_driveSubsystem::getVisionPose); m_armSubsystem = new ArmSubsystem(new ArmIOKraken(), m_driveSubsystem::getVisionPose); m_climber = new ClimberSubsystem(new ClimberIOKraken() {}); } @@ -123,7 +130,7 @@ public RobotContainer() { new ModuleIOSim(DriveConstants.BL_MOD_CONSTANTS), new ModuleIOSim(DriveConstants.BR_MOD_CONSTANTS)); m_shooter = new ShooterSubsystem(new ShooterIOSim()); - m_armSubsystem = new ArmSubsystem(new ArmIOSim(), m_driveSubsystem::getPose); + m_armSubsystem = new ArmSubsystem(new ArmIOSim()); m_climber = new ClimberSubsystem(new ClimberIO() {}); } default -> { @@ -141,22 +148,30 @@ public RobotContainer() { } } - // Set up auto routines - autoChooser = new SendableChooser<>(); + // configure named commands for auto + configureNamedCommands(); + configureDashboard(); + + autoChooser = AutoBuilder.buildAutoChooser(); // Set up feedforward characterization autoChooser.addOption( - "Drive FF Characterization", - new FeedForwardCharacterization( - m_driveSubsystem, - m_driveSubsystem::runCharacterizationVolts, - m_driveSubsystem::getCharacterizationVelocity)); + "Drive Quasistatic Forward", + m_driveSubsystem.runSysidQuasistatic(SysIdRoutine.Direction.kForward)); + autoChooser.addOption( + "Drive Quasistatic Backwards", + m_driveSubsystem.runSysidQuasistatic(SysIdRoutine.Direction.kReverse)); + autoChooser.addOption( + "Drive Dynamic Forward", + m_driveSubsystem.runSysidDynamic(SysIdRoutine.Direction.kForward)); + autoChooser.addOption( + "Drive Dynamic Backwards", + m_driveSubsystem.runSysidDynamic(SysIdRoutine.Direction.kReverse)); + + SmartDashboard.putData("Auto Chooser", autoChooser); // Configure the button bindings configureButtonBindings(); - // configure named commands for auto - configureNamedCommands(); - configureDashboard(); } /** @@ -166,38 +181,60 @@ public RobotContainer() { * edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { - Trigger intakeTrigger = controller.rightTrigger().and(controller.leftTrigger().negate()); - Trigger spinUpTrigger = controller.leftTrigger().and(controller.rightTrigger().negate()); - Trigger shootTrigger = controller.leftTrigger().and(controller.rightTrigger()); + Trigger intakeTrigger = controller.y().and(controller.x().negate()) + .and(controller.a().negate()) // make sure we don't amp + .and(controller.b().negate()) + .debounce(0.1, Debouncer.DebounceType.kBoth); - intakeTrigger.whileTrue(m_shooter.intakeCommand(0.90, 0.5, 0.25) - .alongWith(m_armSubsystem.setDesiredState(ArmSubsystem.ArmState.INTAKE))); + Trigger spinUpTrigger = controller.x().and(controller.y().negate()) + .and(controller.a().negate()) // make sure we don't amp + .and(controller.b().negate()); - spinUpTrigger.whileTrue(m_shooter.runShooterVelocity(false) - .alongWith(m_armSubsystem.setDesiredState(ArmSubsystem.ArmState.AUTO_AIM))); - shootTrigger.whileTrue(m_shooter.runShooterVelocity(true) - .alongWith(m_armSubsystem.setDesiredState(ArmSubsystem.ArmState.AUTO_AIM))); + Trigger shootTrigger = controller.x().and(controller.y()) + .and(controller.a().negate()) // make sure we don't amp + .and(controller.b().negate()) + .debounce(0.1, Debouncer.DebounceType.kBoth); - controller.x().whileTrue(m_armSubsystem.setDesiredState(ArmSubsystem.ArmState.AMP)); - controller.y().whileTrue(Commands.runEnd(() -> m_shooter.setKickerPower(-0.5), - () -> m_shooter.setKickerPower(0.0), - m_shooter)); + Trigger ampLineupTrigger = controller.b().and(controller.a().negate()) + .debounce(0.1, Debouncer.DebounceType.kBoth); -// controller.leftTrigger().onTrue(m_armSubsystem.setDesiredState(ArmSubsystem.ArmState.AUTO_AIM)) -// .whileTrue(DriveCommands.alignmentDrive( -// m_driveSubsystem, -// () -> -controller.getLeftY(), -// () -> -controller.getLeftX(), -// () -> AllianceFlipUtil.apply(FieldConstants.CENTER_SPEAKER) -// )); + Trigger ampDepositeTrigger = controller.b().and(controller.a()) + .and(spinUpTrigger.negate()) // make sure we don't amp while trying to do anything else + .and(shootTrigger.negate()) + .and(intakeTrigger.negate()) + .debounce(0.1, Debouncer.DebounceType.kBoth); + + intakeTrigger.whileTrue(m_shooter.intakeCommand(0.75, 0.5, 0.1) + .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.INTAKE))); + + spinUpTrigger.whileTrue(m_shooter.runShooterVelocity(false) + .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AUTO_AIM)) + .alongWith(DriveCommands.alignmentDrive( + m_driveSubsystem, + () -> -controller.getLeftY(), + () -> -controller.getLeftX(), + () -> AllianceFlipUtil.apply(FieldConstants.CENTER_SPEAKER) + ))); -// controller.rightTrigger().whileTrue(m_shooter.intakeCommand(0.75, 0.25, 0.1)); -// controller.rightBumper().whileTrue(m_shooter.intakeCommand(0.0, -0.25, 0.1)); + shootTrigger.whileTrue(m_shooter.runShooterVelocity(true) + .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AUTO_AIM)) + .alongWith(DriveCommands.alignmentDrive( + m_driveSubsystem, + () -> -controller.getLeftY(), + () -> -controller.getLeftX(), + () -> AllianceFlipUtil.apply(FieldConstants.CENTER_SPEAKER) + ))); - controller.leftBumper().whileTrue(m_climber.setClimberPowerFactory(0.55)); - controller.rightBumper().whileTrue(m_climber.setClimberPowerFactory(-0.55)); + ampLineupTrigger.whileTrue(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AMP)); + ampDepositeTrigger.whileTrue(Commands.runEnd(() -> m_shooter.setKickerPower(-0.5), + () -> m_shooter.setKickerPower(0.0), + m_shooter) + .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AMP))); - double centerDistance = 1.34 - Units.inchesToMeters(3.0); + controller.pov(0).onTrue(Commands.runOnce(SignalLogger::stop)); + controller.pov(180).whileTrue(m_driveSubsystem.pathfollowFactory( + FieldConstants.AMP_LINEUP + )); m_driveSubsystem.setDefaultCommand( DriveCommands.joystickDrive( @@ -211,7 +248,7 @@ private void configureButtonBindings() { Commands.runOnce( () -> m_driveSubsystem.setPose(AllianceFlipUtil.apply( - new Pose2d(new Translation2d(centerDistance + Units.inchesToMeters(240.25), 5.55), + new Pose2d(m_driveSubsystem.getVisionPose().getTranslation(), Rotation2d.fromDegrees(180.0)))), m_driveSubsystem) .ignoringDisable(true)); @@ -221,91 +258,23 @@ private void configureButtonBindings() { * Use this method to configure any named commands needed for PathPlanner autos */ private void configureNamedCommands() { - NamedCommands.registerCommand("Run Intake", Commands.run(() -> m_shooter.setIntakePower(0.5))); -// NamedCommands.registerCommand("Run Shooter", Commands.run(m_shooter::runShooterVelocity)); + NamedCommands.registerCommand("Run Intake", m_shooter.intakeCommand(0.75, 0.5, 0.1) + .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.INTAKE))); + + NamedCommands.registerCommand("AimAndShoot", new ShooterAutoCommand(m_armSubsystem, m_shooter) + .raceWith(DriveCommands.alignmentDrive( + m_driveSubsystem, + () -> -controller.getLeftY(), + () -> -controller.getLeftX(), + () -> AllianceFlipUtil.apply(FieldConstants.CENTER_SPEAKER) + ))); } private void configureDashboard() { - ShuffleboardTab commandTab = Shuffleboard.getTab("Commads"); + ShuffleboardTab commandTab = Shuffleboard.getTab("Commands"); commandTab.add("Disable Arm Brake", m_armSubsystem.enableBrakeMode(false)); commandTab.add("Enable Arm Brake", m_armSubsystem.enableBrakeMode(true)); - - commandTab.add("Center Robot Pose", Commands.runOnce( - () -> - m_driveSubsystem.setPose( - new Pose2d( - FieldConstants.FIELD_LENGTH / 2.0, - FieldConstants.FIELD_WIDTH / 2.0, - new Rotation2d())), - m_driveSubsystem) - .ignoringDisable(true)); - - commandTab.add("CenterToSpeaker", Commands.runOnce( - () -> - m_driveSubsystem.setPose(AllianceFlipUtil.apply( - new Pose2d(new Translation2d(1.34, 5.55), - Rotation2d.fromDegrees(180.0)))), - m_driveSubsystem) - .ignoringDisable(true)); - - double centerDistance = 1.34 - Units.inchesToMeters(3.0); - - commandTab.add("2InchesToSpeaker", Commands.runOnce( - () -> - m_driveSubsystem.setPose(AllianceFlipUtil.apply( - new Pose2d(new Translation2d(centerDistance + Units.inchesToMeters(2.0), 5.55), - Rotation2d.fromDegrees(180.0)))), - m_driveSubsystem) - .ignoringDisable(true)); - - commandTab.add("6InchesToSpeaker", Commands.runOnce( - () -> - m_driveSubsystem.setPose(AllianceFlipUtil.apply( - new Pose2d(new Translation2d(centerDistance + Units.inchesToMeters(6.0), 5.55), - Rotation2d.fromDegrees(180.0)))), - m_driveSubsystem) - .ignoringDisable(true)); - - commandTab.add("12InchesToSpeaker", Commands.runOnce( - () -> - m_driveSubsystem.setPose(AllianceFlipUtil.apply( - new Pose2d(new Translation2d(centerDistance + Units.inchesToMeters(12.0), 5.55), - Rotation2d.fromDegrees(180.0)))), - m_driveSubsystem) - .ignoringDisable(true)); - - commandTab.add("24InchesToSpeaker", Commands.runOnce( - () -> - m_driveSubsystem.setPose(AllianceFlipUtil.apply( - new Pose2d(new Translation2d(centerDistance + Units.inchesToMeters(24.0), 5.55), - Rotation2d.fromDegrees(180.0)))), - m_driveSubsystem) - .ignoringDisable(true)); - - commandTab.add("48InchesToSpeaker", Commands.runOnce( - () -> - m_driveSubsystem.setPose(AllianceFlipUtil.apply( - new Pose2d(new Translation2d(centerDistance + Units.inchesToMeters(48.0), 5.55), - Rotation2d.fromDegrees(180.0)))), - m_driveSubsystem) - .ignoringDisable(true)); - - commandTab.add("96InchesToSpeaker", Commands.runOnce( - () -> - m_driveSubsystem.setPose(AllianceFlipUtil.apply( - new Pose2d(new Translation2d(centerDistance + Units.inchesToMeters(96.0), 5.55), - Rotation2d.fromDegrees(180.0)))), - m_driveSubsystem) - .ignoringDisable(true)); - - commandTab.add("192InchesToSpeaker", Commands.runOnce( - () -> - m_driveSubsystem.setPose(AllianceFlipUtil.apply( - new Pose2d(new Translation2d(centerDistance + Units.inchesToMeters(192.0), 5.55), - Rotation2d.fromDegrees(180.0)))), - m_driveSubsystem) - .ignoringDisable(true)); } /** diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 1fd40e18..11ec5791 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -16,6 +16,7 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.*; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.subsystems.drive.DriveSubsystem; @@ -41,7 +42,7 @@ public static Command joystickDrive( () -> { double xInput = setSensitivity(xSupplier.getAsDouble(), 0.25); double yInput = setSensitivity(ySupplier.getAsDouble(), 0.25); - double omegaInput = setSensitivity(omegaSupplier.getAsDouble(), 0.15); + double omegaInput = setSensitivity(omegaSupplier.getAsDouble(), 0.0); // Apply deadband double linearMagnitude = @@ -57,13 +58,23 @@ public static Command joystickDrive( .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())) .getTranslation(); + Rotation2d heading = new Rotation2d(); + + // if red change heading goal + if (DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == DriverStation.Alliance.Red) { + heading = driveSubsystem.getRotation().plus(Rotation2d.fromDegrees(180)); + } else { + heading = driveSubsystem.getRotation(); + } + // Convert to field relative speeds & send command driveSubsystem.runVelocity( ChassisSpeeds.fromFieldRelativeSpeeds( linearVelocity.getX() * driveSubsystem.getMaxLinearSpeedMetersPerSec(), linearVelocity.getY() * driveSubsystem.getMaxLinearSpeedMetersPerSec(), omega * driveSubsystem.getMaxAngularSpeedRadPerSec(), - driveSubsystem.getRotation())); + heading)); }, driveSubsystem); } @@ -93,8 +104,18 @@ public static Command alignmentDrive( .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())) .getTranslation(); - if (linearVelocity.getNorm() > 0.1) { - omega = omega * 4; +// if (linearVelocity.getNorm() > 0.1) { +// omega = omega * 4; +// } + + Rotation2d heading = new Rotation2d(); + + // if red change heading goal + if (DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == DriverStation.Alliance.Red) { + heading = driveSubsystem.getRotation().plus(Rotation2d.fromDegrees(180)); + } else { + heading = driveSubsystem.getRotation(); } // Convert to field relative speeds & send command @@ -103,7 +124,7 @@ public static Command alignmentDrive( linearVelocity.getX() * driveSubsystem.getMaxLinearSpeedMetersPerSec(), linearVelocity.getY() * driveSubsystem.getMaxLinearSpeedMetersPerSec(), omega * driveSubsystem.getMaxAngularSpeedRadPerSec(), - driveSubsystem.getRotation())); + heading)); }, driveSubsystem); } diff --git a/src/main/java/frc/robot/commands/IntakeControlCommand.java b/src/main/java/frc/robot/commands/IntakeControlCommand.java new file mode 100644 index 00000000..630aff06 --- /dev/null +++ b/src/main/java/frc/robot/commands/IntakeControlCommand.java @@ -0,0 +1,75 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.arm.ArmSubsystem; +import frc.robot.subsystems.shooter.ShooterSubsystem; + + +public class IntakeControlCommand extends Command { + private final ArmSubsystem m_armSubsystem; + private final ShooterSubsystem m_shooterSubsystem; + + private final Timer m_timer; + private boolean hasPieceRising; + + public IntakeControlCommand(ArmSubsystem armSubsystem, ShooterSubsystem shooterSubsystem) { + m_armSubsystem = armSubsystem; + m_shooterSubsystem = shooterSubsystem; + m_timer = new Timer(); + // each subsystem used by the command must be passed into the + // addRequirements() method (which takes a vararg of Subsystem) + addRequirements(m_armSubsystem, m_shooterSubsystem); + } + + @Override + public void initialize() { + hasPieceRising = false; + m_timer.reset(); + } + + @Override + public void execute() { + if(!m_shooterSubsystem.hasPiece() && !hasPieceRising) { + // run intake and kicker wheels in + m_shooterSubsystem.setIntakePower(0.95); + m_shooterSubsystem.setKickerPower(0.35); + m_shooterSubsystem.setShooterPowerLeft(-0.1); + m_shooterSubsystem.setShooterPowerRight(-0.1); + m_armSubsystem.setDesiredState(ArmSubsystem.ArmState.INTAKE); + } else if (m_shooterSubsystem.hasPiece() && !m_timer.hasElapsed(0.0001)) { + // stop running intake + m_shooterSubsystem.setIntakePower(0.0); + m_shooterSubsystem.setKickerPower(0.0); + m_shooterSubsystem.setShooterPowerLeft(0.0); + m_shooterSubsystem.setShooterPowerRight(0.0); + + // piece detected, mark as we have a piece and start moving up + m_timer.start(); + m_armSubsystem.setDesiredState(ArmSubsystem.ArmState.STOW); + } else if (!m_timer.hasElapsed(0.075) && m_armSubsystem.bothAtSetpoint()) { + // arm is up, haven't run kickers back yet + m_shooterSubsystem.setKickerPower(-0.2); + } else { + m_shooterSubsystem.setIntakePower(0.0); + m_shooterSubsystem.setKickerPower(0.0); + m_shooterSubsystem.setShooterPowerLeft(0.0); + m_shooterSubsystem.setShooterPowerRight(0.0); + } + } + + @Override + public boolean isFinished() { + // We won't return true because it's being ran by a .whileTrue() method + return false; + } + + @Override + public void end(boolean interrupted) { + m_shooterSubsystem.setIntakePower(0.0); + m_shooterSubsystem.setKickerPower(0.0); + m_shooterSubsystem.setShooterPowerLeft(0.0); + m_shooterSubsystem.setShooterPowerRight(0.0); + m_armSubsystem.setDesiredState(ArmSubsystem.ArmState.STOW); + } +} diff --git a/src/main/java/frc/robot/commands/ShooterAutoCommand.java b/src/main/java/frc/robot/commands/ShooterAutoCommand.java new file mode 100644 index 00000000..cde298a7 --- /dev/null +++ b/src/main/java/frc/robot/commands/ShooterAutoCommand.java @@ -0,0 +1,41 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.arm.ArmSubsystem; +import frc.robot.subsystems.shooter.ShooterSubsystem; + + +public class ShooterAutoCommand extends Command { + private final ArmSubsystem armSubsystem; + private final ShooterSubsystem shooterSubsystem; + + public ShooterAutoCommand(ArmSubsystem armSubsystem, ShooterSubsystem shooterSubsystem) { + this.armSubsystem = armSubsystem; + this.shooterSubsystem = shooterSubsystem; + // each subsystem used by the command must be passed into the + // addRequirements() method (which takes a vararg of Subsystem) + addRequirements(this.armSubsystem, this.shooterSubsystem); + } + + @Override + public void initialize() { + + } + + @Override + public void execute() { + shooterSubsystem.runShooterVelocity(shooterSubsystem.atSpeed()).execute(); + armSubsystem.setDesiredState(ArmSubsystem.ArmState.AUTO_AIM); + } + + @Override + public boolean isFinished() { + return !shooterSubsystem.hasPiece(); + } + + @Override + public void end(boolean interrupted) { + shooterSubsystem.setShooterPowerFactory(0.0, 0.0, 0.0).execute(); + armSubsystem.setDesiredState(ArmSubsystem.ArmState.STOW); + } +} diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIO.java b/src/main/java/frc/robot/subsystems/arm/ArmIO.java index 3c9edf4d..9944abc6 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIO.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIO.java @@ -1,7 +1,10 @@ package frc.robot.subsystems.arm; +import org.littletonrobotics.junction.AutoLog; + public interface ArmIO { + @AutoLog class ArmIOInputs { public double armPositionDegs = 0.0; public double wristPositionDegs = 0.0; @@ -22,7 +25,7 @@ class ArmIOInputs { public double wristCurrentDraw = 0.0; } - default void updateInputs(ArmIOInputs inputs) {} + default void updateInputs(ArmIOInputsAutoLogged inputs) {} default void setArmVoltage(double voltage) {} diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOKraken.java b/src/main/java/frc/robot/subsystems/arm/ArmIOKraken.java index 05c22d00..5176a41a 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOKraken.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOKraken.java @@ -12,10 +12,14 @@ import com.ctre.phoenix6.signals.*; import com.gos.lib.properties.GosDoubleProperty; import com.gos.lib.properties.HeavyDoubleProperty; + import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + import lib.factories.TalonFXFactory; import lib.properties.phoenix6.Phoenix6PidPropertyBuilder; import lib.properties.phoenix6.PidPropertyPublic; + import frc.robot.Constants.ArmConstants; public class ArmIOKraken implements ArmIO { @@ -48,6 +52,9 @@ public class ArmIOKraken implements ArmIO { private boolean m_tracking = false; + private double m_prevArmVelocityMult = 0.0; + private double m_prevWristVelocityMult = 0.0; + // Status signals private final StatusSignal m_armPositionSignal; private final StatusSignal m_wristPositionSignal; @@ -192,7 +199,7 @@ public ArmIOKraken() { } @Override - public void updateInputs(ArmIOInputs inputs) { + public void updateInputs(ArmIOInputsAutoLogged inputs) { BaseStatusSignal.refreshAll( m_armPositionSignal, m_wristPositionSignal, @@ -245,7 +252,10 @@ public void setArmVoltage(double voltage) { @Override public void setArmAngle(double degrees, double velocityMult) { - m_armMaxVelDegS.updateIfChanged(true); + if (m_prevArmVelocityMult != velocityMult) { + m_armMaxVelDegS.updateIfChanged(true); + } + m_prevArmVelocityMult = velocityMult; m_armDynMMRequest.Velocity = m_armDynMMRequest.Velocity * velocityMult; m_armMaster.setControl(m_armDynMMRequest.withPosition(degrees / 360)); @@ -260,7 +270,10 @@ public void setWristVoltage(double voltage) { @Override public void setWristAngle(double degrees, double velocityMult) { - m_wristMaxVelDegS.updateIfChanged(true); + if (m_prevWristVelocityMult != velocityMult) { + m_wristMaxVelDegS.updateIfChanged(true); + } + m_prevWristVelocityMult = velocityMult; m_wristDynMMRequest.Velocity = m_wristDynMMRequest.Velocity * velocityMult; m_wristMaster.setControl(m_wristDynMMRequest.withPosition(degrees / 360)); diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOPrototype.java b/src/main/java/frc/robot/subsystems/arm/ArmIOPrototype.java index 2fe078b1..8b6ede62 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOPrototype.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOPrototype.java @@ -80,7 +80,7 @@ public ArmIOPrototype() { } @Override - public void updateInputs(ArmIOInputs inputs) { + public void updateInputs(ArmIOInputsAutoLogged inputs) { m_shoulderPID.updateIfChanged(); m_wristPID.updateIfChanged(); diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java b/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java index 3f89b78f..6cc61548 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java @@ -27,7 +27,7 @@ public class ArmIOSim implements ArmIO{ private static final double LOOP_PERIOD_SECS = 0.02; @Override - public void updateInputs(ArmIOInputs inputs) { + public void updateInputs(ArmIOInputsAutoLogged inputs) { m_armSim.update(LOOP_PERIOD_SECS); m_wristSim.update(LOOP_PERIOD_SECS); diff --git a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java index ed971e87..3175eb3a 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java @@ -2,6 +2,9 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; @@ -9,6 +12,10 @@ import frc.robot.Constants.ArmConstants; import frc.robot.Constants.ArmSetpoints; import lib.logger.DataLogUtil; +import org.littletonrobotics.junction.Logger; +import lib.utils.AimbotUtils; +import lib.utils.AllianceFlipUtil; +import lib.utils.FieldConstants; import java.util.function.Supplier; @@ -27,15 +34,15 @@ public enum ArmState { } private final ArmIO m_io; - private final ArmIO.ArmIOInputs m_inputs; + private final ArmIOInputsAutoLogged m_inputs; private double m_desiredArmPoseDegs; private double m_armVelocityMult = 0; private double m_desiredWristPoseDegs; private double m_wristGap; private double m_wristVelocityMult = 0; - private double m_startTime; + private boolean m_disabledBrakeMode = true; - private ArmState m_desiredState = ArmState.DISABLED; + private ArmState m_desiredState = ArmState.STOW; private ArmState m_currentState = ArmState.DISABLED; @@ -52,7 +59,7 @@ public ArmSubsystem(ArmIO io) { public ArmSubsystem(ArmIO io, Supplier supplier) { m_io = io; - m_inputs = new ArmIO.ArmIOInputs(); + m_inputs = new ArmIOInputsAutoLogged(); m_desiredWristPoseDegs = Double.NEGATIVE_INFINITY; m_desiredArmPoseDegs = Double.NEGATIVE_INFINITY; @@ -61,7 +68,7 @@ public ArmSubsystem(ArmIO io, Supplier supplier) { m_poseSupplier = supplier; - setupLogging(); +// setupLogging(); m_poseVisualizer = new ArmVisualizer("Current Arm Pose", Color.kFirstBlue); m_setpointVisualizer = new ArmVisualizer("Current Arm Setpoint", Color.kFirstRed); } @@ -69,6 +76,7 @@ public ArmSubsystem(ArmIO io, Supplier supplier) { @Override public void periodic() { m_io.updateInputs(m_inputs); + Logger.processInputs("Arm", m_inputs); handleState(); @@ -89,19 +97,25 @@ public void periodic() { } // check to make sure we're not in manual control - m_io.enableBrakeMode(m_desiredState == ArmState.DISABLED); + m_io.enableBrakeMode(m_desiredState == ArmState.DISABLED && m_disabledBrakeMode); if (m_desiredState != ArmState.DISABLED) { // check to see if the wrist is currently too close to the rest of the arm - m_io.setWristAngle(m_desiredWristPoseDegs, m_wristVelocityMult); + double predictedUnderGap = MathUtil.clamp(ArmConstants.WRIST_ARM_GAP.getValue() + - (m_desiredArmPoseDegs + m_desiredWristPoseDegs), 0, 180); - double underGap = MathUtil.clamp(ArmConstants.WRIST_ARM_GAP.getValue() - - (m_inputs.armPositionDegs + m_inputs.wristPositionDegs), 0, 180); + m_io.setWristAngle(m_desiredWristPoseDegs + predictedUnderGap, m_wristVelocityMult); // set the arms angle - m_io.setArmAngle(m_desiredArmPoseDegs + underGap, m_armVelocityMult); + m_io.setArmAngle(m_desiredArmPoseDegs, m_armVelocityMult); } + Logger.recordOutput("Arm/Arm Setpoint", m_desiredArmPoseDegs); + Logger.recordOutput("Arm/Wrist Setpoint", m_desiredWristPoseDegs); + + Logger.recordOutput("Arm/Arm Velocity Multiplier"); + Logger.recordOutput("Arm/Wrist Velocity Multiplier"); + // Update arm visualizers m_poseVisualizer.update(m_inputs.armPositionDegs, m_inputs.wristPositionDegs); m_setpointVisualizer.update(m_desiredArmPoseDegs, m_desiredWristPoseDegs); @@ -110,8 +124,8 @@ public void periodic() { public void handleState() { switch(m_desiredState) { case STOW -> { - if (m_inputs.armPositionDegs > 60 && m_currentState == ArmState.AMP) { - m_wristVelocityMult = 0.0; + if (m_inputs.armPositionDegs > 67.5 && m_currentState == ArmState.AMP) { + m_wristVelocityMult = 0.35; m_armVelocityMult = 1.0; } else { m_currentState = ArmState.STOW; @@ -126,10 +140,14 @@ public void handleState() { m_armVelocityMult = 1.0; m_wristVelocityMult = 1.0; + Pose3d speakerPose = new Pose3d(AllianceFlipUtil.apply(FieldConstants.CENTER_SPEAKER), new Rotation3d()); + Translation2d speakerPoseGround = speakerPose.getTranslation().toTranslation2d(); + double groundDistance = m_poseSupplier.get().getTranslation().getDistance(speakerPoseGround); + + m_desiredWristPoseDegs = AimbotUtils.getWristAngle(groundDistance); + m_desiredArmPoseDegs = ArmConstants.WRIST_ARM_GAP.getValue() - m_desiredWristPoseDegs; - m_desiredArmPoseDegs = m_desiredArmPoseDegs >= ArmConstants.ARM_LOWER_LIMIT.getValue() ? m_desiredArmPoseDegs - : ArmConstants.ARM_LOWER_LIMIT.getValue(); - m_desiredWristPoseDegs = ArmSetpoints.WRIST_ANGLE.getValue(); + m_desiredArmPoseDegs = m_desiredArmPoseDegs >= 0 ? m_desiredArmPoseDegs : 0; } case INTAKE -> { m_armVelocityMult = 1.0; @@ -142,11 +160,10 @@ public void handleState() { case AMP -> { if (Math.abs(m_inputs.wristPositionDegs - m_desiredWristPoseDegs) > 5) { m_armVelocityMult = 0.5; - m_wristVelocityMult = 1.0; } else { m_armVelocityMult = 1.0; - m_wristVelocityMult = 1.0; } + m_wristVelocityMult = 1.0; m_currentState = ArmState.AMP; @@ -163,15 +180,31 @@ public void handleState() { } } + public void setDesiredState(ArmState state) { + m_desiredState = state; + } + + public boolean armAtSetpoint() { + return Math.abs(m_inputs.armPositionDegs - m_desiredArmPoseDegs) < 5.0; + } + + public boolean wristAtSetpoint() { + return Math.abs(m_inputs.wristPositionDegs - m_desiredWristPoseDegs) < 5.0; + } + + public boolean bothAtSetpoint() { + return armAtSetpoint() && wristAtSetpoint(); + } + /* Command Factories */ - public Command setDesiredState(ArmState state) { - return runEnd(() -> m_desiredState = state, + public Command setDesiredStateFactory(ArmState state) { + return startEnd(() -> m_desiredState = state, () -> m_desiredState = ArmState.STOW); } public Command enableBrakeMode(boolean enabled) { - return runOnce(() -> m_io.enableBrakeMode(enabled)).ignoringDisable(true); + return runOnce(() -> m_disabledBrakeMode = enabled).ignoringDisable(true); } public Command resetEncoderFactory() { diff --git a/src/main/java/frc/robot/subsystems/arm/ArmVisualizer.java b/src/main/java/frc/robot/subsystems/arm/ArmVisualizer.java index dc8ebaf7..906baf6c 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmVisualizer.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmVisualizer.java @@ -13,6 +13,7 @@ import frc.robot.Constants.ArmConstants; import lib.logger.DataLogUtil; import lib.utils.AimbotUtils; +import org.littletonrobotics.junction.Logger; public class ArmVisualizer { private final Mechanism2d mechanism; @@ -33,8 +34,6 @@ public ArmVisualizer(String key, Color color) { wrist = new MechanismLigament2d("wrist", ArmConstants.WRIST_LENGTH_METERS, 45.0, 5, new Color8Bit(color)); root.append(arm); arm.append(wrist); - - DataLogUtil.getTable("Arm").addPose3dArray(key, () -> new Pose3d[]{pivotArm, pivotWrist}, true); } /** Update arm visualizer with current arm angle */ @@ -43,6 +42,8 @@ public void update(double armAngleDegs, double wristAngleDegs) { arm.setAngle(Rotation2d.fromDegrees(armAngleDegs)); wrist.setAngle(Rotation2d.fromDegrees(wristAngleDegs)); + Logger.recordOutput("Arm/" + key + "2d", mechanism); + // Log 3D poses pivotArm = new Pose3d(ArmConstants.PIVOT_JOINT_TRANSLATION.getX(), 0.0, @@ -51,5 +52,7 @@ public void update(double armAngleDegs, double wristAngleDegs) { pivotWrist = new Pose3d(AimbotUtils.getShooterTransformation(armAngleDegs).getTranslation(), new Rotation3d(0.0, Units.degreesToRadians(-wristAngleDegs), 0.0)); + + Logger.recordOutput("Arm/" + key + "3d", pivotArm, pivotWrist); } } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java index ae59d9aa..04f22d2c 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java @@ -1,6 +1,9 @@ package frc.robot.subsystems.climber; +import org.littletonrobotics.junction.AutoLog; + public interface ClimberIO { + @AutoLog class ClimberIOInputs { protected double leftClimberPosition = 0.0; protected double rightClimberPosition = 0.0; @@ -102,6 +105,6 @@ default void setRightVoltage(double volts) {} default void setLeftVoltage(double volts) {} default void setRightPosition(double degrees) {} default void setLeftPosition(double degrees) {} - default void updateInputs(ClimberIOInputs inputs) {} + default void updateInputs(ClimberIOInputsAutoLogged inputs) {} default void stop() {} } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOKraken.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOKraken.java index 3384ea91..a4669efd 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOKraken.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOKraken.java @@ -98,7 +98,7 @@ public ClimberIOKraken() { } @Override - public void updateInputs(ClimberIOInputs inputs) { + public void updateInputs(ClimberIOInputsAutoLogged inputs) { inputs.leftClimberPosition = m_leftPositionSignal.getValueAsDouble(); inputs.rightClimberPosition = m_rightPositionSignal.getValueAsDouble(); diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOPrototype.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOPrototype.java index 2f5ac8c8..b92b707e 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOPrototype.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOPrototype.java @@ -68,7 +68,7 @@ public void setRightPosition(double degrees) { } @Override - public void updateInputs(ClimberIOInputs inputs) { + public void updateInputs(ClimberIOInputsAutoLogged inputs) { m_leftClimberPid.updateIfChanged(); m_rightClimberPid.updateIfChanged(); diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java index 56924cdb..72b6d27e 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java @@ -3,18 +3,20 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger; public class ClimberSubsystem extends SubsystemBase { private final ClimberIO m_io; - private final ClimberIO.ClimberIOInputs m_inputs; + private final ClimberIOInputsAutoLogged m_inputs; public ClimberSubsystem(ClimberIO io) { m_io = io; - m_inputs = new ClimberIO.ClimberIOInputs(); + m_inputs = new ClimberIOInputsAutoLogged(); } @Override public void periodic() { m_io.updateInputs(m_inputs); + Logger.processInputs("Climber", m_inputs); } public void setClimberPower(double power) { diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index 3eeae2f7..1405fb6a 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -13,11 +13,12 @@ package frc.robot.subsystems.drive; +import com.ctre.phoenix6.SignalLogger; import com.gos.lib.properties.pid.PidProperty; import com.gos.lib.properties.pid.WpiPidPropertyBuilder; import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.pathfinding.LocalADStar; import com.pathplanner.lib.pathfinding.Pathfinding; +import com.pathplanner.lib.util.PathPlannerLogging; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.*; @@ -27,61 +28,66 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.*; import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.Voltage; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants.DriveConstants; import frc.robot.subsystems.drive.module.Module; import frc.robot.subsystems.drive.module.ModuleIO; +import frc.robot.subsystems.drive.module.PhoenixOdometryThread; import frc.robot.subsystems.vision.VisionSubsystem; +import java.util.List; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; +import java.util.function.Supplier; import lib.logger.DataLogUtil; import lib.logger.DataLogUtil.DataLogTable; +import lib.utils.AllianceFlipUtil; +import lib.utils.LocalADStarAK; import lib.utils.PoseEstimator; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +import static edu.wpi.first.units.Units.Volts; public class DriveSubsystem extends SubsystemBase { public static final Lock odometryLock = new ReentrantLock(); private final GyroIO gyroIO; - private final GyroIO.GyroIOInputs gyroInputs = new GyroIO.GyroIOInputs(); + private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); private final Module[] modules = new Module[4]; // FL, FR, BL, BR private final SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); - private final PoseEstimator m_poseEstimator; private final SwerveDrivePoseEstimator m_wpiPoseEstimator; private final Field2d m_field = new Field2d(); - private Pose2d pose = new Pose2d(); - private Rotation2d lastGyroRotation = new Rotation2d(); + private Rotation2d m_rawGyroRotation = new Rotation2d(); + private SwerveModulePosition[] m_lastModulePositions = new SwerveModulePosition[] { + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() + }; private final VisionSubsystem[] m_cameras; private final PIDController m_thetaPid; private final PidProperty m_thetaPidProperty; - private final DataLogTable m_logTable = DataLogUtil.getTable("Swerve"); + private final SysIdRoutine m_sysId; - private final SwerveModuleState[] m_measureStates = new SwerveModuleState[] { - new SwerveModuleState(), - new SwerveModuleState(), - new SwerveModuleState(), - new SwerveModuleState() - }; - private SwerveModuleState[] m_setpointStates = new SwerveModuleState[] { - new SwerveModuleState(), - new SwerveModuleState(), - new SwerveModuleState(), - new SwerveModuleState() - }; private final SwerveModuleState[] m_optimizedStates = new SwerveModuleState[] { new SwerveModuleState(), new SwerveModuleState(), @@ -117,6 +123,8 @@ public DriveSubsystem( modules[2] = new Module(blModuleIO); modules[3] = new Module(brModuleIO); + PhoenixOdometryThread.getInstance().start(); + m_thetaPid = new PIDController(0.0, 0.0, 0.0); m_thetaPid.enableContinuousInput(-Math.PI, Math.PI); m_thetaPidProperty = new WpiPidPropertyBuilder("Drive/Theta Alignment", false, m_thetaPid) @@ -126,19 +134,20 @@ public DriveSubsystem( .build(); m_cameras = cameras; - m_poseEstimator = - new PoseEstimator(VecBuilder.fill( - Units.inchesToMeters(1.0), - Units.inchesToMeters(1.0), - Units.degreesToRadians(30))); - - m_poseEstimator.resetPose(new Pose2d()); m_wpiPoseEstimator = new SwerveDrivePoseEstimator( kinematics, new Rotation2d(), getModulePositions(), - new Pose2d() + new Pose2d(), + VecBuilder.fill( + Units.inchesToMeters(0.5), + Units.inchesToMeters(0.5), + Units.degreesToRadians(0.75)), + VecBuilder.fill( + Units.inchesToMeters(4.0), + Units.inchesToMeters(4.0), + Units.degreesToRadians(25.0)) ); // Configure AutoBuilder for PathPlanner @@ -152,17 +161,36 @@ public DriveSubsystem( DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red, this); - Pathfinding.setPathfinder(new LocalADStar()); + Pathfinding.setPathfinder(new LocalADStarAK()); + PathPlannerLogging.setLogActivePathCallback( + (List activePath) -> Logger.recordOutput( + "Odometry/Trajectory", activePath.toArray(new Pose2d[0]))); + PathPlannerLogging.setLogTargetPoseCallback( + (Pose2d targetPose) -> Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose)); // turn on logging - setupLogging(); SmartDashboard.putData("Field",m_field); + + m_sysId = new SysIdRoutine( + new SysIdRoutine.Config( + null, + null, + null, + (SysIdRoutineLog.State state) -> SignalLogger.writeString("Drive/SysidState", state.toString()) + ), + new SysIdRoutine.Mechanism( + (Measure volts) -> runCharacterizationVolts(volts.in(Volts)), + null, + this + ) + ); } @Override public void periodic() { odometryLock.lock(); // Prevents odometry updates while reading data gyroIO.updateInputs(gyroInputs); + Logger.processInputs("Swerve/Gyro", gyroInputs); for (var module : modules) { module.updateInputs(); } @@ -179,51 +207,60 @@ public void periodic() { } // Update odometry -// int deltaCount = -// gyroInputs.isConnected() ? gyroInputs.getOdometryYawPositions().length : Integer.MAX_VALUE; -// for (int i = 0; i < 4; i++) { -// deltaCount = Math.min(deltaCount, modules[i].getPositionDeltas().length); -// } -// Logger.recordOutput("Odometry/Delta Count", deltaCount); -// for (int deltaIndex = 0; deltaIndex < deltaCount; deltaIndex++) { -// Read wheel deltas from each module -// SwerveModulePosition[] wheelDeltas = new SwerveModulePosition[4]; -// for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) { -// wheelDeltas[moduleIndex] = modules[moduleIndex].getPositionDeltas()[deltaIndex]; -// } - - // The twist represents the motion of the robot since the last - // sample in x, y, and theta based only on the modules, without - // the gyro. The gyro is always disconnected in simulation. -// var twist = kinematics.toTwist2d(wheelDeltas); -// if (gyroInputs.isConnected()) { - // If the gyro is connected, replace the theta component of the twist - // with the change in angle since the last sample. -// Rotation2d gyroRotation = gyroInputs.getOdometryYawPositions()[deltaIndex]; -// twist = new Twist2d(twist.dx, twist.dy, gyroRotation.minus(lastGyroRotation).getRadians()); -// lastGyroRotation = gyroRotation; -// } -// Logger.recordOutput("Odometry/Twist", twist); - // Apply the twist (change since last sample) to the current pose -// m_poseEstimator.addDriveData(Timer.getFPGATimestamp(), twist); -// pose = pose.exp(twist); -// } - -// List visionUpdates = new java.util.ArrayList<>(List.of()); - for (VisionSubsystem camera : m_cameras) { - camera.updateInputs(); - camera.getPose(m_wpiPoseEstimator.getEstimatedPosition()).ifPresent( - (PoseEstimator.TimestampedVisionUpdate pose) -> - m_wpiPoseEstimator.addVisionMeasurement(pose.pose(), pose.timestamp(), pose.stdDevs()) - ); + double[] sampleTimestamps = + modules[0].getOdometryTimestamps(); // All signals are sampled together + int sampleCount = sampleTimestamps.length; + for (int i = 0; i < sampleCount; i++) { + // Read wheel positions and deltas from each module + SwerveModulePosition[] modulePositions = new SwerveModulePosition[4]; + SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[4]; + for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) { + modulePositions[moduleIndex] = modules[moduleIndex].getOdometryPositions()[i]; + moduleDeltas[moduleIndex] = + new SwerveModulePosition( + modulePositions[moduleIndex].distanceMeters + - m_lastModulePositions[moduleIndex].distanceMeters, + modulePositions[moduleIndex].angle); + m_lastModulePositions[moduleIndex] = modulePositions[moduleIndex]; + } + + // Update gyro angle + if (gyroInputs.connected) { + // Use the real gyro angle + m_rawGyroRotation = gyroInputs.odometryYawPositions[i]; + } else { + // Use the angle delta from the kinematics and module deltas + Twist2d twist = kinematics.toTwist2d(moduleDeltas); + m_rawGyroRotation = m_rawGyroRotation.plus(new Rotation2d(twist.dtheta)); + } + + // Apply update + m_wpiPoseEstimator.updateWithTime(sampleTimestamps[i], m_rawGyroRotation, modulePositions); } -// m_poseEstimator.addVisionData(visionUpdates); + // make sure we're not moving too fast before trying to update vision poses + if ((kinematics.toChassisSpeeds(getModuleStates()).vxMetersPerSecond <= Units.inchesToMeters(20)) + && (kinematics.toChassisSpeeds(getModuleStates()).vyMetersPerSecond <= Units.inchesToMeters(20)) + && (kinematics.toChassisSpeeds(getModuleStates()).omegaRadiansPerSecond <= Units.degreesToRadians(60))) { + for (VisionSubsystem camera : m_cameras) { + camera.updateInputs(); + camera.getPose(m_wpiPoseEstimator.getEstimatedPosition()).ifPresent( + (PoseEstimator.TimestampedVisionUpdate pose) -> + m_wpiPoseEstimator.addVisionMeasurement(pose.pose(), pose.timestamp(), pose.stdDevs()) + ); + } + } - m_wpiPoseEstimator.update(gyroInputs.yawPosition, getModulePositions()); + m_wpiPoseEstimator.updateWithTime(Timer.getFPGATimestamp(), gyroInputs.yawPosition, getModulePositions()); m_thetaPidProperty.updateIfChanged(); m_field.setRobotPose(getVisionPose()); + + Logger.recordOutput("Swerve/LeftCamPose", + new Pose3d(getVisionPose()).plus(DriveConstants.LEFT_CAMERA_TRANSFORMATION)); + + Logger.recordOutput("Swerve/RightCamPose", + new Pose3d(getVisionPose()).plus(DriveConstants.RIGHT_CAMERA_TRANSFORMATION)); } /** @@ -234,7 +271,7 @@ public void periodic() { public void runVelocity(ChassisSpeeds speeds) { // Calculate module setpoints ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02); - m_setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); + SwerveModuleState[] m_setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); SwerveDriveKinematics.desaturateWheelSpeeds(m_setpointStates, DriveConstants.MAX_LINEAR_SPEED); // Send setpoints to modules @@ -242,6 +279,9 @@ public void runVelocity(ChassisSpeeds speeds) { // The module returns the optimized state, useful for logging m_optimizedStates[i] = modules[i].runSetpoint(m_setpointStates[i]); } + + Logger.recordOutput("SwerveStates/Setpoints", m_setpointStates); + Logger.recordOutput("SwerveStates/Optimized", m_optimizedStates); } /** @@ -250,12 +290,11 @@ public void runVelocity(ChassisSpeeds speeds) { * @param point the desired point to align to */ public double alignToPoint(Pose3d point) { - Transform3d robotToPoint = new Transform3d(new Pose3d( - new Pose2d(pose.getTranslation(), new Rotation2d())), point); + new Pose2d(m_wpiPoseEstimator.getEstimatedPosition().getTranslation(), new Rotation2d())), point); - double desiredRotation = Math.PI * 2 - (Math.atan2(robotToPoint.getX(), robotToPoint.getY()) - + Units.degreesToRadians(270)); + double desiredRotation = Math.PI * 2 - (Math.atan2(robotToPoint.getX(), robotToPoint.getY())) + + Units.degreesToRadians(90); return m_thetaPid.calculate(getRotation().getRadians(), desiredRotation); } @@ -295,7 +334,7 @@ public double getCharacterizationVelocity() { } /** Returns the module states (turn angles and drive velocities) for all the modules. */ -// @AutoLogOutput(key = "SwerveStates/Measured") + @AutoLogOutput(key = "SwerveStates/Measured") private SwerveModuleState[] getModuleStates() { SwerveModuleState[] states = new SwerveModuleState[4]; for (int i = 0; i < 4; i++) { @@ -312,14 +351,8 @@ private SwerveModulePosition[] getModulePositions() { return positions; } - /** Returns the current odometry pose. */ -// @AutoLogOutput(key = "Odometry/Robot") - public Pose2d getPose() { - return pose; - } - /** Returns the robot pose with vision updates */ -// @AutoLogOutput(key = "Odometry/RobotVision") + @AutoLogOutput(key = "Odometry/RobotVision") public Pose2d getVisionPose() { return m_wpiPoseEstimator.getEstimatedPosition(); } @@ -346,7 +379,8 @@ public double getMaxAngularSpeedRadPerSec() { } public Command pathfollowFactory(Pose2d pose) { - return AutoBuilder.pathfindToPose(pose, DriveConstants.DEFAULT_CONSTRAINTS); + return AutoBuilder.pathfindToPoseFlipped( + pose, DriveConstants.DEFAULT_CONSTRAINTS); } /** Returns an array of module translations. */ @@ -359,23 +393,11 @@ public static Translation2d[] getModuleTranslations() { }; } - public void setupLogging() { - for (int i = 0; i < modules.length; i++) { - Module module = modules[i]; - String moduleName = "Module" + i; - - m_logTable.addDouble(moduleName + "/AzimuthPose", () -> module.getAngle().getDegrees(), true); - m_logTable.addDouble(moduleName + "/DrivePose", module::getPositionMeters, true); - - m_logTable.addDouble(moduleName + "/VelocityMPS", module::getVelocityMetersPerSec, true); - } - - m_logTable.addPose2d("Vision Pose", this::getVisionPose, true); - - m_logTable.addSwerveModuleStateArray("MeasuredStates", () -> m_measureStates, true); - m_logTable.addSwerveModuleStateArray("SetpointStates", () -> m_setpointStates, true); - m_logTable.addSwerveModuleStateArray("OptimizedStates", () -> m_optimizedStates, true); + public Command runSysidQuasistatic(SysIdRoutine.Direction direction) { + return m_sysId.quasistatic(direction); + } - m_logTable.addDouble("GyroHeading", () -> gyroInputs.yawPosition.getDegrees(), true); + public Command runSysidDynamic(SysIdRoutine.Direction direction) { + return m_sysId.dynamic(direction); } } diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java index b11c6ccc..1d386ae1 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIO.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIO.java @@ -14,8 +14,10 @@ package frc.robot.subsystems.drive; import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.AutoLog; public interface GyroIO { + @AutoLog class GyroIOInputs { protected boolean connected = false; protected Rotation2d yawPosition = new Rotation2d(); @@ -55,7 +57,7 @@ public void setYawVelocityRadPerSec(double yawVelocityRadPerSec) { } } - default void updateInputs(GyroIOInputs inputs) {} + default void updateInputs(GyroIOInputsAutoLogged inputs) {} default void resetGyro(double degrees) {} } diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon1.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon1.java index 183004ef..dea3555a 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon1.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon1.java @@ -21,7 +21,7 @@ public GyroIOPigeon1(int id) { } @Override - public void updateInputs(GyroIOInputs inputs) { + public void updateInputs(GyroIOInputsAutoLogged inputs) { inputs.setConnected(m_pigeon.getState() == PigeonIMU.PigeonState.Ready); inputs.setYawPosition(Rotation2d.fromDegrees(m_pigeon.getYaw())); inputs.setYawVelocityRadPerSec((m_pigeon.getYaw() - prevYaw) / Units.millisecondsToSeconds(20)); diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index 2476c75f..1b359561 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -50,7 +50,7 @@ public GyroIOPigeon2(boolean phoenixDrive) { } @Override - public void updateInputs(GyroIOInputs inputs) { + public void updateInputs(GyroIOInputsAutoLogged inputs) { inputs.setConnected(BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK)); inputs.setYawPosition(Rotation2d.fromDegrees(yaw.getValueAsDouble())); inputs.setYawVelocityRadPerSec(Units.degreesToRadians(yawVelocity.getValueAsDouble())); diff --git a/src/main/java/frc/robot/subsystems/drive/module/Module.java b/src/main/java/frc/robot/subsystems/drive/module/Module.java index 25046e59..e2c7ba34 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/module/Module.java @@ -17,22 +17,23 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Constants; import lib.logger.DataLogUtil; +import org.littletonrobotics.junction.Logger; public class Module { private static final double WHEEL_RADIUS = Constants.DriveConstants.WHEEL_RADIUS_METERS; public static final double ODOMETRY_FREQUENCY = 250.0; private final ModuleIO m_io; - private final ModuleIO.ModuleIOInputs m_inputs = new ModuleIO.ModuleIOInputs(); + private final ModuleIOInputsAutoLogged m_inputs = new ModuleIOInputsAutoLogged(); private final int m_index; private Rotation2d m_angleSetpoint = null; // Setpoint for closed loop control, null for open loop private Double m_speedSetpoint = null; // Setpoint for closed loop control, null for open loop private Rotation2d m_turnRelativeOffset = null; // Relative + Offset = Absolute - private double m_lastPositionMeters = 0.0; // Used for delta calculation - private SwerveModulePosition[] m_positionDeltas = new SwerveModulePosition[] {}; + private SwerveModulePosition[] m_odometryPositions = new SwerveModulePosition[] {}; public Module(ModuleIO io) { this.m_io = io; @@ -59,6 +60,8 @@ public void updateInputs() { } public void periodic() { + Logger.processInputs("Swerve/Module" + m_index, m_inputs); + // On first cycle, reset relative turn encoder // Wait until absolute angle is nonzero in case it wasn't initialized yet if (m_turnRelativeOffset == null) { @@ -77,20 +80,28 @@ public void periodic() { } } - // Calculate position deltas for odometry - int deltaCount = - Math.min(m_inputs.getOdometryDrivePositionsRad().length, m_inputs.getOdometryTurnPositions().length); - m_positionDeltas = new SwerveModulePosition[deltaCount]; - for (int i = 0; i < deltaCount; i++) { - double positionMeters = m_inputs.getOdometryDrivePositionsRad()[i] * WHEEL_RADIUS; + // Calculate positions for odometry + int sampleCount = m_inputs.odometryTimestamps.length; // All signals are sampled together + m_odometryPositions = new SwerveModulePosition[sampleCount]; + for (int i = 0; i < sampleCount; i++) { + double positionMeters = m_inputs.odometryDrivePositionsRad[i] * WHEEL_RADIUS; Rotation2d angle = - m_inputs.getOdometryTurnPositions()[i].plus( + m_inputs.odometryTurnPositions[i].plus( m_turnRelativeOffset != null ? m_turnRelativeOffset : new Rotation2d()); - m_positionDeltas[i] = new SwerveModulePosition(positionMeters - m_lastPositionMeters, angle); - m_lastPositionMeters = positionMeters; + m_odometryPositions[i] = new SwerveModulePosition(positionMeters, angle); } } + /** Returns the module positions received this cycle. */ + public SwerveModulePosition[] getOdometryPositions() { + return m_odometryPositions; + } + + /** Returns the timestamps of the samples received this cycle. */ + public double[] getOdometryTimestamps() { + return m_inputs.odometryTimestamps; + } + /** Runs the module with the specified setpoint state. Returns the optimized state. */ public SwerveModuleState runSetpoint(SwerveModuleState state) { // Optimize state based on current angle @@ -159,11 +170,6 @@ public SwerveModuleState getState() { return new SwerveModuleState(getVelocityMetersPerSec(), getAngle()); } - /** Returns the module position deltas received this cycle. */ - public SwerveModulePosition[] getPositionDeltas() { - return m_positionDeltas; - } - /** Returns the drive velocity in radians/sec. */ public double getCharacterizationVelocity() { return m_inputs.getDriveVelocityRadPerSec(); diff --git a/src/main/java/frc/robot/subsystems/drive/module/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/module/ModuleIO.java index 4a3167bf..642db926 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/module/ModuleIO.java @@ -14,8 +14,10 @@ package frc.robot.subsystems.drive.module; import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.AutoLog; public interface ModuleIO { + @AutoLog class ModuleIOInputs { protected double drivePositionRad = 0.0; protected double driveVelocityRadPerSec = 0.0; @@ -31,6 +33,8 @@ class ModuleIOInputs { protected double[] odometryDrivePositionsRad = new double[] {}; protected Rotation2d[] odometryTurnPositions = new Rotation2d[] {}; + protected double[] odometryTimestamps = new double[] {}; + public double getDrivePositionRad() { return drivePositionRad; } @@ -121,7 +125,7 @@ public void setOdometryTurnPositions(Rotation2d[] odometryTurnPositions) { } /** Updates the set of loggable inputs. */ - default void updateInputs(ModuleIOInputs inputs) {} + default void updateInputs(ModuleIOInputsAutoLogged inputs) {} /** Run the drive motor at the specified voltage. */ default void setDriveVelocityMPS(double mps) {} diff --git a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSim.java index e735141e..8360be1c 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSim.java @@ -32,8 +32,11 @@ public class ModuleIOSim implements ModuleIO { private static final double LOOP_PERIOD_SECS = 0.02; - private DCMotorSim driveSim = new DCMotorSim(DCMotor.getKrakenX60(1), ModuleConstants.GearRatios.L3.ratio, 0.025); - private DCMotorSim turnSim = new DCMotorSim(DCMotor.getFalcon500Foc(1), ModuleConstants.GearRatios.TURN.ratio, 0.004); + private final DCMotorSim driveSim = + new DCMotorSim(DCMotor.getKrakenX60(1), ModuleConstants.GearRatios.L3_KRAKEN.ratio, 0.025); + + private final DCMotorSim turnSim = + new DCMotorSim(DCMotor.getFalcon500Foc(1), ModuleConstants.GearRatios.TURN.ratio, 0.004); private final Rotation2d turnAbsoluteInitPosition = new Rotation2d(Math.random() * 2.0 * Math.PI); private double driveAppliedVolts = 0.0; @@ -53,7 +56,7 @@ public ModuleIOSim(ModuleConstants constants) { } @Override - public void updateInputs(ModuleIOInputs inputs) { + public void updateInputs(ModuleIOInputsAutoLogged inputs) { driveSim.update(LOOP_PERIOD_SECS); turnSim.update(LOOP_PERIOD_SECS); @@ -69,6 +72,7 @@ public void updateInputs(ModuleIOInputs inputs) { inputs.turnAppliedVolts = turnAppliedVolts; inputs.turnCurrentAmps = new double[] {Math.abs(turnSim.getCurrentDrawAmps())}; + inputs.odometryTimestamps = new double[] {Timer.getFPGATimestamp()}; inputs.odometryDrivePositionsRad = new double[] {inputs.drivePositionRad}; inputs.odometryTurnPositions = new Rotation2d[] {inputs.turnPosition}; } diff --git a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java index ecb97ff5..1ec2ad14 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java @@ -65,6 +65,7 @@ public class ModuleIOTalonFX implements ModuleIO { private final StatusSignal m_turnAbsolutePosition; private final StatusSignal m_turnPosition; private final Queue m_turnPositionQueue; + private final Queue timestampQueue; private final StatusSignal m_turnVelocity; private final StatusSignal m_turnAppliedVolts; private final StatusSignal m_turnCurrent; @@ -98,8 +99,8 @@ public ModuleIOTalonFX(ModuleConstants moduleConstants) { // run configs on drive motor var driveConfig = new TalonFXConfiguration(); - driveConfig.CurrentLimits.StatorCurrentLimit = 40.0; - driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; + driveConfig.CurrentLimits.SupplyCurrentLimit = 40.0; + driveConfig.CurrentLimits.SupplyCurrentLimitEnable = true; driveConfig.MotorOutput.Inverted = moduleConstants.DRIVE_MOTOR_INVERTED() ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; @@ -168,6 +169,8 @@ public ModuleIOTalonFX(ModuleConstants moduleConstants) { m_turnClosedLoopError = m_turnTalon.getClosedLoopError(); m_turnClosedLoopReference = m_turnTalon.getClosedLoopReference(); + timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); + // setup refresh rates on all inputs BaseStatusSignal.setUpdateFrequencyForAll( Module.ODOMETRY_FREQUENCY, m_drivePosition, m_turnPosition); @@ -188,7 +191,7 @@ public ModuleIOTalonFX(ModuleConstants moduleConstants) { } @Override - public void updateInputs(ModuleIOInputs inputs) { + public void updateInputs(ModuleIOInputsAutoLogged inputs) { BaseStatusSignal.refreshAll( m_drivePosition, m_driveVelocity, diff --git a/src/main/java/frc/robot/subsystems/drive/module/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/module/PhoenixOdometryThread.java index e51ce785..d9c6aa8f 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/PhoenixOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/module/PhoenixOdometryThread.java @@ -17,9 +17,6 @@ import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.hardware.ParentDevice; -import edu.wpi.first.wpilibj.DriverStation; -import frc.robot.subsystems.drive.DriveSubsystem; - import java.util.ArrayList; import java.util.List; import java.util.Queue; @@ -27,6 +24,10 @@ import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; +import edu.wpi.first.wpilibj.Timer; +import frc.robot.subsystems.drive.DriveSubsystem; +//import org.littletonrobotics.junction.Logger; + /** * Provides an interface for asynchronously reading high-frequency measurements to a set of queues. * @@ -40,6 +41,7 @@ public class PhoenixOdometryThread extends Thread { new ReentrantLock(); // Prevents conflicts when registering signals private BaseStatusSignal[] signals = new BaseStatusSignal[0]; private final List> queues = new ArrayList<>(); + private final List> timestampQueues = new ArrayList<>(); private boolean isCANFD = false; private static PhoenixOdometryThread instance = null; @@ -54,7 +56,13 @@ public static PhoenixOdometryThread getInstance() { private PhoenixOdometryThread() { setName("PhoenixOdometryThread"); setDaemon(true); - start(); + } + + @Override + public void start() { + if (!timestampQueues.isEmpty()) { + super.start(); + } } public Queue registerSignal(ParentDevice device, StatusSignal signal) { @@ -75,6 +83,17 @@ public Queue registerSignal(ParentDevice device, StatusSignal si return queue; } + public Queue makeTimestampQueue() { + Queue queue = new ArrayBlockingQueue<>(20); + DriveSubsystem.odometryLock.lock(); + try { + timestampQueues.add(queue); + } finally { + DriveSubsystem.odometryLock.unlock(); + } + return queue; + } + @Override public void run() { while (true) { @@ -89,13 +108,10 @@ public void run() { // of Pro licensing. No reasoning for this behavior // is provided by the documentation. Thread.sleep((long) (1000.0 / Module.ODOMETRY_FREQUENCY)); - if (signals.length > 0) { - BaseStatusSignal.refreshAll(signals); - } + if (signals.length > 0) BaseStatusSignal.refreshAll(signals); } } catch (InterruptedException e) { - DriverStation.reportError(e.toString(), true); - Thread.currentThread().interrupt(); + e.printStackTrace(); } finally { signalsLock.unlock(); } @@ -103,9 +119,20 @@ public void run() { // Save new data to queues DriveSubsystem.odometryLock.lock(); try { + double timestamp = Timer.getFPGATimestamp() / 1e6; + double totalLatency = 0.0; + for (BaseStatusSignal signal : signals) { + totalLatency += signal.getTimestamp().getLatency(); + } + if (signals.length > 0) { + timestamp -= totalLatency / signals.length; + } for (int i = 0; i < signals.length; i++) { queues.get(i).offer(signals[i].getValueAsDouble()); } + for (int i = 0; i < timestampQueues.size(); i++) { + timestampQueues.get(i).offer(timestamp); + } } finally { DriveSubsystem.odometryLock.unlock(); } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index 15dd4e85..4a035f94 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -1,6 +1,9 @@ package frc.robot.subsystems.shooter; +import org.littletonrobotics.junction.AutoLog; + public interface ShooterIO { + @AutoLog class ShooterIOInputs { protected double tlVelocityRPM = 0.0; protected double trVelocityRPM = 0.0; @@ -245,7 +248,8 @@ default void setKickerVoltage(double voltage) {} default void setIntakeVoltage(double voltage) {} default void setLeftVelocityRpm(double rpm) {} default void setRightVelocityRpm(double rpm) {} - default void updateInputs(ShooterIOInputs inputs) {} + default boolean hasPiece() {return false;} + default void updateInputs(ShooterIOInputsAutoLogged inputs) {} default void stop() {} } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOKraken.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOKraken.java index 042e99bb..4e0c21e2 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOKraken.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOKraken.java @@ -30,6 +30,8 @@ public class ShooterIOKraken implements ShooterIO { private final VelocityVoltage m_velRequest; private final NeutralOut m_stopRequest; + private double[] m_prevKickerCurrent = new double[] {0.0, 0.0, 0.0, 0.0}; + private final StatusSignal m_leftVelSignal; private final StatusSignal m_rightVelSignal; @@ -53,7 +55,7 @@ public class ShooterIOKraken implements ShooterIO { public ShooterIOKraken() { m_tof = new TimeOfFlight(28); m_tof.setRangingMode(TimeOfFlight.RangingMode.Short, 25); - + // general motor configs TalonFXConfiguration shooterConfig = new TalonFXConfiguration(); shooterConfig.CurrentLimits.SupplyCurrentLimitEnable = true; @@ -144,7 +146,7 @@ public ShooterIOKraken() { } @Override - public void updateInputs(ShooterIOInputs inputs) { + public void updateInputs(ShooterIOInputsAutoLogged inputs) { BaseStatusSignal.refreshAll( m_leftVelSignal, m_rightVelSignal, @@ -180,17 +182,29 @@ public void updateInputs(ShooterIOInputs inputs) { inputs.indexerCurrentDraw = m_indexerCurrentDrawSignal.getValueAsDouble(); inputs.kickerCurrentDraw = m_kickerCurrentDrawSignal.getValueAsDouble(); + m_prevKickerCurrent = new double[] { + m_prevKickerCurrent[1], + m_prevKickerCurrent[2], + m_prevKickerCurrent[3], + inputs.kickerCurrentDraw + }; + inputs.tlTemperature = m_leftTemperatureSignal.getValueAsDouble(); inputs.trTemperature = m_rightTemperatureSignal.getValueAsDouble(); inputs.intakeTemperature = m_intakeTemperatureSignal.getValueAsDouble(); inputs.indexerTemperature = m_indexerTemperatureSignal.getValueAsDouble(); - inputs.tofDistanceIn = m_tof.getRange(); + inputs.tofDistanceIn = m_tof.getRange(); m_leftProperty.updateIfChanged(); m_rightProperty.updateIfChanged(); } + @Override + public boolean hasPiece() { + return m_tof.getRange() < 60.0; + } + @Override public void setMotorVoltageTL(double voltage) { m_leftTalon.setVoltage(voltage); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOPrototype.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOPrototype.java index 32d714d1..e0b2aeb5 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOPrototype.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOPrototype.java @@ -123,7 +123,7 @@ public void setRightVelocityRpm(double rpm) { } @Override - public void updateInputs(ShooterIOInputs inputs) { + public void updateInputs(ShooterIOInputsAutoLogged inputs) { m_topLeftPidProperty.updateIfChanged(); m_topRightPidProperty.updateIfChanged(); m_bottomLeftPidProperty.updateIfChanged(); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java index a91dcea5..7b56cb18 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java @@ -18,7 +18,7 @@ public class ShooterIOSim implements ShooterIO { private double kickerAppliedVolts = 0.0; @Override - public void updateInputs(ShooterIOInputs inputs) { + public void updateInputs(ShooterIOInputsAutoLogged inputs) { m_simLeft.update(LOOP_PERIOD_SECS); m_simRight.update(LOOP_PERIOD_SECS); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIntakeIOPrototype.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIntakeIOPrototype.java deleted file mode 100644 index 13f84191..00000000 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIntakeIOPrototype.java +++ /dev/null @@ -1,26 +0,0 @@ -package frc.robot.subsystems.shooter; - -import com.revrobotics.CANSparkLowLevel; -import com.revrobotics.CANSparkMax; - -public class ShooterIntakeIOPrototype implements ShooterIO{ - private final CANSparkMax m_leftIntake; - private final CANSparkMax m_rightIntake; - - public ShooterIntakeIOPrototype() { - m_leftIntake = new CANSparkMax(20, CANSparkLowLevel.MotorType.kBrushless); - m_rightIntake = new CANSparkMax(21, CANSparkLowLevel.MotorType.kBrushless); - - m_rightIntake.setInverted(true); - m_leftIntake.setInverted(true); - - m_rightIntake.burnFlash(); - m_leftIntake.burnFlash(); - } - - @Override - public void setIntakeVoltage(double voltage) { - m_leftIntake.setVoltage(voltage); - m_rightIntake.setVoltage(voltage); - } -} diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 9793763c..49890e33 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -1,23 +1,49 @@ package frc.robot.subsystems.shooter; +import com.gos.lib.properties.GosDoubleProperty; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import lib.logger.DataLogUtil; import lib.logger.DataLogUtil.DataLogTable; +import org.littletonrobotics.junction.Logger; +import lib.utils.AimbotUtils; +import lib.utils.AllianceFlipUtil; +import lib.utils.FieldConstants; + +import java.util.function.Supplier; public class ShooterSubsystem extends SubsystemBase { private final ShooterIO m_io; - private final ShooterIO.ShooterIOInputs m_inputs; + private final ShooterIOInputsAutoLogged m_inputs; + + private final Supplier m_poseSupplier; private final DataLogTable m_logTable = DataLogUtil.getTable("Shooter"); + private double m_leftSpeedSetpoint = 0.0; + private double m_rightSpeedSetpoint = 0.0; + + private final GosDoubleProperty m_leftPower = new + GosDoubleProperty(false, "Shooter/Left RPM", 3600); + private final GosDoubleProperty m_rightPower = new + GosDoubleProperty(false, "Shooter/Right RPM", 3600); + public ShooterSubsystem(ShooterIO io) { + this(io, Pose2d::new); + } + + public ShooterSubsystem(ShooterIO io, Supplier poseSupplier) { m_io = io; - m_inputs = new ShooterIO.ShooterIOInputs(); + m_inputs = new ShooterIOInputsAutoLogged(); + m_poseSupplier = poseSupplier; // turn on logging setupLogging(); } @@ -25,6 +51,7 @@ public ShooterSubsystem(ShooterIO io) { @Override public void periodic() { m_io.updateInputs(m_inputs); + Logger.processInputs("Shooter", m_inputs); } public void setShooterPowerLeft(double power) { @@ -47,11 +74,21 @@ public void setIntakePower(double power) { public Command runShooterVelocity(boolean runKicker) { return runEnd(() -> { - m_io.setLeftVelocityRpm(0.0); - m_io.setRightVelocityRpm(0.0); +// m_io.setLeftVelocityRpm(m_leftPower.getValue()); +// m_io.setRightVelocityRpm(m_rightPower.getValue()); + + Pose3d speakerPose = new Pose3d(AllianceFlipUtil.apply(FieldConstants.CENTER_SPEAKER), new Rotation3d()); + Translation2d speakerPoseGround = speakerPose.getTranslation().toTranslation2d(); + double groundDistance = m_poseSupplier.get().getTranslation().getDistance(speakerPoseGround); + + m_leftSpeedSetpoint = AimbotUtils.getLeftSpeed(groundDistance); + m_rightSpeedSetpoint = AimbotUtils.getRightSpeed(groundDistance); + + m_io.setLeftVelocityRpm(m_leftSpeedSetpoint); + m_io.setRightVelocityRpm(m_rightSpeedSetpoint); if (runKicker) { - m_io.setKickerVoltage(3.0); + m_io.setKickerVoltage(12.0); m_io.setIntakeVoltage(0.05); } else { m_io.setKickerVoltage(0.0); @@ -68,7 +105,7 @@ public Command runShooterVelocity(boolean runKicker) { } public boolean hasPiece() { - return m_inputs.tofDistanceIn < 60; + return m_io.hasPiece(); } /** Command Factories */ @@ -83,11 +120,11 @@ public Command intakeCommand(double intakePower, double kickerPower, double time setKickerPower(kickerPower); timer.restart(); } else if (!timer.hasElapsed(timeout)) { - setShooterPowerLeft(0.0); - setShooterPowerRight(0.0); - setKickerPower(-0.25); + setKickerPower(-0.1); setIntakePower(0.0); } else { + setShooterPowerRight(0.0); + setShooterPowerLeft(0.0); setKickerPower(0.0); setIntakePower(0.0); } @@ -111,6 +148,9 @@ public void setupLogging() { m_logTable.addDouble("LeftVelocity", () -> m_inputs.tlVelocityRPM, true); m_logTable.addDouble("RightVelocity", () -> m_inputs.trVelocityRPM, true); + m_logTable.addDouble("LeftSetpoint", () -> m_leftSpeedSetpoint, true); + m_logTable.addDouble("RightSetpoint", () -> m_rightSpeedSetpoint, true); + m_logTable.addDouble("LeftAppliedVolts", () -> m_inputs.tlAppliedVolts, true); m_logTable.addDouble("RightAppliedVolts", () -> m_inputs.trAppliedVolts, true); @@ -119,5 +159,12 @@ public void setupLogging() { m_logTable.addDouble("LeftTemperature", () -> m_inputs.tlTemperature, false); m_logTable.addDouble("RightTemperature", () -> m_inputs.trTemperature, false); + + m_logTable.addDouble("IntakeCurrent", () -> m_inputs.intakeCurrentDraw, true); + } + + public boolean atSpeed() { + return Math.abs(m_leftSpeedSetpoint - m_inputs.tlVelocityRPM) < 75 + && Math.abs(m_rightSpeedSetpoint - m_inputs.trVelocityRPM) < 75; } } diff --git a/src/main/java/frc/robot/subsystems/vision/PhotonVisionIOInputs.java b/src/main/java/frc/robot/subsystems/vision/PhotonVisionIOInputs.java index 92d2619c..e859e47f 100644 --- a/src/main/java/frc/robot/subsystems/vision/PhotonVisionIOInputs.java +++ b/src/main/java/frc/robot/subsystems/vision/PhotonVisionIOInputs.java @@ -1,5 +1,8 @@ package frc.robot.subsystems.vision; +import org.littletonrobotics.junction.AutoLog; + +@AutoLog public class PhotonVisionIOInputs { protected boolean camConnected = false; protected double camLatency = 0.0; diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index b16d3eb2..82ee25bf 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -12,7 +12,10 @@ import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.util.Units; +import lib.logger.DataLogUtil; import lib.utils.PoseEstimator; +import org.littletonrobotics.junction.Logger; import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; @@ -30,10 +33,13 @@ public class VisionSubsystem { private final String m_name; private Pose3d m_lastEstimatedPose = new Pose3d(); - private final double xyStdDevCoefficient = 0.01; - private final double thetaStdDevCoefficient = 0.01; + private final double xyStdDevCoefficient = Units.inchesToMeters(8.0); + private final double thetaStdDevCoefficient = Units.degreesToRadians(24.0); - private final PhotonVisionIOInputs inputs = new PhotonVisionIOInputs(); + private final double xyStdDevMultiTagCoefficient = Units.inchesToMeters(4.0); + private final double thetaStdDevMultiTagCoefficient = Units.degreesToRadians(12.0); + + private final PhotonVisionIOInputsAutoLogged inputs = new PhotonVisionIOInputsAutoLogged(); public VisionSubsystem(String camName, Transform3d camPose) { m_camera = new PhotonCamera(camName); @@ -47,7 +53,7 @@ public VisionSubsystem(String camName, Transform3d camPose) { m_camera, robotToCam); - m_photonPoseEstimator.setMultiTagFallbackStrategy(PhotonPoseEstimator.PoseStrategy.LOWEST_AMBIGUITY); + m_photonPoseEstimator.setMultiTagFallbackStrategy(PhotonPoseEstimator.PoseStrategy.CLOSEST_TO_REFERENCE_POSE); } catch (IOException e){ throw new IllegalStateException(e); } @@ -59,6 +65,18 @@ public void updateInputs() { inputs.setTagIds(getTargetIDs()); inputs.setNumTagsFound(m_camera.getLatestResult().targets.size()); inputs.setTagsFound(m_camera.getLatestResult().hasTargets()); + + Logger.processInputs("Vision/" + m_name, inputs); + } + + public double getTagDistance(){ + var latestResult = m_camera.getLatestResult(); + if (latestResult.hasTargets()){ + var bestTarget = latestResult.getBestTarget(); + var camToTarget = bestTarget.getBestCameraToTarget(); + return camToTarget.getX(); + } + return -1; } public Optional getPose(Pose2d prevEstimatedRobotPose) { @@ -66,6 +84,7 @@ public Optional getPose(Pose2d prevEstima m_photonPoseEstimator.setReferencePose(prevEstimatedRobotPose); PhotonPipelineResult camResult = m_camera.getLatestResult(); + Optional opPose = m_photonPoseEstimator.update(camResult); if (opPose.isEmpty()) { @@ -74,11 +93,35 @@ public Optional getPose(Pose2d prevEstima EstimatedRobotPose estPose = opPose.get(); m_lastEstimatedPose = estPose.estimatedPose; - double distance = estPose.estimatedPose.toPose2d().getTranslation().getDistance( - robotToCam.getTranslation().toTranslation2d()); + // find average distance to tags + int numTags = 0; + double avgDist = 0; + for (var tgt : camResult.targets) { + var tagPose = m_photonPoseEstimator.getFieldTags().getTagPose(tgt.getFiducialId()); + + if (tagPose.isEmpty()) { + continue; + } + + numTags++; + avgDist += + tagPose.get().toPose2d().getTranslation().getDistance(estPose.estimatedPose.toPose2d().getTranslation()); + } + + avgDist /= numTags; + + double xyStdDev; + double thetaStdDev; + + if (estPose.strategy != PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR) { + xyStdDev = xyStdDevCoefficient * Math.pow(avgDist, 2.0); + thetaStdDev = thetaStdDevCoefficient * Math.pow(avgDist, 2.0); + } else { + xyStdDev = xyStdDevMultiTagCoefficient * Math.pow(avgDist, 2.0); + thetaStdDev = thetaStdDevMultiTagCoefficient * Math.pow(avgDist, 2.0); + } - double xyStdDev = xyStdDevCoefficient * Math.pow(distance, 2.0); - double thetaStdDev = thetaStdDevCoefficient * Math.pow(distance, 2.0); + Logger.recordOutput("Vision/" + m_name + "/Estimated Pose", estPose.estimatedPose); return Optional.of(new PoseEstimator.TimestampedVisionUpdate(estPose.timestampSeconds, estPose.estimatedPose.toPose2d(), diff --git a/src/main/java/lib/utils/AimbotUtils.java b/src/main/java/lib/utils/AimbotUtils.java index 44766f2d..b36cc3ed 100644 --- a/src/main/java/lib/utils/AimbotUtils.java +++ b/src/main/java/lib/utils/AimbotUtils.java @@ -9,18 +9,44 @@ public class AimbotUtils { private static final InterpolatingDoubleTreeMap m_angleLerpTable = new InterpolatingDoubleTreeMap(); - private static final InterpolatingDoubleTreeMap m_speedLerpTable = new InterpolatingDoubleTreeMap(); + private static final InterpolatingDoubleTreeMap m_leftSpeedLerpTable = new InterpolatingDoubleTreeMap(); + private static final InterpolatingDoubleTreeMap m_rightSpeedLerpTable = new InterpolatingDoubleTreeMap(); static { // angle measurements, meters -> degrees - m_angleLerpTable.put(Units.inchesToMeters(18.0), 50.0); - m_angleLerpTable.put(Units.inchesToMeters(46.0), 40.0); - m_angleLerpTable.put(Units.inchesToMeters(97.0), 31.5); + m_angleLerpTable.put(Units.inchesToMeters(18.0), 57.0); + m_angleLerpTable.put(Units.inchesToMeters(46.0), 45.0); + m_angleLerpTable.put(Units.inchesToMeters(97.0), 33.5); + m_angleLerpTable.put(Units.inchesToMeters(116.0), 29.75); + m_angleLerpTable.put(Units.inchesToMeters(155.0), 24.65); +// m_angleLerpTable.put(Units.inchesToMeters(229.0), 0.0); //flywheel measurements, meters -> RPM - m_speedLerpTable.put(Units.inchesToMeters(18.0), 3600.0); - m_speedLerpTable.put(Units.inchesToMeters(46.0), 3600.0); - m_speedLerpTable.put(Units.inchesToMeters(97.0), 3600.0); + m_leftSpeedLerpTable.put(Units.inchesToMeters(18.0), 3600.0); + m_leftSpeedLerpTable.put(Units.inchesToMeters(46.0), 3600.0); + m_leftSpeedLerpTable.put(Units.inchesToMeters(97.0), 3600.0); + m_leftSpeedLerpTable.put(Units.inchesToMeters(116.0), 3800.0); + m_leftSpeedLerpTable.put(Units.inchesToMeters(155.0), 5000.0); +// m_leftSpeedLerpTable.put(Units.inchesToMeters(229.0), 0.0); + + m_rightSpeedLerpTable.put(Units.inchesToMeters(18.0), 3600.0); + m_rightSpeedLerpTable.put(Units.inchesToMeters(46.0), 3600.0); + m_rightSpeedLerpTable.put(Units.inchesToMeters(97.0), 3600.0); + m_rightSpeedLerpTable.put(Units.inchesToMeters(116.0), 3800.0); + m_rightSpeedLerpTable.put(Units.inchesToMeters(155.0), 4600.0); +// m_rightSpeedLerpTable.put(Units.inchesToMeters(229.0), 0.0); + } + + public static double getWristAngle(double distance) { + return m_angleLerpTable.get(distance); + } + + public static double getLeftSpeed(double distance) { + return m_leftSpeedLerpTable.get(distance); + } + + public static double getRightSpeed(double distance) { + return m_rightSpeedLerpTable.get(distance); } /** Gets the top point of the shooter for checking limits*/ diff --git a/src/main/java/lib/utils/FieldConstants.java b/src/main/java/lib/utils/FieldConstants.java index eaad9c4a..4bee1313 100644 --- a/src/main/java/lib/utils/FieldConstants.java +++ b/src/main/java/lib/utils/FieldConstants.java @@ -29,6 +29,13 @@ private FieldConstants() {} public static final Translation2d AMP_CENTER = new Translation2d(Units.inchesToMeters(72.455), Units.inchesToMeters(322.996)); + public static final Pose2d AMP_LINEUP = + AllianceFlipUtil.apply(new Pose2d( + AMP_CENTER.getX(), + AMP_CENTER.getY() - Units.inchesToMeters(20), + Rotation2d.fromDegrees(90) + )); + /** Staging locations for each note */ public static final class StagingLocations { public static final double CENTERLINE_X = FIELD_LENGTH / 2.0; diff --git a/src/main/java/lib/utils/LocalADStarAK.java b/src/main/java/lib/utils/LocalADStarAK.java new file mode 100644 index 00000000..92750795 --- /dev/null +++ b/src/main/java/lib/utils/LocalADStarAK.java @@ -0,0 +1,152 @@ +package lib.utils; + +import com.pathplanner.lib.path.GoalEndState; +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.path.PathPoint; +import com.pathplanner.lib.pathfinding.LocalADStar; +import com.pathplanner.lib.pathfinding.Pathfinder; +import edu.wpi.first.math.Pair; +import edu.wpi.first.math.geometry.Translation2d; +import java.util.ArrayList; +import java.util.Collections; +import java.util.List; +import org.littletonrobotics.junction.LogTable; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.inputs.LoggableInputs; + +// NOTE: This file is available at +// https://gist.github.com/mjansen4857/a8024b55eb427184dbd10ae8923bd57d + +public class LocalADStarAK implements Pathfinder { + private final ADStarIO io = new ADStarIO(); + + /** + * Get if a new path has been calculated since the last time a path was retrieved + * + * @return True if a new path is available + */ + @Override + public boolean isNewPathAvailable() { + if (!Logger.hasReplaySource()) { + io.updateIsNewPathAvailable(); + } + + Logger.processInputs("LocalADStarAK", io); + + return io.isNewPathAvailable; + } + + /** + * Get the most recently calculated path + * + * @param constraints The path constraints to use when creating the path + * @param goalEndState The goal end state to use when creating the path + * @return The PathPlannerPath created from the points calculated by the pathfinder + */ + @Override + public PathPlannerPath getCurrentPath(PathConstraints constraints, GoalEndState goalEndState) { + if (!Logger.hasReplaySource()) { + io.updateCurrentPathPoints(constraints, goalEndState); + } + + Logger.processInputs("LocalADStarAK", io); + + if (io.currentPathPoints.isEmpty()) { + return null; + } + + return PathPlannerPath.fromPathPoints(io.currentPathPoints, constraints, goalEndState); + } + + /** + * Set the start position to pathfind from + * + * @param startPosition Start position on the field. If this is within an obstacle it will be + * moved to the nearest non-obstacle node. + */ + @Override + public void setStartPosition(Translation2d startPosition) { + if (!Logger.hasReplaySource()) { + io.adStar.setStartPosition(startPosition); + } + } + + /** + * Set the goal position to pathfind to + * + * @param goalPosition Goal position on the field. f this is within an obstacle it will be moved + * to the nearest non-obstacle node. + */ + @Override + public void setGoalPosition(Translation2d goalPosition) { + if (!Logger.hasReplaySource()) { + io.adStar.setGoalPosition(goalPosition); + } + } + + /** + * Set the dynamic obstacles that should be avoided while pathfinding. + * + * @param obs A List of Translation2d pairs representing obstacles. Each Translation2d represents + * opposite corners of a bounding box. + * @param currentRobotPos The current position of the robot. This is needed to change the start + * position of the path to properly avoid obstacles + */ + @Override + public void setDynamicObstacles( + List> obs, Translation2d currentRobotPos) { + if (!Logger.hasReplaySource()) { + io.adStar.setDynamicObstacles(obs, currentRobotPos); + } + } + + private static class ADStarIO implements LoggableInputs { + public LocalADStar adStar = new LocalADStar(); + public boolean isNewPathAvailable = false; + public List currentPathPoints = Collections.emptyList(); + + @Override + public void toLog(LogTable table) { + table.put("IsNewPathAvailable", isNewPathAvailable); + + double[] pointsLogged = new double[currentPathPoints.size() * 2]; + int idx = 0; + for (PathPoint point : currentPathPoints) { + pointsLogged[idx] = point.position.getX(); + pointsLogged[idx + 1] = point.position.getY(); + idx += 2; + } + + table.put("CurrentPathPoints", pointsLogged); + } + + @Override + public void fromLog(LogTable table) { + isNewPathAvailable = table.get("IsNewPathAvailable", false); + + double[] pointsLogged = table.get("CurrentPathPoints", new double[0]); + + List pathPoints = new ArrayList<>(); + for (int i = 0; i < pointsLogged.length; i += 2) { + pathPoints.add(new PathPoint(new Translation2d(pointsLogged[i], pointsLogged[i + 1]), null)); + } + + currentPathPoints = pathPoints; + } + + public void updateIsNewPathAvailable() { + isNewPathAvailable = adStar.isNewPathAvailable(); + } + + public void updateCurrentPathPoints(PathConstraints constraints, GoalEndState goalEndState) { + PathPlannerPath currentPath = adStar.getCurrentPath(constraints, goalEndState); + + if (currentPath != null) { + currentPathPoints = currentPath.getAllPathPoints(); + } else { + currentPathPoints = Collections.emptyList(); + } + } + } +} diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json new file mode 100644 index 00000000..60360535 --- /dev/null +++ b/vendordeps/AdvantageKit.json @@ -0,0 +1,42 @@ +{ + "fileName": "AdvantageKit.json", + "name": "AdvantageKit", + "version": "3.1.1", + "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", + "frcYear": "2024", + "mavenUrls": [], + "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", + "javaDependencies": [ + { + "groupId": "org.littletonrobotics.akit.junction", + "artifactId": "wpilib-shim", + "version": "3.1.1" + }, + { + "groupId": "org.littletonrobotics.akit.junction", + "artifactId": "junction-core", + "version": "3.1.1" + }, + { + "groupId": "org.littletonrobotics.akit.conduit", + "artifactId": "conduit-api", + "version": "3.1.1" + } + ], + "jniDependencies": [ + { + "groupId": "org.littletonrobotics.akit.conduit", + "artifactId": "conduit-wpilibio", + "version": "3.1.1", + "skipInvalidPlatforms": false, + "isJar": false, + "validPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "cppDependencies": [] +} \ No newline at end of file diff --git a/vendordeps/photonlib-json-1.0.json b/vendordeps/photonlib-json-1.0.json index d2fb2a57..5850529a 100644 --- a/vendordeps/photonlib-json-1.0.json +++ b/vendordeps/photonlib-json-1.0.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2024.2.8", + "version": "v2024.2.9", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2024", "mavenUrls": [ @@ -14,7 +14,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2024.2.8", + "version": "v2024.2.9", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -29,7 +29,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2024.2.8", + "version": "v2024.2.9", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -46,12 +46,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2024.2.8" + "version": "v2024.2.9" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2024.2.8" + "version": "v2024.2.9" } ] } diff --git a/vendordeps/playingwithfusion2024.json b/vendordeps/playingwithfusion2024.json index 07d47e82..9dde4f2e 100644 --- a/vendordeps/playingwithfusion2024.json +++ b/vendordeps/playingwithfusion2024.json @@ -1,7 +1,7 @@ { "fileName": "playingwithfusion2024.json", "name": "PlayingWithFusion", - "version": "2024.01.28", + "version": "2024.03.09", "uuid": "14b8ad04-24df-11ea-978f-2e728ce88125", "frcYear": "2024", "jsonUrl": "https://www.playingwithfusion.com/frc/playingwithfusion2024.json", @@ -12,14 +12,14 @@ { "groupId": "com.playingwithfusion.frc", "artifactId": "PlayingWithFusion-java", - "version": "2024.01.28" + "version": "2024.03.09" } ], "jniDependencies": [ { "groupId": "com.playingwithfusion.frc", "artifactId": "PlayingWithFusion-driver", - "version": "2024.01.28", + "version": "2024.03.09", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -36,7 +36,7 @@ { "groupId": "com.playingwithfusion.frc", "artifactId": "PlayingWithFusion-cpp", - "version": "2024.01.28", + "version": "2024.03.09", "headerClassifier": "headers", "sharedLibrary": false, "libName": "PlayingWithFusion", @@ -53,7 +53,7 @@ { "groupId": "com.playingwithfusion.frc", "artifactId": "PlayingWithFusion-driver", - "version": "2024.01.28", + "version": "2024.03.09", "headerClassifier": "headers", "sharedLibrary": true, "libName": "PlayingWithFusionDriver",