From 93247f69a53d57f7d13512d48142a235661a1c10 Mon Sep 17 00:00:00 2001 From: POBots-353 Date: Sat, 8 Jun 2024 15:30:54 -0400 Subject: [PATCH] Maker Faire --- autos.chor | 1080 ++++++++++++++++- src/main/deploy/choreo/Instant DNP.1.traj | 123 ++ src/main/deploy/choreo/Instant DNP.2.traj | 303 +++++ src/main/deploy/choreo/Instant DNP.3.traj | 465 +++++++ src/main/deploy/choreo/Instant DNP.traj | 861 +++++++++++++ .../deploy/pathplanner/autos/Instant DNP.auto | 87 ++ src/main/java/frc/robot/RobotContainer.java | 8 + src/main/java/frc/robot/subsystems/Arm.java | 3 + .../java/frc/robot/subsystems/Climber.java | 5 + .../java/frc/robot/subsystems/Intake.java | 2 + 10 files changed, 2936 insertions(+), 1 deletion(-) create mode 100644 src/main/deploy/choreo/Instant DNP.1.traj create mode 100644 src/main/deploy/choreo/Instant DNP.2.traj create mode 100644 src/main/deploy/choreo/Instant DNP.3.traj create mode 100644 src/main/deploy/choreo/Instant DNP.traj create mode 100644 src/main/deploy/pathplanner/autos/Instant DNP.auto diff --git a/autos.chor b/autos.chor index 6e4ee1b..d31007d 100644 --- a/autos.chor +++ b/autos.chor @@ -1284,7 +1284,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": true + "isTrajectoryStale": false }, "Source C5 C4 C3": { "waypoints": [ @@ -80814,6 +80814,1084 @@ } ], "isTrajectoryStale": false + }, + "Instant DNP": { + "waypoints": [ + { + "x": 1.3660341501235962, + "y": 5.548494815826416, + "heading": 3.1415, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 11 + }, + { + "x": 2.439382553100586, + "y": 5.571146488189697, + "heading": 3.149226247503918, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 22 + }, + { + "x": 1.8555432558059692, + "y": 7.168908596038818, + "heading": 1.5707963267948966, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 8 + }, + { + "x": 1.846291184425354, + "y": 7.742530822753906, + "heading": 1.5707956385465986, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 23 + }, + { + "x": 2.9286323070526126, + "y": 6.03047685623169, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 24 + }, + { + "x": 4.0665717124938965, + "y": 4.868249893188477, + "heading": 2.0788588780268507, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 1.3660341501235962, + "y": 5.548494815826416, + "heading": 3.1415, + "angularVelocity": 5.981539907314682e-29, + "velocityX": -4.995526911759336e-28, + "velocityY": 5.315254844280788e-26, + "timestamp": 0 + }, + { + "x": 1.4238967756143568, + "y": 5.549715933993696, + "heading": 3.1415000001333797, + "angularVelocity": 1.1018016721845479e-9, + "velocityX": 0.4779833769031679, + "velocityY": 0.010087239910803398, + "timestamp": 0.12105572763983921 + }, + { + "x": 1.5396220212494325, + "y": 5.552158170215426, + "heading": 3.141500000477872, + "angularVelocity": 2.8457359966718716e-9, + "velocityX": 0.9559667096411775, + "velocityY": 0.020174478889556504, + "timestamp": 0.24211145527967842 + }, + { + "x": 1.6606507980369853, + "y": 5.554712330812969, + "heading": 3.1423272347299114, + "angularVelocity": 0.0068334994813339075, + "velocityX": 0.9997773682186555, + "velocityY": 0.021099047912405276, + "timestamp": 0.36316718291951766 + }, + { + "x": 1.7816795748245382, + "y": 5.557266491410513, + "heading": 3.143707917770431, + "angularVelocity": 0.01140535080361435, + "velocityX": 0.9997773682186563, + "velocityY": 0.021099047912405293, + "timestamp": 0.48422291055935685 + }, + { + "x": 1.902708351612091, + "y": 5.559820652008057, + "heading": 3.145363123736773, + "angularVelocity": 0.01367309088643057, + "velocityX": 0.9997773682186563, + "velocityY": 0.02109904791240529, + "timestamp": 0.605278638199196 + }, + { + "x": 2.023737128399644, + "y": 5.5623748126056, + "heading": 3.147018329695959, + "angularVelocity": 0.013673090827311721, + "velocityX": 0.9997773682186563, + "velocityY": 0.02109904791240529, + "timestamp": 0.7263343658390352 + }, + { + "x": 2.144765905187197, + "y": 5.564928973203144, + "heading": 3.148399012770666, + "angularVelocity": 0.011405351086027542, + "velocityX": 0.9997773682186563, + "velocityY": 0.02109904791240529, + "timestamp": 0.8473900934788744 + }, + { + "x": 2.2657946819747496, + "y": 5.567483133800687, + "heading": 3.149226247026046, + "angularVelocity": 0.006833499508931699, + "velocityX": 0.9997773682186555, + "velocityY": 0.021099047912405276, + "timestamp": 0.9684458211187136 + }, + { + "x": 2.3815199276098253, + "y": 5.569925370022418, + "heading": 3.1492262473705384, + "angularVelocity": 2.845735928585778e-9, + "velocityX": 0.9559667096411774, + "velocityY": 0.020174478889556504, + "timestamp": 1.0895015487585529 + }, + { + "x": 2.439382553100586, + "y": 5.571146488189697, + "heading": 3.149226247503918, + "angularVelocity": 1.1018015962710774e-9, + "velocityX": 0.47798337690316783, + "velocityY": 0.0100872399108034, + "timestamp": 1.210557276398392 + }, + { + "x": 2.439382553100586, + "y": 5.571146488189697, + "heading": 3.149226247503918, + "angularVelocity": 3.3725230857726955e-25, + "velocityX": -1.0893794575213779e-23, + "velocityY": 2.8243441481859005e-24, + "timestamp": 1.3316130040382312 + }, + { + "x": 2.4362890387153437, + "y": 5.581045201413149, + "heading": 3.136124834798283, + "angularVelocity": -0.2303700895130178, + "velocityX": -0.05439514057377693, + "velocityY": 0.17405508112644477, + "timestamp": 1.388484157465007 + }, + { + "x": 2.4301904093335223, + "y": 5.600848905472547, + "heading": 3.1099250000899192, + "angularVelocity": -0.4606875916820856, + "velocityX": -0.1072359010561265, + "velocityY": 0.3482205453226071, + "timestamp": 1.445355310891783 + }, + { + "x": 2.4211633685912326, + "y": 5.630611256474957, + "heading": 3.0708177004805495, + "angularVelocity": -0.6876473792591979, + "velocityX": -0.15872793496112414, + "velocityY": 0.5233294774077369, + "timestamp": 1.5022264643185588 + }, + { + "x": 2.409237703542417, + "y": 5.670443910842478, + "heading": 3.0192485456812395, + "angularVelocity": -0.9067717408915852, + "velocityX": -0.2096962050219477, + "velocityY": 0.7004017321154934, + "timestamp": 1.5590976177453346 + }, + { + "x": 2.394318792156192, + "y": 5.720524196272314, + "heading": 2.9560131473235427, + "angularVelocity": -1.1119063804308933, + "velocityX": -0.26232827166822764, + "velocityY": 0.8805920473253224, + "timestamp": 1.6159687711721105 + }, + { + "x": 2.3760338351749253, + "y": 5.781058983135284, + "heading": 2.8822982817207805, + "angularVelocity": -1.2961732119196991, + "velocityX": -0.3215154938752848, + "velocityY": 1.0644198897937152, + "timestamp": 1.6728399245988863 + }, + { + "x": 2.353569474795138, + "y": 5.852065184732581, + "heading": 2.7992155915208365, + "angularVelocity": -1.4608933561883304, + "velocityX": -0.3950044798847132, + "velocityY": 1.2485451290999514, + "timestamp": 1.7297110780256622 + }, + { + "x": 2.326002731310179, + "y": 5.933042032319499, + "heading": 2.706475620370505, + "angularVelocity": -1.630703187156881, + "velocityX": -0.4847227781383496, + "velocityY": 1.4238650477026025, + "timestamp": 1.786582231452438 + }, + { + "x": 2.292962482296499, + "y": 6.0233120695479165, + "heading": 2.60247783021725, + "angularVelocity": -1.8286562499063208, + "velocityX": -0.5809667471615536, + "velocityY": 1.5872728402571346, + "timestamp": 1.8434533848792138 + }, + { + "x": 2.254554012450292, + "y": 6.122466608068637, + "heading": 2.485626955322059, + "angularVelocity": -2.0546598381487273, + "velocityX": -0.6753594314850648, + "velocityY": 1.743494417576513, + "timestamp": 1.9003245383059897 + }, + { + "x": 2.2110992611866678, + "y": 6.230363093640724, + "heading": 2.3547415371141285, + "angularVelocity": -2.3014377293481774, + "velocityX": -0.7640912597205168, + "velocityY": 1.897209377175871, + "timestamp": 1.9571956917327655 + }, + { + "x": 2.1630248930911256, + "y": 6.347033979020736, + "heading": 2.209048781749191, + "angularVelocity": -2.5618041236410543, + "velocityX": -0.845320785649975, + "velocityY": 2.051494973286085, + "timestamp": 2.0140668451595416 + }, + { + "x": 2.1171485756869948, + "y": 6.471321493889193, + "heading": 2.079206437440476, + "angularVelocity": -2.2830967280431977, + "velocityX": -0.8066711265703139, + "velocityY": 2.185422791336262, + "timestamp": 2.0709379985863174 + }, + { + "x": 2.07515399626246, + "y": 6.586581008184095, + "heading": 1.9645775517677246, + "angularVelocity": -2.015589253352165, + "velocityX": -0.7384161722446009, + "velocityY": 2.026677979079547, + "timestamp": 2.1278091520130933 + }, + { + "x": 2.0367546596649357, + "y": 6.692666568692638, + "heading": 1.8650472976205432, + "angularVelocity": -1.7501008534200189, + "velocityX": -0.6751988360314355, + "velocityY": 1.865366782918037, + "timestamp": 2.184680305439869 + }, + { + "x": 2.001724396449355, + "y": 6.78943559555366, + "heading": 1.7803812299137085, + "angularVelocity": -1.4887348436821468, + "velocityX": -0.6159583744100318, + "velocityY": 1.701548518540526, + "timestamp": 2.241551458866645 + }, + { + "x": 1.9698936739721948, + "y": 6.876757760558234, + "heading": 1.7102873807073258, + "angularVelocity": -1.2325026833969792, + "velocityX": -0.5596989081317597, + "velocityY": 1.5354386141825944, + "timestamp": 2.298422612293421 + }, + { + "x": 1.9411415756463717, + "y": 6.954522757040627, + "heading": 1.6544725733697885, + "angularVelocity": -0.9814256257243232, + "velocityX": -0.5055655915760988, + "velocityY": 1.3673891207872078, + "timestamp": 2.3552937657201967 + }, + { + "x": 1.9153842521448374, + "y": 7.022647857772365, + "heading": 1.612694477397962, + "angularVelocity": -0.7346096123339139, + "velocityX": -0.4529066486175296, + "velocityY": 1.1978849843348485, + "timestamp": 2.4121649191469725 + }, + { + "x": 1.8925619144079102, + "y": 7.0810849361144745, + "heading": 1.584807118963501, + "angularVelocity": -0.490360345343922, + "velocityX": -0.40129901297521586, + "velocityY": 1.0275346079862697, + "timestamp": 2.4690360725737484 + }, + { + "x": 1.8726278204084867, + "y": 7.1298262837468185, + "heading": 1.5707956385465986, + "angularVelocity": -0.24637236230741583, + "velocityX": -0.35051327075842725, + "velocityY": 0.8570486915673537, + "timestamp": 2.525907226000524 + }, + { + "x": 1.8555432558059692, + "y": 7.168908596038818, + "heading": 1.5707956385465986, + "angularVelocity": 1.2473082818664105e-25, + "velocityX": -0.30040826628414513, + "velocityY": 0.6872080120956212, + "timestamp": 2.5827783794273 + }, + { + "x": 1.8541434053434975, + "y": 7.255697115566546, + "heading": 1.5707956385465986, + "angularVelocity": 2.030292510674732e-25, + "velocityX": -0.012095508597799284, + "velocityY": 0.7499024447828313, + "timestamp": 2.6985114592852564 + }, + { + "x": 1.8527435790342417, + "y": 7.3424856354838495, + "heading": 1.5707956385465986, + "angularVelocity": 2.0302925236125038e-25, + "velocityX": -0.012095299900201072, + "velocityY": 0.749902448148979, + "timestamp": 2.8142445391432127 + }, + { + "x": 1.851343752724996, + "y": 7.429274155401153, + "heading": 1.5707956385465986, + "angularVelocity": 2.0302925236123036e-25, + "velocityX": -0.012095299900113238, + "velocityY": 0.7499024481489804, + "timestamp": 2.929977619001169 + }, + { + "x": 1.8499439264157502, + "y": 7.516062675318456, + "heading": 1.5707956385465986, + "angularVelocity": 2.030292523612118e-25, + "velocityX": -0.012095299900113238, + "velocityY": 0.7499024481489804, + "timestamp": 3.0457106988591254 + }, + { + "x": 1.8485441001065046, + "y": 7.602851195235759, + "heading": 1.5707956385465986, + "angularVelocity": 2.0302925236119252e-25, + "velocityX": -0.012095299900113238, + "velocityY": 0.7499024481489804, + "timestamp": 3.1614437787170817 + }, + { + "x": 1.847144273797259, + "y": 7.689639715153062, + "heading": 1.5707956385465986, + "angularVelocity": 2.0302925241560946e-25, + "velocityX": -0.012095299900113175, + "velocityY": 0.7499024481489763, + "timestamp": 3.277176858575038 + }, + { + "x": 1.846291184425354, + "y": 7.742530822753906, + "heading": 1.5707956385465986, + "angularVelocity": 2.0313924965619656e-25, + "velocityX": -0.007371180071868358, + "velocityY": 0.4570094191372061, + "timestamp": 3.3929099384329944 + }, + { + "x": 1.846291184425354, + "y": 7.742530822753906, + "heading": 1.5707956385465986, + "angularVelocity": 6.699868866352696e-26, + "velocityX": -2.082851323459477e-26, + "velocityY": -1.1898010382832076e-27, + "timestamp": 3.5086430182909507 + }, + { + "x": 1.8514053614725579, + "y": 7.734353446812523, + "heading": 1.5741246181907276, + "angularVelocity": 0.0666072607638542, + "velocityX": 0.10232604599321918, + "velocityY": -0.16361548279587393, + "timestamp": 3.5586222488726036 + }, + { + "x": 1.8616343027242503, + "y": 7.717998461212048, + "heading": 1.5807797683258857, + "angularVelocity": 0.13315831511822673, + "velocityX": 0.2046638400121233, + "velocityY": -0.32723564188838467, + "timestamp": 3.6086014794542565 + }, + { + "x": 1.8769784699431493, + "y": 7.693465718807923, + "heading": 1.590759853081497, + "angularVelocity": 0.19968464178948309, + "velocityX": 0.3070108731231987, + "velocityY": -0.4908587450950237, + "timestamp": 3.6585807100359093 + }, + { + "x": 1.8974382624232278, + "y": 7.66075518377616, + "heading": 1.6040651512683497, + "angularVelocity": 0.26621654699376207, + "velocityX": 0.40936589543235347, + "velocityY": -0.6544825650791681, + "timestamp": 3.7085599406175622 + }, + { + "x": 1.9230141026421597, + "y": 7.61986696268551, + "heading": 1.6206973511905303, + "angularVelocity": 0.33278223231163817, + "velocityX": 0.5117293708062944, + "velocityY": -0.8181042528025491, + "timestamp": 3.758539171199215 + }, + { + "x": 1.9537065533193043, + "y": 7.570801338700398, + "heading": 1.6406593358512553, + "angularVelocity": 0.3994056016551199, + "velocityX": 0.6141041052443037, + "velocityY": -0.9817202748840232, + "timestamp": 3.808518401780868 + }, + { + "x": 1.9895164788532218, + "y": 7.513558803979521, + "heading": 1.6639547636507832, + "angularVelocity": 0.4661021694095368, + "velocityX": 0.7164961348377226, + "velocityY": -1.1453264497011013, + "timestamp": 3.858497632362521 + }, + { + "x": 2.0304452716552572, + "y": 7.448140082586782, + "heading": 1.6905872964786974, + "angularVelocity": 0.5328720053903956, + "velocityX": 0.8189160242306792, + "velocityY": -1.3089181372222645, + "timestamp": 3.9084768629441737 + }, + { + "x": 2.0764951786721593, + "y": 7.374546131696564, + "heading": 1.720559229351038, + "angularVelocity": 0.5996877607664348, + "velocityX": 0.9213808712334727, + "velocityY": -1.47249067330048, + "timestamp": 3.9584560935258266 + }, + { + "x": 2.127669795519365, + "y": 7.292778100747622, + "heading": 1.7538690746086205, + "angularVelocity": 0.666473750594517, + "velocityX": 1.0239176604289628, + "velocityY": -1.636040211050365, + "timestamp": 4.008435324107479 + }, + { + "x": 2.1839748736231908, + "y": 7.202837211774282, + "heading": 1.7905071824942314, + "angularVelocity": 0.7330666650770846, + "velocityX": 1.1265695259521453, + "velocityY": -1.799565297957115, + "timestamp": 4.058414554689132 + }, + { + "x": 2.2454198021164973, + "y": 7.104724485664905, + "heading": 1.8304471922431862, + "angularVelocity": 0.7991321451758543, + "velocityX": 1.2294092521676905, + "velocityY": -1.9630699586118285, + "timestamp": 4.108393785270785 + }, + { + "x": 2.312020848816739, + "y": 6.998440133109207, + "heading": 1.8736268043801838, + "angularVelocity": 0.863951117983972, + "velocityX": 1.332574469937732, + "velocityY": -2.1265704037211566, + "timestamp": 4.158373015852438 + }, + { + "x": 2.383810472474797, + "y": 6.88398209646351, + "heading": 1.9198918718607836, + "angularVelocity": 0.9256858687533199, + "velocityX": 1.4363891324972011, + "velocityY": -2.2901120188055484, + "timestamp": 4.208352246434091 + }, + { + "x": 2.4608821710178113, + "y": 6.761343181507853, + "heading": 1.9687196716778628, + "angularVelocity": 0.9769618149144491, + "velocityX": 1.5420745306812864, + "velocityY": -2.453797578081906, + "timestamp": 4.258331477015743 + }, + { + "x": 2.5368643782269005, + "y": 6.6415841980546455, + "heading": 1.9971341230841648, + "angularVelocity": 0.5685251868749809, + "velocityX": 1.5202756490009244, + "velocityY": -2.3961750122894134, + "timestamp": 4.308310707597396 + }, + { + "x": 2.6079459028648024, + "y": 6.529904796280597, + "heading": 2.020833851230746, + "angularVelocity": 0.47419153657962093, + "velocityX": 1.422221266927541, + "velocityY": -2.2345162275276365, + "timestamp": 4.358289938179049 + }, + { + "x": 2.674025576949676, + "y": 6.426341391642361, + "heading": 2.0403388712032235, + "angularVelocity": 0.3902625099562424, + "velocityX": 1.3221426843880075, + "velocityY": -2.072128830975926, + "timestamp": 4.408269168760702 + }, + { + "x": 2.7350695794350304, + "y": 6.3309037565632575, + "heading": 2.055822477797095, + "angularVelocity": 0.3098008195339312, + "velocityX": 1.2213873998245102, + "velocityY": -1.9095459047370478, + "timestamp": 4.458248399342355 + }, + { + "x": 2.7910608455388015, + "y": 6.243596266012373, + "heading": 2.067371488792874, + "angularVelocity": 0.23107620628354833, + "velocityX": 1.120290677790572, + "velocityY": -1.7468754427551085, + "timestamp": 4.508227629924008 + }, + { + "x": 2.8419891436748235, + "y": 6.164421466785129, + "heading": 2.075038431959136, + "angularVelocity": 0.15340258497450884, + "velocityX": 1.0189892389963568, + "velocityY": -1.584154023697775, + "timestamp": 4.558206860505661 + }, + { + "x": 2.8878478013227533, + "y": 6.093381162904928, + "heading": 2.0788588780268507, + "angularVelocity": 0.07644067392099715, + "velocityX": 0.9175542943385027, + "velocityY": -1.4213965091787517, + "timestamp": 4.608186091087314 + }, + { + "x": 2.928632307052613, + "y": 6.03047685623169, + "heading": 2.0788588780268507, + "angularVelocity": -1.353091220301102e-27, + "velocityX": 0.8160290835856026, + "velocityY": -1.2586089449790203, + "timestamp": 4.6581653216689665 + }, + { + "x": 2.98215373218464, + "y": 5.969221498684807, + "heading": 2.0788588780268507, + "angularVelocity": -9.036957956814612e-28, + "velocityX": 0.9869522884399109, + "velocityY": -1.1295684888989306, + "timestamp": 4.7123943110121385 + }, + { + "x": 3.0392348647762897, + "y": 5.9112688793985475, + "heading": 2.0788588780268507, + "angularVelocity": -9.024177769261626e-28, + "velocityX": 1.052594438565491, + "velocityY": -1.0686649334274896, + "timestamp": 4.7666233003553105 + }, + { + "x": 3.096316073363738, + "y": 5.853316334965364, + "heading": 2.0788588780268507, + "angularVelocity": -9.024178327381887e-28, + "velocityX": 1.0525958399524507, + "velocityY": -1.0686635531126647, + "timestamp": 4.8208522896984825 + }, + { + "x": 3.153397281952565, + "y": 5.795363790533539, + "heading": 2.0788588780268507, + "angularVelocity": -9.024178327298733e-28, + "velocityX": 1.052595839977865, + "velocityY": -1.0686635530876325, + "timestamp": 4.8750812790416544 + }, + { + "x": 3.2104784905413917, + "y": 5.737411246101713, + "heading": 2.0788588780268507, + "angularVelocity": -9.024178314475913e-28, + "velocityX": 1.0525958399778652, + "velocityY": -1.0686635530876323, + "timestamp": 4.929310268384826 + }, + { + "x": 3.267559699130218, + "y": 5.679458701669888, + "heading": 2.0788588780268507, + "angularVelocity": -9.02417832729875e-28, + "velocityX": 1.0525958399778652, + "velocityY": -1.0686635530876323, + "timestamp": 4.983539257727998 + }, + { + "x": 3.324640907719045, + "y": 5.621506157238062, + "heading": 2.0788588780268507, + "angularVelocity": -9.024178340121581e-28, + "velocityX": 1.0525958399778652, + "velocityY": -1.0686635530876323, + "timestamp": 5.03776824707117 + }, + { + "x": 3.3817221163078717, + "y": 5.563553612806236, + "heading": 2.0788588780268507, + "angularVelocity": -9.024178352944405e-28, + "velocityX": 1.0525958399778652, + "velocityY": -1.0686635530876323, + "timestamp": 5.091997236414342 + }, + { + "x": 3.4388033248966985, + "y": 5.5056010683744105, + "heading": 2.0788588780268507, + "angularVelocity": -9.02417836576725e-28, + "velocityX": 1.0525958399778652, + "velocityY": -1.0686635530876323, + "timestamp": 5.146226225757514 + }, + { + "x": 3.495884533485525, + "y": 5.4476485239425845, + "heading": 2.0788588780268507, + "angularVelocity": -9.024178378590079e-28, + "velocityX": 1.0525958399778652, + "velocityY": -1.0686635530876323, + "timestamp": 5.200455215100686 + }, + { + "x": 3.5529657420743517, + "y": 5.389695979510759, + "heading": 2.0788588780268507, + "angularVelocity": -9.024178391412911e-28, + "velocityX": 1.0525958399778652, + "velocityY": -1.0686635530876323, + "timestamp": 5.254684204443858 + }, + { + "x": 3.6100469506631785, + "y": 5.331743435078933, + "heading": 2.0788588780268507, + "angularVelocity": -9.024178404235742e-28, + "velocityX": 1.0525958399778652, + "velocityY": -1.0686635530876323, + "timestamp": 5.30891319378703 + }, + { + "x": 3.6671281592520053, + "y": 5.273790890647108, + "heading": 2.0788588780268507, + "angularVelocity": -9.024178417058576e-28, + "velocityX": 1.0525958399778652, + "velocityY": -1.0686635530876323, + "timestamp": 5.363142183130202 + }, + { + "x": 3.724209367840832, + "y": 5.215838346215282, + "heading": 2.0788588780268507, + "angularVelocity": -9.024178429881409e-28, + "velocityX": 1.0525958399778652, + "velocityY": -1.0686635530876323, + "timestamp": 5.417371172473374 + }, + { + "x": 3.7812905764296585, + "y": 5.157885801783457, + "heading": 2.0788588780268507, + "angularVelocity": -9.024178442704241e-28, + "velocityX": 1.0525958399778652, + "velocityY": -1.0686635530876323, + "timestamp": 5.471600161816546 + }, + { + "x": 3.8383717850184853, + "y": 5.099933257351631, + "heading": 2.0788588780268507, + "angularVelocity": -9.024178455527075e-28, + "velocityX": 1.052595839977865, + "velocityY": -1.0686635530876316, + "timestamp": 5.525829151159718 + }, + { + "x": 3.8954217579928927, + "y": 5.04201242534106, + "heading": 2.0788588780268507, + "angularVelocity": -9.024178455214888e-28, + "velocityX": 1.0520198452046383, + "velocityY": -1.0680787658430528, + "timestamp": 5.58005814050289 + }, + { + "x": 3.9443217414721357, + "y": 4.992365991157971, + "heading": 2.0788588780268507, + "angularVelocity": -9.024178468659725e-28, + "velocityX": 0.9017314184078514, + "velocityY": -0.9154962093967133, + "timestamp": 5.634287129846062 + }, + { + "x": 3.985071730179638, + "y": 4.950993960159487, + "heading": 2.0788588780268507, + "angularVelocity": -9.02417846230043e-28, + "velocityX": 0.751442894309324, + "velocityY": -0.7629135541633372, + "timestamp": 5.688516119189234 + }, + { + "x": 4.017671722356639, + "y": 4.917896334131213, + "heading": 2.0788588780268507, + "angularVelocity": -1.5394220322890216e-27, + "velocityX": 0.6011543377786844, + "velocityY": -0.6103308660027776, + "timestamp": 5.742745108532406 + }, + { + "x": 4.042121717123779, + "y": 4.893073113965937, + "heading": 2.0788588780268507, + "angularVelocity": -2.3472970316100667e-28, + "velocityX": 0.4508657650323279, + "velocityY": -0.45774816137897073, + "timestamp": 5.796974097875578 + }, + { + "x": 4.058421713953444, + "y": 4.876524300199322, + "heading": 2.0788588780268507, + "angularVelocity": -9.024178457605914e-28, + "velocityX": 0.3005771825566498, + "velocityY": -0.3051654468773257, + "timestamp": 5.85120308721875 + }, + { + "x": 4.0665717124938965, + "y": 4.868249893188477, + "heading": 2.0788588780268507, + "angularVelocity": -9.024178457563788e-28, + "velocityX": 0.15028859359480273, + "velocityY": -0.15258272579050136, + "timestamp": 5.905432076561922 + }, + { + "x": 4.0665717124938965, + "y": 4.868249893188477, + "heading": 2.0788588780268507, + "angularVelocity": -2.9569195590907886e-28, + "velocityX": -5.9543467827551576e-27, + "velocityY": 8.214243587658572e-26, + "timestamp": 5.959661065905094 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 1.3660341501235962, + "y": 5.548494815826416, + "heading": 3.1415, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 11 + }, + { + "timestamp": 1.3316130040382312, + "isStopPoint": true, + "x": 2.439382553100586, + "y": 5.571146488189697, + "heading": 3.149226247503918, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 22 + }, + { + "timestamp": 2.5827783794273, + "isStopPoint": false, + "x": 1.8555432558059692, + "y": 7.168908596038818, + "heading": 1.5707963267948966, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 8 + }, + { + "timestamp": 3.5086430182909507, + "isStopPoint": true, + "x": 1.846291184425354, + "y": 7.742530822753906, + "heading": 1.5707956385465986, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 23 + }, + { + "timestamp": 4.6581653216689665, + "isStopPoint": false, + "x": 2.9286323070526126, + "y": 6.03047685623169, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 24 + }, + { + "timestamp": 5.959661065905094, + "isStopPoint": true, + "x": 4.0665717124938965, + "y": 4.868249893188477, + "heading": 2.0788588780268507, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + }, + { + "scope": [ + 1 + ], + "type": "StopPoint" + }, + { + "scope": [ + 2, + 3 + ], + "type": "ZeroAngularVelocity" + }, + { + "scope": [ + 2, + 3 + ], + "type": "MaxVelocity", + "velocity": 0.75 + }, + { + "scope": [ + 3 + ], + "type": "StopPoint" + }, + { + "scope": [ + 4, + 5 + ], + "type": "MaxVelocity", + "velocity": 1.5 + }, + { + "scope": [ + 0, + 1 + ], + "type": "MaxVelocity", + "velocity": 1 + }, + { + "scope": [ + 4, + 5 + ], + "type": "ZeroAngularVelocity" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [], + "eventMarkers": [ + { + "name": "Intake W2", + "target": 1, + "trajTargetIndex": 1, + "targetTimestamp": 1.3316130040382312, + "offset": -0.75, + "command": { + "type": "named", + "data": { + "name": "Start Intake" + } + } + }, + { + "name": "Arm to Amp", + "target": 3, + "trajTargetIndex": 3, + "targetTimestamp": 3.5086430182909507, + "offset": -1.6, + "command": { + "type": "named", + "data": { + "name": "Arm to Amp" + } + } + }, + { + "name": "Climb", + "target": 5, + "trajTargetIndex": 5, + "targetTimestamp": 5.959661065905094, + "offset": -0.75, + "command": { + "type": "named", + "data": { + "name": "Climb" + } + } + }, + { + "name": "Shooter Amp", + "target": 3, + "trajTargetIndex": 3, + "targetTimestamp": 3.5086430182909507, + "offset": -0.5, + "command": { + "type": "named", + "data": { + "name": "Warm Up Shooter Amp" + } + } + }, + { + "name": "Marker", + "target": 4, + "trajTargetIndex": 4, + "targetTimestamp": 4.6581653216689665, + "offset": 0, + "command": { + "type": "named", + "data": { + "name": "" + } + } + }, + { + "name": "Arm to Pickup", + "target": 3, + "trajTargetIndex": 3, + "targetTimestamp": 3.5086430182909507, + "offset": 0.1, + "command": { + "type": "named", + "data": { + "name": "Arm to Pickup" + } + } + } + ], + "isTrajectoryStale": false } }, "splitTrajectoriesAtStopPoints": true, diff --git a/src/main/deploy/choreo/Instant DNP.1.traj b/src/main/deploy/choreo/Instant DNP.1.traj new file mode 100644 index 0000000..4d2ede1 --- /dev/null +++ b/src/main/deploy/choreo/Instant DNP.1.traj @@ -0,0 +1,123 @@ +{ + "samples": [ + { + "x": 1.3660341501235962, + "y": 5.548494815826416, + "heading": 3.1415, + "angularVelocity": 5.981539907314682e-29, + "velocityX": -4.995526911759336e-28, + "velocityY": 5.315254844280788e-26, + "timestamp": 0 + }, + { + "x": 1.4238967756143568, + "y": 5.549715933993696, + "heading": 3.1415000001333797, + "angularVelocity": 1.1018016721845479e-9, + "velocityX": 0.4779833769031679, + "velocityY": 0.010087239910803398, + "timestamp": 0.12105572763983921 + }, + { + "x": 1.5396220212494325, + "y": 5.552158170215426, + "heading": 3.141500000477872, + "angularVelocity": 2.8457359966718716e-9, + "velocityX": 0.9559667096411775, + "velocityY": 0.020174478889556504, + "timestamp": 0.24211145527967842 + }, + { + "x": 1.6606507980369853, + "y": 5.554712330812969, + "heading": 3.1423272347299114, + "angularVelocity": 0.0068334994813339075, + "velocityX": 0.9997773682186555, + "velocityY": 0.021099047912405276, + "timestamp": 0.36316718291951766 + }, + { + "x": 1.7816795748245382, + "y": 5.557266491410513, + "heading": 3.143707917770431, + "angularVelocity": 0.01140535080361435, + "velocityX": 0.9997773682186563, + "velocityY": 0.021099047912405293, + "timestamp": 0.48422291055935685 + }, + { + "x": 1.902708351612091, + "y": 5.559820652008057, + "heading": 3.145363123736773, + "angularVelocity": 0.01367309088643057, + "velocityX": 0.9997773682186563, + "velocityY": 0.02109904791240529, + "timestamp": 0.605278638199196 + }, + { + "x": 2.023737128399644, + "y": 5.5623748126056, + "heading": 3.147018329695959, + "angularVelocity": 0.013673090827311721, + "velocityX": 0.9997773682186563, + "velocityY": 0.02109904791240529, + "timestamp": 0.7263343658390352 + }, + { + "x": 2.144765905187197, + "y": 5.564928973203144, + "heading": 3.148399012770666, + "angularVelocity": 0.011405351086027542, + "velocityX": 0.9997773682186563, + "velocityY": 0.02109904791240529, + "timestamp": 0.8473900934788744 + }, + { + "x": 2.2657946819747496, + "y": 5.567483133800687, + "heading": 3.149226247026046, + "angularVelocity": 0.006833499508931699, + "velocityX": 0.9997773682186555, + "velocityY": 0.021099047912405276, + "timestamp": 0.9684458211187136 + }, + { + "x": 2.3815199276098253, + "y": 5.569925370022418, + "heading": 3.1492262473705384, + "angularVelocity": 2.845735928585778e-9, + "velocityX": 0.9559667096411774, + "velocityY": 0.020174478889556504, + "timestamp": 1.0895015487585529 + }, + { + "x": 2.439382553100586, + "y": 5.571146488189697, + "heading": 3.149226247503918, + "angularVelocity": 1.1018015962710774e-9, + "velocityX": 0.47798337690316783, + "velocityY": 0.0100872399108034, + "timestamp": 1.210557276398392 + }, + { + "x": 2.439382553100586, + "y": 5.571146488189697, + "heading": 3.149226247503918, + "angularVelocity": 3.3725230857726955e-25, + "velocityX": -1.0893794575213779e-23, + "velocityY": 2.8243441481859005e-24, + "timestamp": 1.3316130040382312 + } + ], + "eventMarkers": [ + { + "timestamp": 0.5816130040382312, + "command": { + "type": "named", + "data": { + "name": "Start Intake" + } + } + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Instant DNP.2.traj b/src/main/deploy/choreo/Instant DNP.2.traj new file mode 100644 index 0000000..8bc0850 --- /dev/null +++ b/src/main/deploy/choreo/Instant DNP.2.traj @@ -0,0 +1,303 @@ +{ + "samples": [ + { + "x": 2.439382553100586, + "y": 5.571146488189697, + "heading": 3.149226247503918, + "angularVelocity": 3.3725230857726955e-25, + "velocityX": -1.0893794575213779e-23, + "velocityY": 2.8243441481859005e-24, + "timestamp": 0 + }, + { + "x": 2.4362890387153437, + "y": 5.581045201413149, + "heading": 3.136124834798283, + "angularVelocity": -0.2303700895130178, + "velocityX": -0.05439514057377693, + "velocityY": 0.17405508112644477, + "timestamp": 0.056871153426775845 + }, + { + "x": 2.4301904093335223, + "y": 5.600848905472547, + "heading": 3.1099250000899192, + "angularVelocity": -0.4606875916820856, + "velocityX": -0.1072359010561265, + "velocityY": 0.3482205453226071, + "timestamp": 0.11374230685355169 + }, + { + "x": 2.4211633685912326, + "y": 5.630611256474957, + "heading": 3.0708177004805495, + "angularVelocity": -0.6876473792591979, + "velocityX": -0.15872793496112414, + "velocityY": 0.5233294774077369, + "timestamp": 0.17061346028032753 + }, + { + "x": 2.409237703542417, + "y": 5.670443910842478, + "heading": 3.0192485456812395, + "angularVelocity": -0.9067717408915852, + "velocityX": -0.2096962050219477, + "velocityY": 0.7004017321154934, + "timestamp": 0.22748461370710338 + }, + { + "x": 2.394318792156192, + "y": 5.720524196272314, + "heading": 2.9560131473235427, + "angularVelocity": -1.1119063804308933, + "velocityX": -0.26232827166822764, + "velocityY": 0.8805920473253224, + "timestamp": 0.2843557671338792 + }, + { + "x": 2.3760338351749253, + "y": 5.781058983135284, + "heading": 2.8822982817207805, + "angularVelocity": -1.2961732119196991, + "velocityX": -0.3215154938752848, + "velocityY": 1.0644198897937152, + "timestamp": 0.34122692056065507 + }, + { + "x": 2.353569474795138, + "y": 5.852065184732581, + "heading": 2.7992155915208365, + "angularVelocity": -1.4608933561883304, + "velocityX": -0.3950044798847132, + "velocityY": 1.2485451290999514, + "timestamp": 0.3980980739874309 + }, + { + "x": 2.326002731310179, + "y": 5.933042032319499, + "heading": 2.706475620370505, + "angularVelocity": -1.630703187156881, + "velocityX": -0.4847227781383496, + "velocityY": 1.4238650477026025, + "timestamp": 0.45496922741420676 + }, + { + "x": 2.292962482296499, + "y": 6.0233120695479165, + "heading": 2.60247783021725, + "angularVelocity": -1.8286562499063208, + "velocityX": -0.5809667471615536, + "velocityY": 1.5872728402571346, + "timestamp": 0.5118403808409826 + }, + { + "x": 2.254554012450292, + "y": 6.122466608068637, + "heading": 2.485626955322059, + "angularVelocity": -2.0546598381487273, + "velocityX": -0.6753594314850648, + "velocityY": 1.743494417576513, + "timestamp": 0.5687115342677584 + }, + { + "x": 2.2110992611866678, + "y": 6.230363093640724, + "heading": 2.3547415371141285, + "angularVelocity": -2.3014377293481774, + "velocityX": -0.7640912597205168, + "velocityY": 1.897209377175871, + "timestamp": 0.6255826876945343 + }, + { + "x": 2.1630248930911256, + "y": 6.347033979020736, + "heading": 2.209048781749191, + "angularVelocity": -2.5618041236410543, + "velocityX": -0.845320785649975, + "velocityY": 2.051494973286085, + "timestamp": 0.6824538411213104 + }, + { + "x": 2.1171485756869948, + "y": 6.471321493889193, + "heading": 2.079206437440476, + "angularVelocity": -2.2830967280431977, + "velocityX": -0.8066711265703139, + "velocityY": 2.185422791336262, + "timestamp": 0.7393249945480862 + }, + { + "x": 2.07515399626246, + "y": 6.586581008184095, + "heading": 1.9645775517677246, + "angularVelocity": -2.015589253352165, + "velocityX": -0.7384161722446009, + "velocityY": 2.026677979079547, + "timestamp": 0.796196147974862 + }, + { + "x": 2.0367546596649357, + "y": 6.692666568692638, + "heading": 1.8650472976205432, + "angularVelocity": -1.7501008534200189, + "velocityX": -0.6751988360314355, + "velocityY": 1.865366782918037, + "timestamp": 0.8530673014016379 + }, + { + "x": 2.001724396449355, + "y": 6.78943559555366, + "heading": 1.7803812299137085, + "angularVelocity": -1.4887348436821468, + "velocityX": -0.6159583744100318, + "velocityY": 1.701548518540526, + "timestamp": 0.9099384548284137 + }, + { + "x": 1.9698936739721948, + "y": 6.876757760558234, + "heading": 1.7102873807073258, + "angularVelocity": -1.2325026833969792, + "velocityX": -0.5596989081317597, + "velocityY": 1.5354386141825944, + "timestamp": 0.9668096082551896 + }, + { + "x": 1.9411415756463717, + "y": 6.954522757040627, + "heading": 1.6544725733697885, + "angularVelocity": -0.9814256257243232, + "velocityX": -0.5055655915760988, + "velocityY": 1.3673891207872078, + "timestamp": 1.0236807616819654 + }, + { + "x": 1.9153842521448374, + "y": 7.022647857772365, + "heading": 1.612694477397962, + "angularVelocity": -0.7346096123339139, + "velocityX": -0.4529066486175296, + "velocityY": 1.1978849843348485, + "timestamp": 1.0805519151087413 + }, + { + "x": 1.8925619144079102, + "y": 7.0810849361144745, + "heading": 1.584807118963501, + "angularVelocity": -0.490360345343922, + "velocityX": -0.40129901297521586, + "velocityY": 1.0275346079862697, + "timestamp": 1.1374230685355171 + }, + { + "x": 1.8726278204084867, + "y": 7.1298262837468185, + "heading": 1.5707956385465986, + "angularVelocity": -0.24637236230741583, + "velocityX": -0.35051327075842725, + "velocityY": 0.8570486915673537, + "timestamp": 1.194294221962293 + }, + { + "x": 1.8555432558059692, + "y": 7.168908596038818, + "heading": 1.5707956385465986, + "angularVelocity": 1.2473082818664105e-25, + "velocityX": -0.30040826628414513, + "velocityY": 0.6872080120956212, + "timestamp": 1.2511653753890688 + }, + { + "x": 1.8541434053434975, + "y": 7.255697115566546, + "heading": 1.5707956385465986, + "angularVelocity": 2.030292510674732e-25, + "velocityX": -0.012095508597799284, + "velocityY": 0.7499024447828313, + "timestamp": 1.3668984552470251 + }, + { + "x": 1.8527435790342417, + "y": 7.3424856354838495, + "heading": 1.5707956385465986, + "angularVelocity": 2.0302925236125038e-25, + "velocityX": -0.012095299900201072, + "velocityY": 0.749902448148979, + "timestamp": 1.4826315351049815 + }, + { + "x": 1.851343752724996, + "y": 7.429274155401153, + "heading": 1.5707956385465986, + "angularVelocity": 2.0302925236123036e-25, + "velocityX": -0.012095299900113238, + "velocityY": 0.7499024481489804, + "timestamp": 1.5983646149629378 + }, + { + "x": 1.8499439264157502, + "y": 7.516062675318456, + "heading": 1.5707956385465986, + "angularVelocity": 2.030292523612118e-25, + "velocityX": -0.012095299900113238, + "velocityY": 0.7499024481489804, + "timestamp": 1.7140976948208941 + }, + { + "x": 1.8485441001065046, + "y": 7.602851195235759, + "heading": 1.5707956385465986, + "angularVelocity": 2.0302925236119252e-25, + "velocityX": -0.012095299900113238, + "velocityY": 0.7499024481489804, + "timestamp": 1.8298307746788505 + }, + { + "x": 1.847144273797259, + "y": 7.689639715153062, + "heading": 1.5707956385465986, + "angularVelocity": 2.0302925241560946e-25, + "velocityX": -0.012095299900113175, + "velocityY": 0.7499024481489763, + "timestamp": 1.9455638545368068 + }, + { + "x": 1.846291184425354, + "y": 7.742530822753906, + "heading": 1.5707956385465986, + "angularVelocity": 2.0313924965619656e-25, + "velocityX": -0.007371180071868358, + "velocityY": 0.4570094191372061, + "timestamp": 2.061296934394763 + }, + { + "x": 1.846291184425354, + "y": 7.742530822753906, + "heading": 1.5707956385465986, + "angularVelocity": 6.699868866352696e-26, + "velocityX": -2.082851323459477e-26, + "velocityY": -1.1898010382832076e-27, + "timestamp": 2.1770300142527192 + } + ], + "eventMarkers": [ + { + "timestamp": 0.5770300142527194, + "command": { + "type": "named", + "data": { + "name": "Arm to Amp" + } + } + }, + { + "timestamp": 1.6770300142527195, + "command": { + "type": "named", + "data": { + "name": "Warm Up Shooter Amp" + } + } + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Instant DNP.3.traj b/src/main/deploy/choreo/Instant DNP.3.traj new file mode 100644 index 0000000..2c54584 --- /dev/null +++ b/src/main/deploy/choreo/Instant DNP.3.traj @@ -0,0 +1,465 @@ +{ + "samples": [ + { + "x": 1.846291184425354, + "y": 7.742530822753906, + "heading": 1.5707956385465986, + "angularVelocity": 6.699868866352696e-26, + "velocityX": -2.082851323459477e-26, + "velocityY": -1.1898010382832076e-27, + "timestamp": 0 + }, + { + "x": 1.8514053614725579, + "y": 7.734353446812523, + "heading": 1.5741246181907276, + "angularVelocity": 0.0666072607638542, + "velocityX": 0.10232604599321918, + "velocityY": -0.16361548279587393, + "timestamp": 0.04997923058165288 + }, + { + "x": 1.8616343027242503, + "y": 7.717998461212048, + "heading": 1.5807797683258857, + "angularVelocity": 0.13315831511822673, + "velocityX": 0.2046638400121233, + "velocityY": -0.32723564188838467, + "timestamp": 0.09995846116330576 + }, + { + "x": 1.8769784699431493, + "y": 7.693465718807923, + "heading": 1.590759853081497, + "angularVelocity": 0.19968464178948309, + "velocityX": 0.3070108731231987, + "velocityY": -0.4908587450950237, + "timestamp": 0.14993769174495863 + }, + { + "x": 1.8974382624232278, + "y": 7.66075518377616, + "heading": 1.6040651512683497, + "angularVelocity": 0.26621654699376207, + "velocityX": 0.40936589543235347, + "velocityY": -0.6544825650791681, + "timestamp": 0.1999169223266115 + }, + { + "x": 1.9230141026421597, + "y": 7.61986696268551, + "heading": 1.6206973511905303, + "angularVelocity": 0.33278223231163817, + "velocityX": 0.5117293708062944, + "velocityY": -0.8181042528025491, + "timestamp": 0.2498961529082644 + }, + { + "x": 1.9537065533193043, + "y": 7.570801338700398, + "heading": 1.6406593358512553, + "angularVelocity": 0.3994056016551199, + "velocityX": 0.6141041052443037, + "velocityY": -0.9817202748840232, + "timestamp": 0.29987538348991727 + }, + { + "x": 1.9895164788532218, + "y": 7.513558803979521, + "heading": 1.6639547636507832, + "angularVelocity": 0.4661021694095368, + "velocityX": 0.7164961348377226, + "velocityY": -1.1453264497011013, + "timestamp": 0.34985461407157015 + }, + { + "x": 2.0304452716552572, + "y": 7.448140082586782, + "heading": 1.6905872964786974, + "angularVelocity": 0.5328720053903956, + "velocityX": 0.8189160242306792, + "velocityY": -1.3089181372222645, + "timestamp": 0.399833844653223 + }, + { + "x": 2.0764951786721593, + "y": 7.374546131696564, + "heading": 1.720559229351038, + "angularVelocity": 0.5996877607664348, + "velocityX": 0.9213808712334727, + "velocityY": -1.47249067330048, + "timestamp": 0.4498130752348759 + }, + { + "x": 2.127669795519365, + "y": 7.292778100747622, + "heading": 1.7538690746086205, + "angularVelocity": 0.666473750594517, + "velocityX": 1.0239176604289628, + "velocityY": -1.636040211050365, + "timestamp": 0.49979230581652834 + }, + { + "x": 2.1839748736231908, + "y": 7.202837211774282, + "heading": 1.7905071824942314, + "angularVelocity": 0.7330666650770846, + "velocityX": 1.1265695259521453, + "velocityY": -1.799565297957115, + "timestamp": 0.5497715363981812 + }, + { + "x": 2.2454198021164973, + "y": 7.104724485664905, + "heading": 1.8304471922431862, + "angularVelocity": 0.7991321451758543, + "velocityX": 1.2294092521676905, + "velocityY": -1.9630699586118285, + "timestamp": 0.5997507669798341 + }, + { + "x": 2.312020848816739, + "y": 6.998440133109207, + "heading": 1.8736268043801838, + "angularVelocity": 0.863951117983972, + "velocityX": 1.332574469937732, + "velocityY": -2.1265704037211566, + "timestamp": 0.649729997561487 + }, + { + "x": 2.383810472474797, + "y": 6.88398209646351, + "heading": 1.9198918718607836, + "angularVelocity": 0.9256858687533199, + "velocityX": 1.4363891324972011, + "velocityY": -2.2901120188055484, + "timestamp": 0.6997092281431398 + }, + { + "x": 2.4608821710178113, + "y": 6.761343181507853, + "heading": 1.9687196716778628, + "angularVelocity": 0.9769618149144491, + "velocityX": 1.5420745306812864, + "velocityY": -2.453797578081906, + "timestamp": 0.7496884587247927 + }, + { + "x": 2.5368643782269005, + "y": 6.6415841980546455, + "heading": 1.9971341230841648, + "angularVelocity": 0.5685251868749809, + "velocityX": 1.5202756490009244, + "velocityY": -2.3961750122894134, + "timestamp": 0.7996676893064456 + }, + { + "x": 2.6079459028648024, + "y": 6.529904796280597, + "heading": 2.020833851230746, + "angularVelocity": 0.47419153657962093, + "velocityX": 1.422221266927541, + "velocityY": -2.2345162275276365, + "timestamp": 0.8496469198880985 + }, + { + "x": 2.674025576949676, + "y": 6.426341391642361, + "heading": 2.0403388712032235, + "angularVelocity": 0.3902625099562424, + "velocityX": 1.3221426843880075, + "velocityY": -2.072128830975926, + "timestamp": 0.8996261504697514 + }, + { + "x": 2.7350695794350304, + "y": 6.3309037565632575, + "heading": 2.055822477797095, + "angularVelocity": 0.3098008195339312, + "velocityX": 1.2213873998245102, + "velocityY": -1.9095459047370478, + "timestamp": 0.9496053810514042 + }, + { + "x": 2.7910608455388015, + "y": 6.243596266012373, + "heading": 2.067371488792874, + "angularVelocity": 0.23107620628354833, + "velocityX": 1.120290677790572, + "velocityY": -1.7468754427551085, + "timestamp": 0.9995846116330571 + }, + { + "x": 2.8419891436748235, + "y": 6.164421466785129, + "heading": 2.075038431959136, + "angularVelocity": 0.15340258497450884, + "velocityX": 1.0189892389963568, + "velocityY": -1.584154023697775, + "timestamp": 1.04956384221471 + }, + { + "x": 2.8878478013227533, + "y": 6.093381162904928, + "heading": 2.0788588780268507, + "angularVelocity": 0.07644067392099715, + "velocityX": 0.9175542943385027, + "velocityY": -1.4213965091787517, + "timestamp": 1.0995430727963629 + }, + { + "x": 2.928632307052613, + "y": 6.03047685623169, + "heading": 2.0788588780268507, + "angularVelocity": -1.353091220301102e-27, + "velocityX": 0.8160290835856026, + "velocityY": -1.2586089449790203, + "timestamp": 1.1495223033780158 + }, + { + "x": 2.98215373218464, + "y": 5.969221498684807, + "heading": 2.0788588780268507, + "angularVelocity": -9.036957956814612e-28, + "velocityX": 0.9869522884399109, + "velocityY": -1.1295684888989306, + "timestamp": 1.2037512927211877 + }, + { + "x": 3.0392348647762897, + "y": 5.9112688793985475, + "heading": 2.0788588780268507, + "angularVelocity": -9.024177769261626e-28, + "velocityX": 1.052594438565491, + "velocityY": -1.0686649334274896, + "timestamp": 1.2579802820643597 + }, + { + "x": 3.096316073363738, + "y": 5.853316334965364, + "heading": 2.0788588780268507, + "angularVelocity": -9.024178327381887e-28, + "velocityX": 1.0525958399524507, + "velocityY": -1.0686635531126647, + "timestamp": 1.3122092714075317 + }, + { + "x": 3.153397281952565, + "y": 5.795363790533539, + "heading": 2.0788588780268507, + "angularVelocity": -9.024178327298733e-28, + "velocityX": 1.052595839977865, + "velocityY": -1.0686635530876325, + "timestamp": 1.3664382607507037 + }, + { + "x": 3.2104784905413917, + "y": 5.737411246101713, + "heading": 2.0788588780268507, + "angularVelocity": -9.024178314475913e-28, + "velocityX": 1.0525958399778652, + "velocityY": -1.0686635530876323, + "timestamp": 1.4206672500938757 + }, + { + "x": 3.267559699130218, + "y": 5.679458701669888, + "heading": 2.0788588780268507, + "angularVelocity": -9.02417832729875e-28, + "velocityX": 1.0525958399778652, + "velocityY": -1.0686635530876323, + "timestamp": 1.4748962394370477 + }, + { + "x": 3.324640907719045, + "y": 5.621506157238062, + "heading": 2.0788588780268507, + "angularVelocity": -9.024178340121581e-28, + "velocityX": 1.0525958399778652, + "velocityY": -1.0686635530876323, + "timestamp": 1.5291252287802197 + }, + { + "x": 3.3817221163078717, + "y": 5.563553612806236, + "heading": 2.0788588780268507, + "angularVelocity": -9.024178352944405e-28, + "velocityX": 1.0525958399778652, + "velocityY": -1.0686635530876323, + "timestamp": 1.5833542181233917 + }, + { + "x": 3.4388033248966985, + "y": 5.5056010683744105, + "heading": 2.0788588780268507, + "angularVelocity": -9.02417836576725e-28, + "velocityX": 1.0525958399778652, + "velocityY": -1.0686635530876323, + "timestamp": 1.6375832074665637 + }, + { + "x": 3.495884533485525, + "y": 5.4476485239425845, + "heading": 2.0788588780268507, + "angularVelocity": -9.024178378590079e-28, + "velocityX": 1.0525958399778652, + "velocityY": -1.0686635530876323, + "timestamp": 1.6918121968097357 + }, + { + "x": 3.5529657420743517, + "y": 5.389695979510759, + "heading": 2.0788588780268507, + "angularVelocity": -9.024178391412911e-28, + "velocityX": 1.0525958399778652, + "velocityY": -1.0686635530876323, + "timestamp": 1.7460411861529077 + }, + { + "x": 3.6100469506631785, + "y": 5.331743435078933, + "heading": 2.0788588780268507, + "angularVelocity": -9.024178404235742e-28, + "velocityX": 1.0525958399778652, + "velocityY": -1.0686635530876323, + "timestamp": 1.8002701754960797 + }, + { + "x": 3.6671281592520053, + "y": 5.273790890647108, + "heading": 2.0788588780268507, + "angularVelocity": -9.024178417058576e-28, + "velocityX": 1.0525958399778652, + "velocityY": -1.0686635530876323, + "timestamp": 1.8544991648392517 + }, + { + "x": 3.724209367840832, + "y": 5.215838346215282, + "heading": 2.0788588780268507, + "angularVelocity": -9.024178429881409e-28, + "velocityX": 1.0525958399778652, + "velocityY": -1.0686635530876323, + "timestamp": 1.9087281541824237 + }, + { + "x": 3.7812905764296585, + "y": 5.157885801783457, + "heading": 2.0788588780268507, + "angularVelocity": -9.024178442704241e-28, + "velocityX": 1.0525958399778652, + "velocityY": -1.0686635530876323, + "timestamp": 1.9629571435255957 + }, + { + "x": 3.8383717850184853, + "y": 5.099933257351631, + "heading": 2.0788588780268507, + "angularVelocity": -9.024178455527075e-28, + "velocityX": 1.052595839977865, + "velocityY": -1.0686635530876316, + "timestamp": 2.0171861328687677 + }, + { + "x": 3.8954217579928927, + "y": 5.04201242534106, + "heading": 2.0788588780268507, + "angularVelocity": -9.024178455214888e-28, + "velocityX": 1.0520198452046383, + "velocityY": -1.0680787658430528, + "timestamp": 2.0714151222119397 + }, + { + "x": 3.9443217414721357, + "y": 4.992365991157971, + "heading": 2.0788588780268507, + "angularVelocity": -9.024178468659725e-28, + "velocityX": 0.9017314184078514, + "velocityY": -0.9154962093967133, + "timestamp": 2.1256441115551117 + }, + { + "x": 3.985071730179638, + "y": 4.950993960159487, + "heading": 2.0788588780268507, + "angularVelocity": -9.02417846230043e-28, + "velocityX": 0.751442894309324, + "velocityY": -0.7629135541633372, + "timestamp": 2.1798731008982837 + }, + { + "x": 4.017671722356639, + "y": 4.917896334131213, + "heading": 2.0788588780268507, + "angularVelocity": -1.5394220322890216e-27, + "velocityX": 0.6011543377786844, + "velocityY": -0.6103308660027776, + "timestamp": 2.2341020902414557 + }, + { + "x": 4.042121717123779, + "y": 4.893073113965937, + "heading": 2.0788588780268507, + "angularVelocity": -2.3472970316100667e-28, + "velocityX": 0.4508657650323279, + "velocityY": -0.45774816137897073, + "timestamp": 2.2883310795846277 + }, + { + "x": 4.058421713953444, + "y": 4.876524300199322, + "heading": 2.0788588780268507, + "angularVelocity": -9.024178457605914e-28, + "velocityX": 0.3005771825566498, + "velocityY": -0.3051654468773257, + "timestamp": 2.3425600689277997 + }, + { + "x": 4.0665717124938965, + "y": 4.868249893188477, + "heading": 2.0788588780268507, + "angularVelocity": -9.024178457563788e-28, + "velocityX": 0.15028859359480273, + "velocityY": -0.15258272579050136, + "timestamp": 2.3967890582709717 + }, + { + "x": 4.0665717124938965, + "y": 4.868249893188477, + "heading": 2.0788588780268507, + "angularVelocity": -2.9569195590907886e-28, + "velocityX": -5.9543467827551576e-27, + "velocityY": 8.214243587658572e-26, + "timestamp": 2.4510180476141437 + } + ], + "eventMarkers": [ + { + "timestamp": 1.7010180476141437, + "command": { + "type": "named", + "data": { + "name": "Climb" + } + } + }, + { + "timestamp": 1.1495223033780158, + "command": { + "type": "named", + "data": { + "name": "" + } + } + }, + { + "timestamp": 0.10000000000000009, + "command": { + "type": "named", + "data": { + "name": "Arm to Pickup" + } + } + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Instant DNP.traj b/src/main/deploy/choreo/Instant DNP.traj new file mode 100644 index 0000000..a4b5cfe --- /dev/null +++ b/src/main/deploy/choreo/Instant DNP.traj @@ -0,0 +1,861 @@ +{ + "samples": [ + { + "x": 1.3660341501235962, + "y": 5.548494815826416, + "heading": 3.1415, + "angularVelocity": 5.981539907314682e-29, + "velocityX": -4.995526911759336e-28, + "velocityY": 5.315254844280788e-26, + "timestamp": 0 + }, + { + "x": 1.4238967756143568, + "y": 5.549715933993696, + "heading": 3.1415000001333797, + "angularVelocity": 1.1018016721845479e-9, + "velocityX": 0.4779833769031679, + "velocityY": 0.010087239910803398, + "timestamp": 0.12105572763983921 + }, + { + "x": 1.5396220212494325, + "y": 5.552158170215426, + "heading": 3.141500000477872, + "angularVelocity": 2.8457359966718716e-9, + "velocityX": 0.9559667096411775, + "velocityY": 0.020174478889556504, + "timestamp": 0.24211145527967842 + }, + { + "x": 1.6606507980369853, + "y": 5.554712330812969, + "heading": 3.1423272347299114, + "angularVelocity": 0.0068334994813339075, + "velocityX": 0.9997773682186555, + "velocityY": 0.021099047912405276, + "timestamp": 0.36316718291951766 + }, + { + "x": 1.7816795748245382, + "y": 5.557266491410513, + "heading": 3.143707917770431, + "angularVelocity": 0.01140535080361435, + "velocityX": 0.9997773682186563, + "velocityY": 0.021099047912405293, + "timestamp": 0.48422291055935685 + }, + { + "x": 1.902708351612091, + "y": 5.559820652008057, + "heading": 3.145363123736773, + "angularVelocity": 0.01367309088643057, + "velocityX": 0.9997773682186563, + "velocityY": 0.02109904791240529, + "timestamp": 0.605278638199196 + }, + { + "x": 2.023737128399644, + "y": 5.5623748126056, + "heading": 3.147018329695959, + "angularVelocity": 0.013673090827311721, + "velocityX": 0.9997773682186563, + "velocityY": 0.02109904791240529, + "timestamp": 0.7263343658390352 + }, + { + "x": 2.144765905187197, + "y": 5.564928973203144, + "heading": 3.148399012770666, + "angularVelocity": 0.011405351086027542, + "velocityX": 0.9997773682186563, + "velocityY": 0.02109904791240529, + "timestamp": 0.8473900934788744 + }, + { + "x": 2.2657946819747496, + "y": 5.567483133800687, + "heading": 3.149226247026046, + "angularVelocity": 0.006833499508931699, + "velocityX": 0.9997773682186555, + "velocityY": 0.021099047912405276, + "timestamp": 0.9684458211187136 + }, + { + "x": 2.3815199276098253, + "y": 5.569925370022418, + "heading": 3.1492262473705384, + "angularVelocity": 2.845735928585778e-9, + "velocityX": 0.9559667096411774, + "velocityY": 0.020174478889556504, + "timestamp": 1.0895015487585529 + }, + { + "x": 2.439382553100586, + "y": 5.571146488189697, + "heading": 3.149226247503918, + "angularVelocity": 1.1018015962710774e-9, + "velocityX": 0.47798337690316783, + "velocityY": 0.0100872399108034, + "timestamp": 1.210557276398392 + }, + { + "x": 2.439382553100586, + "y": 5.571146488189697, + "heading": 3.149226247503918, + "angularVelocity": 3.3725230857726955e-25, + "velocityX": -1.0893794575213779e-23, + "velocityY": 2.8243441481859005e-24, + "timestamp": 1.3316130040382312 + }, + { + "x": 2.4362890387153437, + "y": 5.581045201413149, + "heading": 3.136124834798283, + "angularVelocity": -0.2303700895130178, + "velocityX": -0.05439514057377693, + "velocityY": 0.17405508112644477, + "timestamp": 1.388484157465007 + }, + { + "x": 2.4301904093335223, + "y": 5.600848905472547, + "heading": 3.1099250000899192, + "angularVelocity": -0.4606875916820856, + "velocityX": -0.1072359010561265, + "velocityY": 0.3482205453226071, + "timestamp": 1.445355310891783 + }, + { + "x": 2.4211633685912326, + "y": 5.630611256474957, + "heading": 3.0708177004805495, + "angularVelocity": -0.6876473792591979, + "velocityX": -0.15872793496112414, + "velocityY": 0.5233294774077369, + "timestamp": 1.5022264643185588 + }, + { + "x": 2.409237703542417, + "y": 5.670443910842478, + "heading": 3.0192485456812395, + "angularVelocity": -0.9067717408915852, + "velocityX": -0.2096962050219477, + "velocityY": 0.7004017321154934, + "timestamp": 1.5590976177453346 + }, + { + "x": 2.394318792156192, + "y": 5.720524196272314, + "heading": 2.9560131473235427, + "angularVelocity": -1.1119063804308933, + "velocityX": -0.26232827166822764, + "velocityY": 0.8805920473253224, + "timestamp": 1.6159687711721105 + }, + { + "x": 2.3760338351749253, + "y": 5.781058983135284, + "heading": 2.8822982817207805, + "angularVelocity": -1.2961732119196991, + "velocityX": -0.3215154938752848, + "velocityY": 1.0644198897937152, + "timestamp": 1.6728399245988863 + }, + { + "x": 2.353569474795138, + "y": 5.852065184732581, + "heading": 2.7992155915208365, + "angularVelocity": -1.4608933561883304, + "velocityX": -0.3950044798847132, + "velocityY": 1.2485451290999514, + "timestamp": 1.7297110780256622 + }, + { + "x": 2.326002731310179, + "y": 5.933042032319499, + "heading": 2.706475620370505, + "angularVelocity": -1.630703187156881, + "velocityX": -0.4847227781383496, + "velocityY": 1.4238650477026025, + "timestamp": 1.786582231452438 + }, + { + "x": 2.292962482296499, + "y": 6.0233120695479165, + "heading": 2.60247783021725, + "angularVelocity": -1.8286562499063208, + "velocityX": -0.5809667471615536, + "velocityY": 1.5872728402571346, + "timestamp": 1.8434533848792138 + }, + { + "x": 2.254554012450292, + "y": 6.122466608068637, + "heading": 2.485626955322059, + "angularVelocity": -2.0546598381487273, + "velocityX": -0.6753594314850648, + "velocityY": 1.743494417576513, + "timestamp": 1.9003245383059897 + }, + { + "x": 2.2110992611866678, + "y": 6.230363093640724, + "heading": 2.3547415371141285, + "angularVelocity": -2.3014377293481774, + "velocityX": -0.7640912597205168, + "velocityY": 1.897209377175871, + "timestamp": 1.9571956917327655 + }, + { + "x": 2.1630248930911256, + "y": 6.347033979020736, + "heading": 2.209048781749191, + "angularVelocity": -2.5618041236410543, + "velocityX": -0.845320785649975, + "velocityY": 2.051494973286085, + "timestamp": 2.0140668451595416 + }, + { + "x": 2.1171485756869948, + "y": 6.471321493889193, + "heading": 2.079206437440476, + "angularVelocity": -2.2830967280431977, + "velocityX": -0.8066711265703139, + "velocityY": 2.185422791336262, + "timestamp": 2.0709379985863174 + }, + { + "x": 2.07515399626246, + "y": 6.586581008184095, + "heading": 1.9645775517677246, + "angularVelocity": -2.015589253352165, + "velocityX": -0.7384161722446009, + "velocityY": 2.026677979079547, + "timestamp": 2.1278091520130933 + }, + { + "x": 2.0367546596649357, + "y": 6.692666568692638, + "heading": 1.8650472976205432, + "angularVelocity": -1.7501008534200189, + "velocityX": -0.6751988360314355, + "velocityY": 1.865366782918037, + "timestamp": 2.184680305439869 + }, + { + "x": 2.001724396449355, + "y": 6.78943559555366, + "heading": 1.7803812299137085, + "angularVelocity": -1.4887348436821468, + "velocityX": -0.6159583744100318, + "velocityY": 1.701548518540526, + "timestamp": 2.241551458866645 + }, + { + "x": 1.9698936739721948, + "y": 6.876757760558234, + "heading": 1.7102873807073258, + "angularVelocity": -1.2325026833969792, + "velocityX": -0.5596989081317597, + "velocityY": 1.5354386141825944, + "timestamp": 2.298422612293421 + }, + { + "x": 1.9411415756463717, + "y": 6.954522757040627, + "heading": 1.6544725733697885, + "angularVelocity": -0.9814256257243232, + "velocityX": -0.5055655915760988, + "velocityY": 1.3673891207872078, + "timestamp": 2.3552937657201967 + }, + { + "x": 1.9153842521448374, + "y": 7.022647857772365, + "heading": 1.612694477397962, + "angularVelocity": -0.7346096123339139, + "velocityX": -0.4529066486175296, + "velocityY": 1.1978849843348485, + "timestamp": 2.4121649191469725 + }, + { + "x": 1.8925619144079102, + "y": 7.0810849361144745, + "heading": 1.584807118963501, + "angularVelocity": -0.490360345343922, + "velocityX": -0.40129901297521586, + "velocityY": 1.0275346079862697, + "timestamp": 2.4690360725737484 + }, + { + "x": 1.8726278204084867, + "y": 7.1298262837468185, + "heading": 1.5707956385465986, + "angularVelocity": -0.24637236230741583, + "velocityX": -0.35051327075842725, + "velocityY": 0.8570486915673537, + "timestamp": 2.525907226000524 + }, + { + "x": 1.8555432558059692, + "y": 7.168908596038818, + "heading": 1.5707956385465986, + "angularVelocity": 1.2473082818664105e-25, + "velocityX": -0.30040826628414513, + "velocityY": 0.6872080120956212, + "timestamp": 2.5827783794273 + }, + { + "x": 1.8541434053434975, + "y": 7.255697115566546, + "heading": 1.5707956385465986, + "angularVelocity": 2.030292510674732e-25, + "velocityX": -0.012095508597799284, + "velocityY": 0.7499024447828313, + "timestamp": 2.6985114592852564 + }, + { + "x": 1.8527435790342417, + "y": 7.3424856354838495, + "heading": 1.5707956385465986, + "angularVelocity": 2.0302925236125038e-25, + "velocityX": -0.012095299900201072, + "velocityY": 0.749902448148979, + "timestamp": 2.8142445391432127 + }, + { + "x": 1.851343752724996, + "y": 7.429274155401153, + "heading": 1.5707956385465986, + "angularVelocity": 2.0302925236123036e-25, + "velocityX": -0.012095299900113238, + "velocityY": 0.7499024481489804, + "timestamp": 2.929977619001169 + }, + { + "x": 1.8499439264157502, + "y": 7.516062675318456, + "heading": 1.5707956385465986, + "angularVelocity": 2.030292523612118e-25, + "velocityX": -0.012095299900113238, + "velocityY": 0.7499024481489804, + "timestamp": 3.0457106988591254 + }, + { + "x": 1.8485441001065046, + "y": 7.602851195235759, + "heading": 1.5707956385465986, + "angularVelocity": 2.0302925236119252e-25, + "velocityX": -0.012095299900113238, + "velocityY": 0.7499024481489804, + "timestamp": 3.1614437787170817 + }, + { + "x": 1.847144273797259, + "y": 7.689639715153062, + "heading": 1.5707956385465986, + "angularVelocity": 2.0302925241560946e-25, + "velocityX": -0.012095299900113175, + "velocityY": 0.7499024481489763, + "timestamp": 3.277176858575038 + }, + { + "x": 1.846291184425354, + "y": 7.742530822753906, + "heading": 1.5707956385465986, + "angularVelocity": 2.0313924965619656e-25, + "velocityX": -0.007371180071868358, + "velocityY": 0.4570094191372061, + "timestamp": 3.3929099384329944 + }, + { + "x": 1.846291184425354, + "y": 7.742530822753906, + "heading": 1.5707956385465986, + "angularVelocity": 6.699868866352696e-26, + "velocityX": -2.082851323459477e-26, + "velocityY": -1.1898010382832076e-27, + "timestamp": 3.5086430182909507 + }, + { + "x": 1.8514053614725579, + "y": 7.734353446812523, + "heading": 1.5741246181907276, + "angularVelocity": 0.0666072607638542, + "velocityX": 0.10232604599321918, + "velocityY": -0.16361548279587393, + "timestamp": 3.5586222488726036 + }, + { + "x": 1.8616343027242503, + "y": 7.717998461212048, + "heading": 1.5807797683258857, + "angularVelocity": 0.13315831511822673, + "velocityX": 0.2046638400121233, + "velocityY": -0.32723564188838467, + "timestamp": 3.6086014794542565 + }, + { + "x": 1.8769784699431493, + "y": 7.693465718807923, + "heading": 1.590759853081497, + "angularVelocity": 0.19968464178948309, + "velocityX": 0.3070108731231987, + "velocityY": -0.4908587450950237, + "timestamp": 3.6585807100359093 + }, + { + "x": 1.8974382624232278, + "y": 7.66075518377616, + "heading": 1.6040651512683497, + "angularVelocity": 0.26621654699376207, + "velocityX": 0.40936589543235347, + "velocityY": -0.6544825650791681, + "timestamp": 3.7085599406175622 + }, + { + "x": 1.9230141026421597, + "y": 7.61986696268551, + "heading": 1.6206973511905303, + "angularVelocity": 0.33278223231163817, + "velocityX": 0.5117293708062944, + "velocityY": -0.8181042528025491, + "timestamp": 3.758539171199215 + }, + { + "x": 1.9537065533193043, + "y": 7.570801338700398, + "heading": 1.6406593358512553, + "angularVelocity": 0.3994056016551199, + "velocityX": 0.6141041052443037, + "velocityY": -0.9817202748840232, + "timestamp": 3.808518401780868 + }, + { + "x": 1.9895164788532218, + "y": 7.513558803979521, + "heading": 1.6639547636507832, + "angularVelocity": 0.4661021694095368, + "velocityX": 0.7164961348377226, + "velocityY": -1.1453264497011013, + "timestamp": 3.858497632362521 + }, + { + "x": 2.0304452716552572, + "y": 7.448140082586782, + "heading": 1.6905872964786974, + "angularVelocity": 0.5328720053903956, + "velocityX": 0.8189160242306792, + "velocityY": -1.3089181372222645, + "timestamp": 3.9084768629441737 + }, + { + "x": 2.0764951786721593, + "y": 7.374546131696564, + "heading": 1.720559229351038, + "angularVelocity": 0.5996877607664348, + "velocityX": 0.9213808712334727, + "velocityY": -1.47249067330048, + "timestamp": 3.9584560935258266 + }, + { + "x": 2.127669795519365, + "y": 7.292778100747622, + "heading": 1.7538690746086205, + "angularVelocity": 0.666473750594517, + "velocityX": 1.0239176604289628, + "velocityY": -1.636040211050365, + "timestamp": 4.008435324107479 + }, + { + "x": 2.1839748736231908, + "y": 7.202837211774282, + "heading": 1.7905071824942314, + "angularVelocity": 0.7330666650770846, + "velocityX": 1.1265695259521453, + "velocityY": -1.799565297957115, + "timestamp": 4.058414554689132 + }, + { + "x": 2.2454198021164973, + "y": 7.104724485664905, + "heading": 1.8304471922431862, + "angularVelocity": 0.7991321451758543, + "velocityX": 1.2294092521676905, + "velocityY": -1.9630699586118285, + "timestamp": 4.108393785270785 + }, + { + "x": 2.312020848816739, + "y": 6.998440133109207, + "heading": 1.8736268043801838, + "angularVelocity": 0.863951117983972, + "velocityX": 1.332574469937732, + "velocityY": -2.1265704037211566, + "timestamp": 4.158373015852438 + }, + { + "x": 2.383810472474797, + "y": 6.88398209646351, + "heading": 1.9198918718607836, + "angularVelocity": 0.9256858687533199, + "velocityX": 1.4363891324972011, + "velocityY": -2.2901120188055484, + "timestamp": 4.208352246434091 + }, + { + "x": 2.4608821710178113, + "y": 6.761343181507853, + "heading": 1.9687196716778628, + "angularVelocity": 0.9769618149144491, + "velocityX": 1.5420745306812864, + "velocityY": -2.453797578081906, + "timestamp": 4.258331477015743 + }, + { + "x": 2.5368643782269005, + "y": 6.6415841980546455, + "heading": 1.9971341230841648, + "angularVelocity": 0.5685251868749809, + "velocityX": 1.5202756490009244, + "velocityY": -2.3961750122894134, + "timestamp": 4.308310707597396 + }, + { + "x": 2.6079459028648024, + "y": 6.529904796280597, + "heading": 2.020833851230746, + "angularVelocity": 0.47419153657962093, + "velocityX": 1.422221266927541, + "velocityY": -2.2345162275276365, + "timestamp": 4.358289938179049 + }, + { + "x": 2.674025576949676, + "y": 6.426341391642361, + "heading": 2.0403388712032235, + "angularVelocity": 0.3902625099562424, + "velocityX": 1.3221426843880075, + "velocityY": -2.072128830975926, + "timestamp": 4.408269168760702 + }, + { + "x": 2.7350695794350304, + "y": 6.3309037565632575, + "heading": 2.055822477797095, + "angularVelocity": 0.3098008195339312, + "velocityX": 1.2213873998245102, + "velocityY": -1.9095459047370478, + "timestamp": 4.458248399342355 + }, + { + "x": 2.7910608455388015, + "y": 6.243596266012373, + "heading": 2.067371488792874, + "angularVelocity": 0.23107620628354833, + "velocityX": 1.120290677790572, + "velocityY": -1.7468754427551085, + "timestamp": 4.508227629924008 + }, + { + "x": 2.8419891436748235, + "y": 6.164421466785129, + "heading": 2.075038431959136, + "angularVelocity": 0.15340258497450884, + "velocityX": 1.0189892389963568, + "velocityY": -1.584154023697775, + "timestamp": 4.558206860505661 + }, + { + "x": 2.8878478013227533, + "y": 6.093381162904928, + "heading": 2.0788588780268507, + "angularVelocity": 0.07644067392099715, + "velocityX": 0.9175542943385027, + "velocityY": -1.4213965091787517, + "timestamp": 4.608186091087314 + }, + { + "x": 2.928632307052613, + "y": 6.03047685623169, + "heading": 2.0788588780268507, + "angularVelocity": -1.353091220301102e-27, + "velocityX": 0.8160290835856026, + "velocityY": -1.2586089449790203, + "timestamp": 4.6581653216689665 + }, + { + "x": 2.98215373218464, + "y": 5.969221498684807, + "heading": 2.0788588780268507, + "angularVelocity": -9.036957956814612e-28, + "velocityX": 0.9869522884399109, + "velocityY": -1.1295684888989306, + "timestamp": 4.7123943110121385 + }, + { + "x": 3.0392348647762897, + "y": 5.9112688793985475, + "heading": 2.0788588780268507, + "angularVelocity": -9.024177769261626e-28, + "velocityX": 1.052594438565491, + "velocityY": -1.0686649334274896, + "timestamp": 4.7666233003553105 + }, + { + "x": 3.096316073363738, + "y": 5.853316334965364, + "heading": 2.0788588780268507, + "angularVelocity": -9.024178327381887e-28, + "velocityX": 1.0525958399524507, + "velocityY": -1.0686635531126647, + "timestamp": 4.8208522896984825 + }, + { + "x": 3.153397281952565, + "y": 5.795363790533539, + "heading": 2.0788588780268507, + "angularVelocity": -9.024178327298733e-28, + "velocityX": 1.052595839977865, + "velocityY": -1.0686635530876325, + "timestamp": 4.8750812790416544 + }, + { + "x": 3.2104784905413917, + "y": 5.737411246101713, + "heading": 2.0788588780268507, + "angularVelocity": -9.024178314475913e-28, + "velocityX": 1.0525958399778652, + "velocityY": -1.0686635530876323, + "timestamp": 4.929310268384826 + }, + { + "x": 3.267559699130218, + "y": 5.679458701669888, + "heading": 2.0788588780268507, + "angularVelocity": -9.02417832729875e-28, + "velocityX": 1.0525958399778652, + "velocityY": -1.0686635530876323, + "timestamp": 4.983539257727998 + }, + { + "x": 3.324640907719045, + "y": 5.621506157238062, + "heading": 2.0788588780268507, + "angularVelocity": -9.024178340121581e-28, + "velocityX": 1.0525958399778652, + "velocityY": -1.0686635530876323, + "timestamp": 5.03776824707117 + }, + { + "x": 3.3817221163078717, + "y": 5.563553612806236, + "heading": 2.0788588780268507, + "angularVelocity": -9.024178352944405e-28, + "velocityX": 1.0525958399778652, + "velocityY": -1.0686635530876323, + "timestamp": 5.091997236414342 + }, + { + "x": 3.4388033248966985, + "y": 5.5056010683744105, + "heading": 2.0788588780268507, + "angularVelocity": -9.02417836576725e-28, + "velocityX": 1.0525958399778652, + "velocityY": -1.0686635530876323, + "timestamp": 5.146226225757514 + }, + { + "x": 3.495884533485525, + "y": 5.4476485239425845, + "heading": 2.0788588780268507, + "angularVelocity": -9.024178378590079e-28, + "velocityX": 1.0525958399778652, + "velocityY": -1.0686635530876323, + "timestamp": 5.200455215100686 + }, + { + "x": 3.5529657420743517, + "y": 5.389695979510759, + "heading": 2.0788588780268507, + "angularVelocity": -9.024178391412911e-28, + "velocityX": 1.0525958399778652, + "velocityY": -1.0686635530876323, + "timestamp": 5.254684204443858 + }, + { + "x": 3.6100469506631785, + "y": 5.331743435078933, + "heading": 2.0788588780268507, + "angularVelocity": -9.024178404235742e-28, + "velocityX": 1.0525958399778652, + "velocityY": -1.0686635530876323, + "timestamp": 5.30891319378703 + }, + { + "x": 3.6671281592520053, + "y": 5.273790890647108, + "heading": 2.0788588780268507, + "angularVelocity": -9.024178417058576e-28, + "velocityX": 1.0525958399778652, + "velocityY": -1.0686635530876323, + "timestamp": 5.363142183130202 + }, + { + "x": 3.724209367840832, + "y": 5.215838346215282, + "heading": 2.0788588780268507, + "angularVelocity": -9.024178429881409e-28, + "velocityX": 1.0525958399778652, + "velocityY": -1.0686635530876323, + "timestamp": 5.417371172473374 + }, + { + "x": 3.7812905764296585, + "y": 5.157885801783457, + "heading": 2.0788588780268507, + "angularVelocity": -9.024178442704241e-28, + "velocityX": 1.0525958399778652, + "velocityY": -1.0686635530876323, + "timestamp": 5.471600161816546 + }, + { + "x": 3.8383717850184853, + "y": 5.099933257351631, + "heading": 2.0788588780268507, + "angularVelocity": -9.024178455527075e-28, + "velocityX": 1.052595839977865, + "velocityY": -1.0686635530876316, + "timestamp": 5.525829151159718 + }, + { + "x": 3.8954217579928927, + "y": 5.04201242534106, + "heading": 2.0788588780268507, + "angularVelocity": -9.024178455214888e-28, + "velocityX": 1.0520198452046383, + "velocityY": -1.0680787658430528, + "timestamp": 5.58005814050289 + }, + { + "x": 3.9443217414721357, + "y": 4.992365991157971, + "heading": 2.0788588780268507, + "angularVelocity": -9.024178468659725e-28, + "velocityX": 0.9017314184078514, + "velocityY": -0.9154962093967133, + "timestamp": 5.634287129846062 + }, + { + "x": 3.985071730179638, + "y": 4.950993960159487, + "heading": 2.0788588780268507, + "angularVelocity": -9.02417846230043e-28, + "velocityX": 0.751442894309324, + "velocityY": -0.7629135541633372, + "timestamp": 5.688516119189234 + }, + { + "x": 4.017671722356639, + "y": 4.917896334131213, + "heading": 2.0788588780268507, + "angularVelocity": -1.5394220322890216e-27, + "velocityX": 0.6011543377786844, + "velocityY": -0.6103308660027776, + "timestamp": 5.742745108532406 + }, + { + "x": 4.042121717123779, + "y": 4.893073113965937, + "heading": 2.0788588780268507, + "angularVelocity": -2.3472970316100667e-28, + "velocityX": 0.4508657650323279, + "velocityY": -0.45774816137897073, + "timestamp": 5.796974097875578 + }, + { + "x": 4.058421713953444, + "y": 4.876524300199322, + "heading": 2.0788588780268507, + "angularVelocity": -9.024178457605914e-28, + "velocityX": 0.3005771825566498, + "velocityY": -0.3051654468773257, + "timestamp": 5.85120308721875 + }, + { + "x": 4.0665717124938965, + "y": 4.868249893188477, + "heading": 2.0788588780268507, + "angularVelocity": -9.024178457563788e-28, + "velocityX": 0.15028859359480273, + "velocityY": -0.15258272579050136, + "timestamp": 5.905432076561922 + }, + { + "x": 4.0665717124938965, + "y": 4.868249893188477, + "heading": 2.0788588780268507, + "angularVelocity": -2.9569195590907886e-28, + "velocityX": -5.9543467827551576e-27, + "velocityY": 8.214243587658572e-26, + "timestamp": 5.959661065905094 + } + ], + "eventMarkers": [ + { + "timestamp": 0.5816130040382312, + "command": { + "type": "named", + "data": { + "name": "Start Intake" + } + } + }, + { + "timestamp": 1.9086430182909506, + "command": { + "type": "named", + "data": { + "name": "Arm to Amp" + } + } + }, + { + "timestamp": 5.209661065905094, + "command": { + "type": "named", + "data": { + "name": "Climb" + } + } + }, + { + "timestamp": 3.0086430182909507, + "command": { + "type": "named", + "data": { + "name": "Warm Up Shooter Amp" + } + } + }, + { + "timestamp": 4.6581653216689665, + "command": { + "type": "named", + "data": { + "name": "" + } + } + }, + { + "timestamp": 3.608643018290951, + "command": { + "type": "named", + "data": { + "name": "Arm to Pickup" + } + } + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Instant DNP.auto b/src/main/deploy/pathplanner/autos/Instant DNP.auto new file mode 100644 index 0000000..58e9c0e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Instant DNP.auto @@ -0,0 +1,87 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.3660341501235962, + "y": 5.548494815826416 + }, + "rotation": 179.99469134034814 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Arm to Subwoofer" + } + }, + { + "type": "named", + "data": { + "name": "Warm Up Shooter" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Instant DNP.1" + } + }, + { + "type": "named", + "data": { + "name": "Arm to Pickup" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Intake Until Beam Break" + } + }, + { + "type": "path", + "data": { + "pathName": "Instant DNP.2" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "Instant DNP.3" + } + } + ] + } + }, + "folder": "Choreo Autos", + "choreoAuto": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3d8f7a0..701f367 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -157,6 +157,11 @@ public RobotContainer() { NamedCommands.registerCommand( "Arm to Behind W2", arm.autoMoveToPosition(ArmConstants.behindWing2Angle).withTimeout(3.0).asProxy()); + NamedCommands.registerCommand( + "Arm to Amp", arm.autoMoveToPosition(ArmConstants.ampAngle).withTimeout(3.0).asProxy()); + + NamedCommands.registerCommand( + "Climb", climber.run(climber::autoClimb).withTimeout(3.0).asProxy()); NamedCommands.registerCommand( "Auto Shoot", @@ -187,6 +192,9 @@ public RobotContainer() { NamedCommands.registerCommand( "Warm Up Shooter Idle", shooter.run(() -> shooter.setShooterState(ShooterConstants.idleState)).asProxy()); + NamedCommands.registerCommand( + "Warm Up Shooter Amp", + shooter.run(() -> shooter.setShooterState(ShooterConstants.ampState)).asProxy()); NamedCommands.registerCommand( "Shoot", intake diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index 31b75b8..ebe6980 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -498,6 +498,9 @@ public void periodic() { SmartDashboard.putNumber( "Arm/Velocity Error", Units.radiansToDegrees(previousSetpoint.velocity - getVelocity())); + SmartDashboard.putNumber("Arm/Main Temperature", mainMotor.getMotorTemperature()); + SmartDashboard.putNumber("Arm/Follower Temperature", followerMotor.getMotorTemperature()); + currentAngleLigament.setAngle(Rotation2d.fromRadians(Math.PI).minus(getPosition())); setpointLigament.setAngle(Rotation2d.fromRadians(Math.PI - previousSetpoint.position)); diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index 3d6e607..bbab5a7 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -95,6 +95,11 @@ public void climbBoth() { rightMotor.set(ClimberConstants.climberMotorSpeed); } + public void autoClimb() { + leftMotor.set(0.60); + rightMotor.set(0.60); + } + public void climbLeft() { leftMotor.set(ClimberConstants.climberMotorSpeed); } diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 2c961e2..698bd73 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -12,6 +12,7 @@ import com.revrobotics.RelativeEncoder; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.lib.controllers.VirtualJoystick; @@ -125,6 +126,7 @@ public double getVelocity() { @Override public void periodic() { // This method will be called once per scheduler run + SmartDashboard.putNumber("Intake/Temperature", intakeMotor.getMotorTemperature()); } @Override