diff --git a/Choreo.chor b/Choreo.chor index 858f088..ce7ebc6 100644 --- a/Choreo.chor +++ b/Choreo.chor @@ -31,11 +31,11 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 11 + "controlIntervalCount": 13 }, { - "x": 2.8, - "y": 7, + "x": 3, + "y": 7.2, "heading": 0, "isInitialGuess": false, "translationConstrained": true, @@ -47,236 +47,254 @@ { "x": 1.35, "y": 5.55, - "heading": -4.3570993968158437e-29, - "angularVelocity": -3.1916384239726345e-29, - "velocityX": -3.871776401165942e-19, - "velocityY": -1.7121699442332394e-19, + "heading": 4.888663708781933e-29, + "angularVelocity": 1.29891504869848e-29, + "velocityX": -6.998570319946774e-19, + "velocityY": 6.406261762853189e-20, "timestamp": 0 }, { - "x": 1.3501631846588158, - "y": 5.566593107338153, - "heading": -1.8535831899328968e-19, - "angularVelocity": -2.538192156666707e-18, - "velocityX": 0.002234558539406566, - "velocityY": 0.22721663890968896, - "timestamp": 0.07302769470482262 - }, - { - "x": 1.350914678124224, - "y": 5.599769692082677, - "heading": -6.066880814154026e-19, - "angularVelocity": -5.7694517888806784e-18, - "velocityX": 0.01029052701962414, - "velocityY": 0.45430141097368165, - "timestamp": 0.14605538940964524 - }, - { - "x": 1.352801430038854, - "y": 5.649501306434915, - "heading": -8.506132474260515e-19, - "angularVelocity": -3.3401735682626825e-18, - "velocityX": 0.025836114941689765, - "velocityY": 0.6809966349514003, - "timestamp": 0.21908308411446786 - }, - { - "x": 1.3565504574403007, - "y": 5.715721999894424, - "heading": -1.3108169136313962e-18, - "angularVelocity": -6.301769195562485e-18, - "velocityX": 0.0513370635205263, - "velocityY": 0.9067887700295842, - "timestamp": 0.2921107788192905 - }, - { - "x": 1.3631679130135839, - "y": 5.798286802261685, - "heading": -1.9319982944289288e-18, - "angularVelocity": -8.506106957907115e-18, - "velocityX": 0.09061569860077923, - "velocityY": 1.1305957650840854, - "timestamp": 0.3651384735241131 - }, - { - "x": 1.3741160035630344, - "y": 5.896870448002508, - "heading": -2.781131043423213e-18, - "angularVelocity": -1.1627544214070701e-17, - "velocityX": 0.1499169677170913, - "velocityY": 1.349948757651865, - "timestamp": 0.4381661682289357 - }, - { - "x": 1.391633872206545, - "y": 6.01069206301915, - "heading": -3.400891794190723e-18, - "angularVelocity": -8.4866536405877e-18, - "velocityX": 0.23987979785700603, - "velocityY": 1.5586089014145148, - "timestamp": 0.5111938629337583 - }, - { - "x": 1.4192205811698626, - "y": 6.137703676953337, - "heading": -2.811150296646803e-18, - "angularVelocity": 8.075586501309249e-18, - "velocityX": 0.3777568095862571, - "velocityY": 1.7392252959292935, - "timestamp": 0.584221557638581 - }, - { - "x": 1.46141230152151, - "y": 6.2725925476565525, - "heading": -1.99823128110294e-18, - "angularVelocity": 1.1131653276194991e-17, - "velocityX": 0.5777495855830653, - "velocityY": 1.8470920004859954, - "timestamp": 0.6572492523434036 - }, - { - "x": 1.520159403184126, - "y": 6.406351428886348, - "heading": -7.093289582699771e-19, - "angularVelocity": 1.7649499876047656e-17, - "velocityX": 0.8044496255845875, - "velocityY": 1.8316185629346156, - "timestamp": 0.7302769470482262 - }, - { - "x": 1.5933871109473903, - "y": 6.53200662848169, - "heading": 5.565078136770623e-19, - "angularVelocity": 1.7333653245495004e-17, - "velocityX": 1.0027388658356116, - "velocityY": 1.720651324176584, - "timestamp": 0.8033046417530488 - }, - { - "x": 1.6785544190056327, - "y": 6.646137782298121, - "heading": 1.4392716745677544e-18, - "angularVelocity": 1.2088069870156301e-17, - "velocityX": 1.1662330079390562, - "velocityY": 1.5628475508858934, - "timestamp": 0.8763323364578715 - }, - { - "x": 1.7738116959515682, - "y": 6.747095088288257, - "heading": 2.049610897566005e-18, - "angularVelocity": 8.35764120853224e-18, - "velocityX": 1.3043993423450733, - "velocityY": 1.3824523202945844, - "timestamp": 0.9493600311626941 + "x": 1.350190332759138, + "y": 5.56593160461978, + "heading": 9.174483180873406e-20, + "angularVelocity": 1.2821035239185832e-18, + "velocityX": 0.002659837213357137, + "velocityY": 0.22263889321053065, + "timestamp": 0.07155804805728497 + }, + { + "x": 1.351029129550766, + "y": 5.597782748731759, + "heading": 1.9567996568902531e-19, + "angularVelocity": 1.4524589397396e-18, + "velocityX": 0.011721907103222531, + "velocityY": 0.44510918026333973, + "timestamp": 0.14311609611456994 + }, + { + "x": 1.3530971237579688, + "y": 5.645519147187204, + "heading": 2.323928160474487e-20, + "angularVelocity": -2.4098014605552323e-18, + "velocityX": 0.028899533497705212, + "velocityY": 0.6671003437229781, + "timestamp": 0.2146741441718549 + }, + { + "x": 1.3571508520819868, + "y": 5.709064058447636, + "heading": 1.4355943363493926e-18, + "angularVelocity": 1.9737193420930323e-17, + "velocityX": 0.0566495095096798, + "velocityY": 0.8880190696342456, + "timestamp": 0.28623219222913987 + }, + { + "x": 1.3642081271406485, + "y": 5.788256043062995, + "heading": 2.837313300503685e-18, + "angularVelocity": 1.9588557533693758e-17, + "velocityX": 0.09862307945738516, + "velocityY": 1.1066817327374148, + "timestamp": 0.3577902402864248 + }, + { + "x": 1.375688555587407, + "y": 5.8827544931398625, + "heading": 2.4296061373187325e-18, + "angularVelocity": -5.697572534333239e-18, + "velocityX": 0.16043518177070248, + "velocityY": 1.3205845134464655, + "timestamp": 0.42934828834370975 + }, + { + "x": 1.3936384924939247, + "y": 5.991813080364416, + "heading": 2.0321993765613036e-18, + "angularVelocity": -5.553627734615682e-18, + "velocityX": 0.25084441781838157, + "velocityY": 1.5240576033709512, + "timestamp": 0.5009063364009947 + }, + { + "x": 1.4210099929994566, + "y": 6.1137202559029316, + "heading": 2.387818450434642e-18, + "angularVelocity": 4.969658939882509e-18, + "velocityX": 0.38250764587652614, + "velocityY": 1.7036123657506923, + "timestamp": 0.5724643844582796 + }, + { + "x": 1.4615209177877277, + "y": 6.2446389549136825, + "heading": 4.1549156972604374e-18, + "angularVelocity": 2.4694598170958594e-17, + "velocityX": 0.5661267444702003, + "velocityY": 1.8295454189218536, + "timestamp": 0.6440224325155646 + }, + { + "x": 1.517786418341362, + "y": 6.377933647634973, + "heading": 5.516917516083444e-18, + "angularVelocity": 1.9033523612071367e-17, + "velocityX": 0.7862917181486545, + "velocityY": 1.8627491433902317, + "timestamp": 0.7155804805728495 + }, + { + "x": 1.5893857848153803, + "y": 6.506901147448853, + "heading": 4.405162345070945e-18, + "angularVelocity": -1.553640969128598e-17, + "velocityX": 1.0005774111813406, + "velocityY": 1.8022780569780525, + "timestamp": 0.7871385286301344 + }, + { + "x": 1.6744058580153465, + "y": 6.627281637831437, + "heading": 2.820209714351365e-18, + "angularVelocity": -2.214918716327058e-17, + "velocityX": 1.1881273386767466, + "velocityY": 1.6822774467880341, + "timestamp": 0.8586965766874194 + }, + { + "x": 1.7710338269138677, + "y": 6.736748425632742, + "heading": 9.46416367998995e-19, + "angularVelocity": -2.618564039797122e-17, + "velocityX": 1.3503438330529065, + "velocityY": 1.5297620711123807, + "timestamp": 0.9302546247447043 }, { "x": 1.877881646156311, "y": 6.833992004394531, - "heading": 1.1030466332221302e-28, - "angularVelocity": -2.806621244533741e-17, - "velocityX": 1.425075111919525, - "velocityY": 1.1899172835356724, - "timestamp": 1.0223877258675167 - }, - { - "x": 2.0010551603042983, - "y": 6.911532089428588, - "heading": -3.6362548401734434e-18, - "angularVelocity": -4.556313599596901e-17, - "velocityX": 1.5433933885676219, - "velocityY": 0.9715956828793025, - "timestamp": 1.10219467272885 - }, - { - "x": 2.1310480548368265, - "y": 6.9704646424860215, - "heading": -4.939772330537322e-18, - "angularVelocity": -1.6333383631698326e-17, - "velocityX": 1.628841844545734, - "velocityY": 0.7384388875250714, - "timestamp": 1.1820016195901835 - }, - { - "x": 2.2634767805233813, - "y": 7.009729687566875, - "heading": -6.6279164787986864e-18, - "angularVelocity": -2.115284745662761e-17, - "velocityX": 1.6593633874562848, - "velocityY": 0.49200034113834645, - "timestamp": 1.2618085664515168 - }, - { - "x": 2.3913962069596706, - "y": 7.0296967972646875, - "heading": -7.182609782617947e-18, - "angularVelocity": -6.950439076122258e-18, - "velocityX": 1.6028607968002706, - "velocityY": 0.25019262712943974, - "timestamp": 1.3416155133128502 - }, - { - "x": 2.506814337283472, - "y": 7.034286596670825, - "heading": -5.078238122509265e-18, - "angularVelocity": 2.6368276646069607e-17, - "velocityX": 1.4462165871858836, - "velocityY": 0.0575112767256497, - "timestamp": 1.4214224601741836 - }, - { - "x": 2.6046690912282204, - "y": 7.02969642171058, - "heading": -3.3203370414525467e-18, - "angularVelocity": 2.2026917916704058e-17, - "velocityX": 1.2261433094922236, - "velocityY": -0.057515982523440666, - "timestamp": 1.501229407035517 - }, - { - "x": 2.6831207423418895, - "y": 7.0210734096231695, - "heading": -1.8781525026802954e-18, - "angularVelocity": 1.807091470440922e-17, - "velocityX": 0.983017822370291, - "velocityY": -0.1080483895855807, - "timestamp": 1.5810363538968504 - }, - { - "x": 2.7417633976981484, - "y": 7.011860561958911, - "heading": -7.970782206201774e-19, - "angularVelocity": 1.3546117453473769e-17, - "velocityX": 0.7348064004799985, - "velocityY": -0.11543916949298974, - "timestamp": 1.6608433007581838 - }, - { - "x": 2.780659054634594, - "y": 7.004321125214992, - "heading": -3.0717798619680885e-19, - "angularVelocity": 6.138566300092163e-18, - "velocityX": 0.48737181994675427, - "velocityY": -0.09447093316661626, - "timestamp": 1.7406502476195171 - }, - { - "x": 2.8, - "y": 7, - "heading": -1.3713079058511322e-28, - "angularVelocity": 3.849013128206165e-18, - "velocityX": 0.24234663931073228, - "velocityY": -0.054144725303924224, - "timestamp": 1.8204571944808505 - }, - { - "x": 2.8, - "y": 7, - "heading": -6.673171676019937e-29, - "angularVelocity": 4.595285505142823e-29, - "velocityX": -4.453544453490236e-19, - "velocityY": -4.796301206361866e-19, - "timestamp": 1.900264141342184 + "heading": -2.1890981767747156e-28, + "angularVelocity": -1.3225855172469438e-17, + "velocityX": 1.4931628536991257, + "velocityY": 1.3589467768079406, + "timestamp": 1.0018126728019894 + }, + { + "x": 1.9986537416900598, + "y": 6.920886452908, + "heading": 4.554502013328015e-19, + "angularVelocity": 6.133458490722991e-18, + "velocityX": 1.6264141902972535, + "velocityY": 1.1701905435895423, + "timestamp": 1.0760693389413314 + }, + { + "x": 2.127534528318365, + "y": 6.992660852589142, + "heading": 1.9097316940541736e-19, + "angularVelocity": -3.56166054865944e-18, + "velocityX": 1.735612347408851, + "velocityY": 0.9665718030591074, + "timestamp": 1.1503260050806734 + }, + { + "x": 2.2616402166936984, + "y": 7.048093064188636, + "heading": -3.1290127944716826e-19, + "angularVelocity": -6.7855786351346426e-18, + "velocityX": 1.8059750773498724, + "velocityY": 0.7464947523570259, + "timestamp": 1.2245826712200154 + }, + { + "x": 2.396323060426075, + "y": 7.086377869098208, + "heading": -6.935100968670578e-19, + "angularVelocity": -5.125584449886424e-18, + "velocityX": 1.8137475156855276, + "velocityY": 0.5155739800726428, + "timestamp": 1.2988393373593574 + }, + { + "x": 2.525169727689623, + "y": 7.108528682607002, + "heading": -6.363650281895382e-19, + "angularVelocity": 7.695615179675032e-19, + "velocityX": 1.7351528685818678, + "velocityY": 0.29830067333883487, + "timestamp": 1.3730960034986994 + }, + { + "x": 2.642239264382167, + "y": 7.118202884308512, + "heading": 9.398260833717345e-19, + "angularVelocity": 2.1226257354906617e-17, + "velocityX": 1.5765525545325272, + "velocityY": 0.13028058227281855, + "timestamp": 1.4473526696380414 + }, + { + "x": 2.744150088890627, + "y": 7.1198409094509065, + "heading": 2.2211820928632972e-18, + "angularVelocity": 1.725577079428282e-17, + "velocityX": 1.3724131422435668, + "velocityY": 0.022058964218889237, + "timestamp": 1.5216093357773834 + }, + { + "x": 2.8295064534990373, + "y": 7.116971541906974, + "heading": 4.289301494316625e-18, + "angularVelocity": 2.78509594470971e-17, + "velocityX": 1.1494774684228555, + "velocityY": -0.038641211529968134, + "timestamp": 1.5958660019167255 + }, + { + "x": 2.8978272292721137, + "y": 7.112063675436808, + "heading": 5.032303806817314e-18, + "angularVelocity": 1.0005866814564975e-17, + "velocityX": 0.9200625253656417, + "velocityY": -0.06609327788006458, + "timestamp": 1.6701226680560675 + }, + { + "x": 2.9489939084325294, + "y": 7.106833692702939, + "heading": 2.7876752491252017e-18, + "angularVelocity": -3.022797359622289e-17, + "velocityX": 0.6890516612242018, + "velocityY": -0.07043115462081231, + "timestamp": 1.7443793341954095 + }, + { + "x": 2.983027390820784, + "y": 7.102509456230238, + "heading": 1.1453982616859274e-18, + "angularVelocity": -2.2116222868817296e-17, + "velocityX": 0.4583222511533027, + "velocityY": -0.05823364685414326, + "timestamp": 1.8186360003347515 + }, + { + "x": 3, + "y": 7.1, + "heading": 3.4956470282563025e-28, + "angularVelocity": -1.5424854285689865e-17, + "velocityX": 0.2285668083679807, + "velocityY": -0.03379435626197756, + "timestamp": 1.8928926664740935 + }, + { + "x": 3, + "y": 7.1, + "heading": 1.7002100246869952e-28, + "angularVelocity": -1.282403088850194e-28, + "velocityX": -3.9396245875565884e-19, + "velocityY": -1.0210640209084137e-18, + "timestamp": 1.9671493326134355 } ], "trajectoryWaypoints": [ @@ -292,7 +310,7 @@ "controlIntervalCount": 14 }, { - "timestamp": 1.0223877258675167, + "timestamp": 1.0018126728019894, "isStopPoint": false, "x": 1.877881646156311, "y": 6.833992004394531, @@ -300,13 +318,13 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 11 + "controlIntervalCount": 13 }, { - "timestamp": 1.900264141342184, + "timestamp": 1.9671493326134355, "isStopPoint": true, - "x": 2.8, - "y": 7, + "x": 3, + "y": 7.1, "heading": 0, "isInitialGuess": false, "translationConstrained": true, @@ -333,7 +351,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": false + "isTrajectoryStale": true }, "IntakeBottom": { "waypoints": [ @@ -353,11 +371,11 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 6 + "controlIntervalCount": 7 }, { - "x": 2.5, - "y": 4.1, + "x": 2.6, + "y": 4.05, "heading": 0, "isInitialGuess": false, "translationConstrained": true, @@ -369,200 +387,209 @@ { "x": 1.45, "y": 5.55, - "heading": 2.768251771677101e-30, - "angularVelocity": 3.753704150028592e-30, - "velocityX": 1.78848438354095e-18, - "velocityY": 1.274461077577e-18, + "heading": -5.2307394201705865e-30, + "angularVelocity": -8.864376707937236e-30, + "velocityX": -1.3538604630521143e-22, + "velocityY": 3.4418814600525267e-22, "timestamp": 0 }, { - "x": 1.4535815159544254, - "y": 5.524007481828958, - "heading": -9.823289382952179e-20, - "angularVelocity": -1.0697381656316235e-18, - "velocityX": 0.03900204848849219, - "velocityY": -0.2830537311420744, - "timestamp": 0.09182891907542194 - }, - { - "x": 1.4610505989891902, - "y": 5.472066455928889, - "heading": -2.6372924069556547e-19, - "angularVelocity": -1.8022248744733155e-18, - "velocityX": 0.08133693731394162, - "velocityY": -0.5656281966805212, - "timestamp": 0.18365783815084388 - }, - { - "x": 1.4728314962474147, - "y": 5.3942440380110686, - "heading": -1.237796661602218e-18, - "angularVelocity": -1.0607414612468509e-17, - "velocityX": 0.12829179933186746, - "velocityY": -0.8474717845073405, - "timestamp": 0.2754867572262658 - }, - { - "x": 1.4895508316112023, - "y": 5.290652453052921, - "heading": 9.017349016336884e-19, - "angularVelocity": 2.3299103400186367e-17, - "velocityX": 0.18207047988508238, - "velocityY": -1.1280932630061935, - "timestamp": 0.36731567630168777 - }, - { - "x": 1.5122251256195887, - "y": 5.161507462826687, - "heading": 3.7532473257995165e-18, - "angularVelocity": 3.1052443148810655e-17, - "velocityX": 0.24691888173101642, - "velocityY": -1.406365135584243, - "timestamp": 0.4591445953771097 - }, - { - "x": 1.5427732007543524, - "y": 5.007333660042885, - "heading": 4.963081319348206e-18, - "angularVelocity": 1.317486833181533e-17, - "velocityX": 0.332662906655519, - "velocityY": -1.6789242902580064, - "timestamp": 0.5509735144525316 - }, - { - "x": 1.5859604538125973, - "y": 4.83016662668056, - "heading": 3.7369902588295676e-18, - "angularVelocity": -1.3351904924389808e-17, - "velocityX": 0.47030122419883236, - "velocityY": -1.929316332439631, - "timestamp": 0.6428024335279535 - }, - { - "x": 1.6551996012505341, - "y": 4.649879493853798, - "heading": 4.2454026339170335e-18, - "angularVelocity": 5.536517239702926e-18, - "velocityX": 0.7540015513094512, - "velocityY": -1.9632936404077395, - "timestamp": 0.7346313526033754 - }, - { - "x": 1.7358734935380205, - "y": 4.493207693475708, - "heading": 6.286624819906553e-18, - "angularVelocity": 2.222853328563577e-17, - "velocityX": 0.8785238147244084, - "velocityY": -1.7061270235581365, - "timestamp": 0.8264602716787973 - }, - { - "x": 1.821714417214334, - "y": 4.362260193361412, - "heading": 5.80968001326458e-18, - "angularVelocity": -5.193840614109149e-18, - "velocityX": 0.9347918339856989, - "velocityY": -1.4259941359651997, - "timestamp": 0.9182891907542192 - }, - { - "x": 1.910434779967122, - "y": 4.25739231863962, - "heading": 4.6383131324528144e-18, - "angularVelocity": -1.2755968893296887e-17, - "velocityX": 0.9661483947137779, - "velocityY": -1.141991823251114, - "timestamp": 1.0101181098296412 - }, - { - "x": 2.000880418708742, - "y": 4.178705764553529, - "heading": 3.484037709528354e-18, - "angularVelocity": -1.2569846135457474e-17, - "velocityX": 0.984936332171851, - "velocityY": -0.8568820680684339, - "timestamp": 1.1019470289050632 - }, - { - "x": 2.092359402339598, - "y": 4.126236959884595, - "heading": 2.3397136627308054e-18, - "angularVelocity": -1.2461477510076924e-17, - "velocityX": 0.9961892675275468, - "velocityY": -0.5713756101896847, - "timestamp": 1.1937759479804853 - }, - { - "x": 2.1844115737917704, - "y": 4.100000000000099, - "heading": 1.2005907158708014e-18, - "angularVelocity": -1.2404838513215584e-17, - "velocityX": 1.0024311772210557, - "velocityY": -0.2857156563340674, - "timestamp": 1.2856048670559073 + "x": 1.452606941735721, + "y": 5.523859415940948, + "heading": -5.453770233981043e-19, + "angularVelocity": -5.9354202330099835e-18, + "velocityX": 0.028371739293966493, + "velocityY": -0.2844919108674468, + "timestamp": 0.09188515757564493 + }, + { + "x": 1.4581773637573403, + "y": 5.471616264225246, + "heading": -1.1901577281995487e-18, + "angularVelocity": -7.017245459535184e-18, + "velocityX": 0.06062374129394484, + "velocityY": -0.5685700835055113, + "timestamp": 0.18377031515128986 + }, + { + "x": 1.4672070882638757, + "y": 5.393331617868329, + "heading": -2.4927335692346936e-18, + "angularVelocity": -1.4176128935270865e-17, + "velocityX": 0.09827185091456853, + "velocityY": -0.8519835893241934, + "timestamp": 0.2756554727269348 + }, + { + "x": 1.480431582387895, + "y": 5.289113786785799, + "heading": -3.475442526041714e-18, + "angularVelocity": -1.0694969489413464e-17, + "velocityX": 0.1439241600378399, + "velocityY": -1.1342183420291005, + "timestamp": 0.3675406303025797 + }, + { + "x": 1.4990508129714903, + "y": 5.159185590256103, + "heading": -5.7244658355497334e-18, + "angularVelocity": -2.4476459189296043e-17, + "velocityX": 0.20263588891673706, + "velocityY": -1.414028118988987, + "timestamp": 0.45942578787822463 + }, + { + "x": 1.5253463185703813, + "y": 5.004133685702563, + "heading": -6.359199100896898e-18, + "angularVelocity": -6.90789766365868e-18, + "velocityX": 0.2861779453035507, + "velocityY": -1.6874532149100712, + "timestamp": 0.5513109454538696 + }, + { + "x": 1.5649780333860743, + "y": 4.8264483765464625, + "heading": -3.569415506183744e-18, + "angularVelocity": 3.036163476589336e-17, + "velocityX": 0.4313179175109533, + "velocityY": -1.933775963869018, + "timestamp": 0.6431961030295146 + }, + { + "x": 1.6308669955396757, + "y": 4.64793756312066, + "heading": -1.4562604741055196e-18, + "angularVelocity": 2.2997784275917627e-17, + "velocityX": 0.7170794924017834, + "velocityY": -1.942760051087059, + "timestamp": 0.7350812606051595 + }, + { + "x": 1.7098906699648946, + "y": 4.492177678744699, + "heading": -1.558428454464565e-18, + "angularVelocity": -1.1119095080144942e-18, + "velocityX": 0.8600265430264089, + "velocityY": -1.6951582658791249, + "timestamp": 0.8269664181808045 + }, + { + "x": 1.7959410105992413, + "y": 4.361730871045984, + "heading": -1.2903210307338604e-18, + "angularVelocity": 2.9178534467353044e-18, + "velocityX": 0.9364988089997597, + "velocityY": -1.4196722424002381, + "timestamp": 0.9188515757564495 + }, + { + "x": 1.8866178379387055, + "y": 4.257143716998589, + "heading": -1.2077568984981302e-18, + "angularVelocity": 8.985578783272284e-19, + "velocityX": 0.9868495601676925, + "velocityY": -1.1382377394444156, + "timestamp": 1.0107367333320945 + }, + { + "x": 1.9806760078532668, + "y": 4.178608293809009, + "heading": -1.3266358389595163e-18, + "angularVelocity": -1.2937774020609767e-18, + "velocityX": 1.0236492203556078, + "velocityY": -0.8547128313397577, + "timestamp": 1.1026218909077394 + }, + { + "x": 2.077358446166334, + "y": 4.126211719670334, + "heading": -7.845208319757716e-19, + "angularVelocity": 5.8999192175576245e-18, + "velocityX": 1.0522095283285875, + "velocityY": -0.5702398028271249, + "timestamp": 1.1945070484833844 + }, + { + "x": 2.1761574024693604, + "y": 4.1, + "heading": -2.7370402589601427e-19, + "angularVelocity": 5.559296186267328e-18, + "velocityX": 1.0752439121812418, + "velocityY": -0.2852660904320238, + "timestamp": 1.2863922060590294 }, { "x": 2.2767090797424316, "y": 4.1, - "heading": -3.149908443603892e-30, - "angularVelocity": -1.3074211163549053e-17, - "velocityX": 1.0051028246870142, - "velocityY": 8.993242339720801e-19, - "timestamp": 1.3774337861313293 + "heading": -1.8826809191115895e-30, + "angularVelocity": 2.9787621104516863e-18, + "velocityX": 1.0943190383092243, + "velocityY": -4.40245241179264e-22, + "timestamp": 1.3782773636346743 }, { - "x": 2.3511393798440374, - "y": 4.099999999999869, - "heading": 1.3002264264519373e-21, - "angularVelocity": 1.879818059738522e-20, - "velocityX": 1.0760850992260855, - "velocityY": -1.5351048777679467e-20, - "timestamp": 1.4466014571217791 + "x": 2.3690778996719204, + "y": 4.099999999999997, + "heading": 5.44516115570243e-20, + "angularVelocity": 7.741237177309166e-19, + "velocityX": 1.3131823325293643, + "velocityY": 5.885346127125529e-19, + "timestamp": 1.4486170397340123 }, { - "x": 2.410683625785678, - "y": 4.100000000000077, - "heading": 1.1949798510975526e-21, - "angularVelocity": -1.521615128458001e-21, - "velocityX": 0.8608681641102585, - "velocityY": -1.535104870754249e-20, - "timestamp": 1.515769128112229 + "x": 2.4460519288415474, + "y": 4.099999999999996, + "heading": 5.24132830289922e-20, + "angularVelocity": -2.8978360918787595e-20, + "velocityX": 1.0943187890276875, + "velocityY": 5.88534612635337e-19, + "timestamp": 1.5189567158333503 }, { - "x": 2.455341812172794, - "y": 4.099999999999992, - "heading": 7.380066473834956e-22, - "angularVelocity": -6.606745193011693e-21, - "velocityX": 0.6456511510226041, - "velocityY": -1.535104870754326e-20, - "timestamp": 1.5849367991026788 + "x": 2.5076311556444177, + "y": 4.099999999999994, + "heading": 3.7707989431980485e-20, + "angularVelocity": -2.0906115019816175e-19, + "velocityX": 0.8754550805139398, + "velocityY": 5.885346126353339e-19, + "timestamp": 1.5892963919326883 }, { - "x": 2.485113937210665, - "y": 4.099999999999996, - "heading": 2.8113994728970013e-22, - "angularVelocity": -6.605205487782831e-21, - "velocityX": 0.4304341119443362, - "velocityY": -1.535104870754403e-20, - "timestamp": 1.6541044700931287 + "x": 2.553815577211842, + "y": 4.100000000000007, + "heading": 2.1154826696226223e-20, + "angularVelocity": -2.353318020011725e-19, + "velocityX": 0.6565913312168156, + "velocityY": 5.88534612635337e-19, + "timestamp": 1.6596360680320263 }, { - "x": 2.5, + "x": 2.5846051922441835, + "y": 4.100000000000005, + "heading": 7.653818243547418e-21, + "angularVelocity": -1.9194015659802422e-19, + "velocityX": 0.43772756344312364, + "velocityY": 5.885346126353432e-19, + "timestamp": 1.7299757441313643 + }, + { + "x": 2.6, "y": 4.1, - "heading": 7.593583950238481e-31, - "angularVelocity": -4.0646147043633575e-21, - "velocityX": 0.21521705987076065, - "velocityY": -1.5351049104609927e-20, - "timestamp": 1.7232721410835785 + "heading": 1.4358514620772855e-29, + "angularVelocity": -1.0881224717702113e-19, + "velocityX": 0.21886378512853635, + "velocityY": 5.885346134223182e-19, + "timestamp": 1.8003154202307023 }, { - "x": 2.5, + "x": 2.6, "y": 4.1, - "heading": 3.769724316157637e-31, - "angularVelocity": -7.826678887830059e-32, - "velocityX": 8.574729652195167e-20, - "velocityY": -5.117016267149893e-21, - "timestamp": 1.7924398120740284 + "heading": 7.114235406395646e-30, + "angularVelocity": -1.8487973671908514e-30, + "velocityX": -9.551547841275521e-23, + "velocityY": 1.961782036630677e-19, + "timestamp": 1.8706550963300403 } ], "trajectoryWaypoints": [ @@ -578,7 +605,7 @@ "controlIntervalCount": 15 }, { - "timestamp": 1.3774337861313293, + "timestamp": 1.3782773636346743, "isStopPoint": false, "x": 2.2767090797424316, "y": 4.1, @@ -586,12 +613,12 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 6 + "controlIntervalCount": 7 }, { - "timestamp": 1.7924398120740284, + "timestamp": 1.8706550963300403, "isStopPoint": true, - "x": 2.5, + "x": 2.6, "y": 4.1, "heading": 0, "isInitialGuess": false, @@ -626,18 +653,18 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": false + "isTrajectoryStale": true }, "ShootTop": { "waypoints": [ { - "x": 2.8, - "y": 7, + "x": 3, + "y": 7.2, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 11 + "controlIntervalCount": 12 }, { "x": 1.924111247062683, @@ -660,254 +687,263 @@ ], "trajectory": [ { - "x": 2.8, - "y": 7, - "heading": -1.9646033564625967e-29, - "angularVelocity": -9.696989945655977e-30, - "velocityX": 4.018058415605973e-19, - "velocityY": 3.0077486913643975e-19, + "x": 3, + "y": 7.1, + "heading": 5.328728317892494e-29, + "angularVelocity": 2.994551890502943e-29, + "velocityX": 2.391620856941307e-19, + "velocityY": 5.543392408149442e-19, "timestamp": 0 }, { - "x": 2.784542553218789, - "y": 7.004075603918491, - "heading": -1.074560688956109e-20, - "angularVelocity": -1.499173101945667e-19, - "velocityX": -0.215654459627409, - "velocityY": 0.056860759292140066, - "timestamp": 0.07167691689713619 - }, - { - "x": 2.7534308188059504, - "y": 7.01138949058107, - "heading": -8.298930766365304e-20, - "angularVelocity": -1.0079075787262438e-18, - "velocityX": -0.434055142987649, - "velocityY": 0.1020396381320535, - "timestamp": 0.14335383379427238 - }, - { - "x": 2.706470215496378, - "y": 7.020790608738608, - "heading": -1.4026418473866712e-19, - "angularVelocity": -7.990700882676518e-19, - "velocityX": -0.6551705255006609, - "velocityY": 0.13115963359595603, - "timestamp": 0.2150307506914086 - }, - { - "x": 2.643529956238723, - "y": 7.030631903107819, - "heading": -2.536336497701418e-19, - "angularVelocity": -1.5816733923360723e-18, - "velocityX": -0.8781105826300982, - "velocityY": 0.1373007489057606, - "timestamp": 0.28670766758854477 - }, - { - "x": 2.5647300828668627, - "y": 7.038469258749899, - "heading": -1.817623793169356e-18, - "angularVelocity": -2.181999736144724e-17, - "velocityX": -1.0993758769616024, - "velocityY": 0.10934281189932535, - "timestamp": 0.35838458448568095 - }, - { - "x": 2.4709713896509418, - "y": 7.04066952277123, - "heading": -2.6584019225216905e-18, - "angularVelocity": -1.1730109963461826e-17, - "velocityX": -1.308073746397686, - "velocityY": 0.03069696796568231, - "timestamp": 0.4300615013828171 - }, - { - "x": 2.3650710437640527, - "y": 7.0324715500090775, - "heading": -2.3590886008150758e-18, - "angularVelocity": 4.175867742019161e-18, - "velocityX": -1.477467927913001, - "velocityY": -0.11437395911374147, - "timestamp": 0.5017384182799534 - }, - { - "x": 2.252413840249641, - "y": 7.00978606790068, - "heading": -9.526829247284803e-19, - "angularVelocity": 1.962145876864572e-17, - "velocityX": -1.5717361793864884, - "velocityY": -0.31649634345565264, - "timestamp": 0.5734153351770895 - }, - { - "x": 2.138829620759392, - "y": 6.971141772445123, - "heading": 2.730306996341542e-19, - "angularVelocity": 1.710053491141221e-17, - "velocityX": -1.584669436235366, - "velocityY": -0.5391456151926471, - "timestamp": 0.6450922520742257 - }, - { - "x": 2.028517459409352, - "y": 6.916850216651246, - "heading": 3.0972418486164364e-19, - "angularVelocity": 5.119291230262301e-19, - "velocityX": -1.5390193401914194, - "velocityY": -0.7574482573255116, - "timestamp": 0.7167691689713619 + "x": 2.9835267953809015, + "y": 7.102951517663552, + "heading": -2.3397675981061006e-19, + "angularVelocity": -3.1903606396247306e-18, + "velocityX": -0.2246183033453931, + "velocityY": 0.040245046741689515, + "timestamp": 0.07333865661770078 + }, + { + "x": 2.9504670020736095, + "y": 7.108130812530926, + "heading": -4.184209395656505e-19, + "angularVelocity": -2.5149654253878084e-18, + "velocityX": -0.4507826408599954, + "velocityY": 0.07062162175238734, + "timestamp": 0.14667731323540156 + }, + { + "x": 2.9007198966227685, + "y": 7.114579561743433, + "heading": -8.177693496627639e-19, + "angularVelocity": -5.445264878549276e-18, + "velocityX": -0.6783203803450059, + "velocityY": 0.08793110631895902, + "timestamp": 0.22001596985310234 + }, + { + "x": 2.8342373194127544, + "y": 7.120985107849634, + "heading": -1.0271876922091664e-18, + "angularVelocity": -2.8554973915260827e-18, + "velocityX": -0.9065147941904007, + "velocityY": 0.08734201581247407, + "timestamp": 0.2933546264708031 + }, + { + "x": 2.7511281681952666, + "y": 7.125484114660636, + "heading": -1.4107742006798458e-18, + "angularVelocity": -5.230345310248024e-18, + "velocityX": -1.1332243464829255, + "velocityY": 0.06134563978803308, + "timestamp": 0.3666932830885039 + }, + { + "x": 2.6519339713436176, + "y": 7.125362609918432, + "heading": -4.898612672627759e-19, + "angularVelocity": 1.2556991953459379e-17, + "velocityX": -1.3525499569576642, + "velocityY": -0.0016567625832099155, + "timestamp": 0.4400319397062047 + }, + { + "x": 2.5383097922427513, + "y": 7.116764467339315, + "heading": 9.100508105717479e-20, + "angularVelocity": 7.920329473296263e-18, + "velocityX": -1.5493081594500338, + "velocityY": -0.11723888840781427, + "timestamp": 0.5133705963239055 + }, + { + "x": 2.414178078079128, + "y": 7.095140602226604, + "heading": 2.293214994757053e-18, + "angularVelocity": 3.002795510829523e-17, + "velocityX": -1.6925823281801557, + "velocityY": -0.29484948470396755, + "timestamp": 0.5867092529416063 + }, + { + "x": 2.2857248751565793, + "y": 7.057348791139869, + "heading": 2.914794726214484e-18, + "angularVelocity": 8.475471990508203e-18, + "velocityX": -1.7515074429584823, + "velocityY": -0.5153054723109491, + "timestamp": 0.660047909559307 + }, + { + "x": 2.1588237316891385, + "y": 7.002893580306527, + "heading": 2.7554966147745315e-18, + "angularVelocity": -2.1720891833152955e-18, + "velocityX": -1.7303445320662507, + "velocityY": -0.742517157315788, + "timestamp": 0.7333865661770078 + }, + { + "x": 2.0374652766601096, + "y": 6.932647345835075, + "heading": 1.4910872369782204e-18, + "angularVelocity": -1.7240694270637026e-17, + "velocityX": -1.6547679031207028, + "velocityY": -0.9578336679644566, + "timestamp": 0.8067252227947086 }, { "x": 1.924111247062683, "y": 6.84770393371582, - "heading": 6.407090500217256e-29, - "angularVelocity": -4.32111484648676e-18, - "velocityX": -1.456622534371389, - "velocityY": -0.9646938781478046, - "timestamp": 0.7884460858684981 - }, - { - "x": 1.818298105467391, - "y": 6.754462828175385, - "heading": -2.2877849346052042e-18, - "angularVelocity": -2.897858022545703e-17, - "velocityX": -1.3402984736566912, - "velocityY": -1.1810528404474963, - "timestamp": 0.8673935320345054 - }, - { - "x": 1.7227052966018288, - "y": 6.6447401770404, - "heading": -5.41708992016127e-18, - "angularVelocity": -3.9637823526915386e-17, - "velocityX": -1.2108410532264366, - "velocityY": -1.389818879060185, - "timestamp": 0.9463409782005128 - }, - { - "x": 1.6388486864527716, - "y": 6.51957867430666, - "heading": -8.034081738524998e-18, - "angularVelocity": -3.3148529718639963e-17, - "velocityX": -1.0621826825569416, - "velocityY": -1.5853774733943033, - "timestamp": 1.0252884243665201 - }, - { - "x": 1.5690391425832642, - "y": 6.3810464441104715, - "heading": -8.167060718050385e-18, - "angularVelocity": -1.6843985029222782e-18, - "velocityX": -0.8842533515551733, - "velocityY": -1.7547398544694959, - "timestamp": 1.1042358705325275 - }, - { - "x": 1.5165991873344993, - "y": 6.233889035842811, - "heading": -8.03067407946299e-18, - "angularVelocity": 1.727562204439328e-18, - "velocityX": -0.6642387790326363, - "velocityY": -1.8639920024562813, - "timestamp": 1.1831833166985348 - }, - { - "x": 1.4834743568980886, - "y": 6.088470059244437, - "heading": -6.730491121185925e-18, - "angularVelocity": 1.646896772299913e-17, - "velocityX": -0.41958077233925584, - "velocityY": -1.8419718896624646, - "timestamp": 1.2621307628645422 - }, - { - "x": 1.464950061080987, - "y": 5.955815119719372, - "heading": -5.033393105944581e-18, - "angularVelocity": 2.1496553576719962e-17, - "velocityX": -0.23464084928047224, - "velocityY": -1.6802942459500272, - "timestamp": 1.3410782090305495 - }, - { - "x": 1.45518191909667, - "y": 5.8404641148694925, - "heading": -2.193426124351463e-18, - "angularVelocity": 3.597287899593999e-17, - "velocityX": -0.12372967662064968, - "velocityY": -1.46111128923947, - "timestamp": 1.4200256551965569 - }, - { - "x": 1.4505536205430332, - "y": 5.743812802117125, - "heading": -1.0958624904634658e-18, - "angularVelocity": 1.3902458700836077e-17, - "velocityX": -0.058625057282380676, - "velocityY": -1.22424875592576, - "timestamp": 1.4989731013625642 - }, - { - "x": 1.4489044529314807, - "y": 5.666324508692117, - "heading": -9.453505954440147e-19, - "angularVelocity": 1.9064820550018808e-18, - "velocityX": -0.020889435840024388, - "velocityY": -0.9815174168140834, - "timestamp": 1.5779205475285716 - }, - { - "x": 1.4488507884880897, - "y": 5.608163681141726, - "heading": -4.960724404906744e-19, - "angularVelocity": 5.690850862681648e-18, - "velocityX": -0.0006797489508501602, - "velocityY": -0.7367030901586236, - "timestamp": 1.656867993694579 - }, - { - "x": 1.4494445328511927, - "y": 5.569385254234905, - "heading": -9.719625039323152e-20, - "angularVelocity": 5.0524266158253124e-18, - "velocityX": 0.0075207545296392874, - "velocityY": -0.49119292377419976, - "timestamp": 1.7358154398605863 - }, - { - "x": 1.45, + "heading": -7.102434620663822e-29, + "angularVelocity": -2.033153143668509e-17, + "velocityX": -1.545624569977504, + "velocityY": -1.1582351795979753, + "timestamp": 0.8800638794124094 + }, + { + "x": 1.8153214232861066, + "y": 6.7429964897093875, + "heading": 1.2402111094447622e-18, + "angularVelocity": 1.6056002253164752e-17, + "velocityX": -1.408413197959982, + "velocityY": -1.355561953719872, + "timestamp": 0.9573067112277251 + }, + { + "x": 1.71832902814594, + "y": 6.6239547571808215, + "heading": 2.352886334010048e-18, + "angularVelocity": 1.440489923585888e-17, + "velocityX": -1.2556815028766475, + "velocityY": -1.5411363065144632, + "timestamp": 1.034549543043041 + }, + { + "x": 1.6348123053495307, + "y": 6.492143633170018, + "heading": 4.278303768048229e-18, + "angularVelocity": 2.4926810257145375e-17, + "velocityX": -1.0812229540730383, + "velocityY": -1.7064512125329725, + "timestamp": 1.1117923748583567 + }, + { + "x": 1.5670661222111097, + "y": 6.3505375555945065, + "heading": 2.3126233047868595e-18, + "angularVelocity": -2.5448062617155708e-17, + "velocityX": -0.8770546281936653, + "velocityY": -1.833258494642424, + "timestamp": 1.1890352066736725 + }, + { + "x": 1.5174753121386582, + "y": 6.20505418783942, + "heading": 4.2984344546564e-19, + "angularVelocity": -2.437481609179316e-17, + "velocityX": -0.6420118075284986, + "velocityY": -1.8834546110763, + "timestamp": 1.2662780384889882 + }, + { + "x": 1.4856195693046967, + "y": 6.065058833994227, + "heading": -2.0521702764096268e-18, + "angularVelocity": -3.213260872902254e-17, + "velocityX": -0.4124103439190814, + "velocityY": -1.8124057670451137, + "timestamp": 1.343520870304304 + }, + { + "x": 1.4668576245906764, + "y": 5.938224157051082, + "heading": -2.054194079511719e-19, + "angularVelocity": 2.3908378413503538e-17, + "velocityX": -0.24289560950743078, + "velocityY": -1.6420252075483086, + "timestamp": 1.4207637021196198 + }, + { + "x": 1.4564790556484006, + "y": 5.8279535998004075, + "heading": -1.2036239635638084e-20, + "angularVelocity": 2.5035742348848644e-18, + "velocityX": -0.13436287482161724, + "velocityY": -1.4275830476337525, + "timestamp": 1.4980065339349355 + }, + { + "x": 1.4513037055003448, + "y": 5.735503750387027, + "heading": 6.030268437067719e-19, + "angularVelocity": 7.962720338440099e-18, + "velocityX": -0.0670010410892224, + "velocityY": -1.1968728649715128, + "timestamp": 1.5752493657502513 + }, + { + "x": 1.4492712175744427, + "y": 5.661350719645461, + "heading": 3.9070459796724298e-19, + "angularVelocity": -2.7487631495883376e-18, + "velocityX": -0.02631296494437102, + "velocityY": -0.9599988633106512, + "timestamp": 1.652492197565567 + }, + { + "x": 1.4489910088861964, + "y": 5.605679590959494, + "heading": 1.4359510929457683e-19, + "angularVelocity": -3.1991252215095752e-18, + "velocityX": -0.003627633552384207, + "velocityY": -0.720728737898362, + "timestamp": 1.7297350293808829 + }, + { + "x": 1.4494771717267334, + "y": 5.568557421333253, + "heading": 7.624915547302237e-20, + "angularVelocity": -8.718732250403981e-19, + "velocityX": 0.0062939541324402715, + "velocityY": -0.480590480099053, + "timestamp": 1.8069778611961986 + }, + { + "x": 1.4499999999999997, "y": 5.55, - "heading": -9.227100842119755e-29, - "angularVelocity": 1.2311512301956196e-18, - "velocityX": 0.007035910288550044, - "velocityY": -0.2455463118361552, - "timestamp": 1.8147628860265936 + "heading": 3.564481997291273e-29, + "angularVelocity": -9.871356945218981e-19, + "velocityX": 0.006768631613475691, + "velocityY": -0.2402478119593811, + "timestamp": 1.8842206930115144 }, { "x": 1.45, "y": 5.55, - "heading": -4.442595582278228e-29, - "angularVelocity": 4.3308515496563856e-29, - "velocityX": 5.101718397952589e-19, - "velocityY": 3.912914703079033e-19, - "timestamp": 1.893710332192601 + "heading": 1.7735415592254587e-29, + "angularVelocity": -2.2524913066799505e-30, + "velocityX": 7.383067793848295e-19, + "velocityY": 2.654058206215708e-19, + "timestamp": 1.9614635248268302 } ], "trajectoryWaypoints": [ { "timestamp": 0, "isStopPoint": true, - "x": 2.8, - "y": 7, + "x": 3, + "y": 7.1, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 11 + "controlIntervalCount": 12 }, { - "timestamp": 0.7884460858684981, + "timestamp": 0.8800638794124094, "isStopPoint": false, "x": 1.924111247062683, "y": 6.84770393371582, @@ -918,7 +954,7 @@ "controlIntervalCount": 14 }, { - "timestamp": 1.893710332192601, + "timestamp": 1.9614635248268302, "isStopPoint": true, "x": 1.45, "y": 5.55, @@ -948,18 +984,18 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": false + "isTrajectoryStale": true }, "ShootBottom": { "waypoints": [ { - "x": 2.5, - "y": 4.1, + "x": 2.6, + "y": 4.05, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 10 + "controlIntervalCount": 11 }, { "x": 1.7625038623809814, @@ -982,236 +1018,245 @@ ], "trajectory": [ { - "x": 2.5, + "x": 2.6, "y": 4.1, - "heading": 3.0967108534958695e-28, - "angularVelocity": 1.2537509885957492e-28, - "velocityX": 3.670919197136962e-19, - "velocityY": -1.9210528115478423e-19, + "heading": 6.261638676921501e-29, + "angularVelocity": 9.170070232409291e-29, + "velocityX": 4.146356482747438e-19, + "velocityY": -2.5365636076256593e-19, "timestamp": 0 }, { - "x": 2.4836763563982793, - "y": 4.098255982314194, - "heading": 6.919098083314485e-19, - "angularVelocity": 9.525666459582977e-18, - "velocityX": -0.2247310069213002, - "velocityY": -0.024010255319366687, - "timestamp": 0.07263636569490929 - }, - { - "x": 2.4509528242374956, - "y": 4.095772654138915, - "heading": -1.3582350238151511e-18, - "angularVelocity": -2.8224771398888464e-17, - "velocityX": -0.4505116940707321, - "velocityY": -0.03418849705732298, - "timestamp": 0.14527273138981858 - }, - { - "x": 2.401825930824725, - "y": 4.093947084553211, - "heading": -2.4277820237028326e-18, - "angularVelocity": -1.4724676742822545e-17, - "velocityX": -0.6763401905323785, - "velocityY": -0.02513299735042192, - "timestamp": 0.2179090970847279 - }, - { - "x": 2.3365018551955687, - "y": 4.094796243076533, - "heading": -2.935902435422405e-18, - "angularVelocity": -6.995399856818831e-18, - "velocityX": -0.8993301771591694, - "velocityY": 0.011690542551199607, - "timestamp": 0.29054546277963716 - }, - { - "x": 2.255764085823297, - "y": 4.101295259482685, - "heading": -1.2006463225403345e-18, - "angularVelocity": 2.3889631972433773e-17, - "velocityX": -1.1115337145557038, - "velocityY": 0.08947331469771397, - "timestamp": 0.36318182847454644 - }, - { - "x": 2.1618621036192964, - "y": 4.11760275744144, - "heading": -1.8160986813788466e-18, - "angularVelocity": -8.473061122341092e-18, - "velocityX": -1.2927681789261922, - "velocityY": 0.22450872647343323, - "timestamp": 0.4358181941694557 - }, - { - "x": 2.0597008424943715, - "y": 4.148097823772798, - "heading": -2.079600408456036e-18, - "angularVelocity": -3.627682928555843e-18, - "velocityX": -1.4064753949106839, - "velocityY": 0.419831940106532, - "timestamp": 0.508454559864365 - }, - { - "x": 1.955870470222415, - "y": 4.194924358715386, - "heading": -2.601281739414446e-18, - "angularVelocity": -7.182095235468659e-18, - "velocityX": -1.429454396270173, - "velocityY": 0.6446706755575111, - "timestamp": 0.5810909255592743 - }, - { - "x": 1.855695665580765, - "y": 4.2577552575109054, - "heading": -1.2347809272193171e-18, - "angularVelocity": 1.88129020336254e-17, - "velocityX": -1.3791274340908255, - "velocityY": 0.8650060915602888, - "timestamp": 0.6537272912541836 + "x": 2.584485493302708, + "y": 4.0976238494988095, + "heading": 1.8174207932807514e-19, + "angularVelocity": 2.558915271403553e-18, + "velocityX": -0.21844311324879007, + "velocityY": -0.033456024297447116, + "timestamp": 0.07102309826367538 + }, + { + "x": 2.5533538161879417, + "y": 4.093682455972083, + "heading": 7.536168868149404e-19, + "angularVelocity": 8.051955440347105e-18, + "velocityX": -0.4383317241293747, + "velocityY": -0.055494530977205865, + "timestamp": 0.14204619652735076 + }, + { + "x": 2.5065333342312233, + "y": 4.089285667316947, + "heading": 1.9577689998101147e-18, + "angularVelocity": 1.6954372710601954e-17, + "velocityX": -0.6592289424265461, + "velocityY": -0.06190646089295108, + "timestamp": 0.21306929479102615 + }, + { + "x": 2.444057819633018, + "y": 4.086014007048202, + "heading": 7.236754163313542e-19, + "angularVelocity": -1.7375946775806794e-17, + "velocityX": -0.8796506506486734, + "velocityY": -0.046064735961910484, + "timestamp": 0.2840923930547015 + }, + { + "x": 2.366272803010291, + "y": 4.086201404386874, + "heading": -4.600695337702949e-20, + "angularVelocity": -1.0837071110321152e-17, + "velocityX": -1.0952073131726736, + "velocityY": 0.002638540742727494, + "timestamp": 0.3551154913183769 + }, + { + "x": 2.2743949408144704, + "y": 4.093298041709748, + "heading": -6.810145680022682e-19, + "angularVelocity": -8.940860384565438e-18, + "velocityX": -1.2936335423528253, + "velocityY": 0.09992013157018244, + "timestamp": 0.42613858958205225 + }, + { + "x": 2.1716878866524802, + "y": 4.111755773050512, + "heading": -4.3949372659187034e-18, + "angularVelocity": -5.229175761929786e-17, + "velocityX": -1.4461077687825412, + "velocityY": 0.2598834997657011, + "timestamp": 0.4971616878457276 + }, + { + "x": 2.064038363671175, + "y": 4.145110411818994, + "heading": -4.6915774186366566e-18, + "angularVelocity": -4.176671850418443e-18, + "velocityX": -1.5156973662543642, + "velocityY": 0.4696308607192342, + "timestamp": 0.568184786109403 + }, + { + "x": 1.9574928253627835, + "y": 4.1941215859148056, + "heading": -4.071164669637306e-18, + "angularVelocity": 8.735365335656254e-18, + "velocityX": -1.5001533432460301, + "velocityY": 0.6900737266289959, + "timestamp": 0.6392078843730784 + }, + { + "x": 1.8561614442574845, + "y": 4.2579367623514575, + "heading": -2.1843538176340424e-18, + "angularVelocity": 2.6566157957752987e-17, + "velocityX": -1.4267383933250315, + "velocityY": 0.8985129908021519, + "timestamp": 0.7102309826367538 }, { "x": 1.7625038623809814, "y": 4.335443496704102, - "heading": -6.141212604766239e-28, - "angularVelocity": 1.6999486763902178e-17, - "velocityX": -1.2829909964176864, - "velocityY": 1.0695501963786802, - "timestamp": 0.7263636569490929 - }, - { - "x": 1.6699865973975574, - "y": 4.438822812194518, - "heading": 4.608208814864715e-19, - "angularVelocity": 5.7050924640908856e-18, - "velocityX": -1.145389651626816, - "velocityY": 1.2798648790090887, - "timestamp": 0.8071372739032042 - }, - { - "x": 1.5897651108899586, - "y": 4.558355615986803, - "heading": -1.7820560110587166e-19, - "angularVelocity": -7.911325637921699e-18, - "velocityX": -0.9931644704308275, - "velocityY": 1.4798495882652127, - "timestamp": 0.8879108908573156 - }, - { - "x": 1.5236215839947922, - "y": 4.69251484598628, - "heading": 6.666171136643965e-20, - "angularVelocity": 3.031526464747949e-18, - "velocityX": -0.8188753876494026, - "velocityY": 1.6609288411115835, - "timestamp": 0.9686845078114269 - }, - { - "x": 1.4743062679980197, - "y": 4.83802892097306, - "heading": -2.1750975226542985e-20, - "angularVelocity": -1.0945727833584788e-18, - "velocityX": -0.6105374236932697, - "velocityY": 1.8015050021734174, - "timestamp": 1.0494581247655383 - }, - { - "x": 1.445007945742217, - "y": 4.986925449597659, - "heading": -3.492239943434842e-18, - "angularVelocity": -4.296562495216166e-17, - "velocityX": -0.36272143504929216, - "velocityY": 1.8433807255284014, - "timestamp": 1.1302317417196497 - }, - { - "x": 1.4332643201691235, - "y": 5.125626213958646, - "heading": -7.08378479723341e-18, - "angularVelocity": -4.4464331331999135e-17, - "velocityX": -0.14538937360284254, - "velocityY": 1.717154308444269, - "timestamp": 1.211005358673761 - }, - { - "x": 1.4313491306993646, - "y": 5.246564011836658, - "heading": -6.251112940318414e-18, - "angularVelocity": 1.0308710395039795e-17, - "velocityX": -0.023710582020729846, - "velocityY": 1.4972438085464903, - "timestamp": 1.2917789756278724 - }, - { - "x": 1.4341603888101957, - "y": 5.347758913467255, - "heading": -5.6476367392195124e-18, - "angularVelocity": 7.471203777908395e-18, - "velocityX": 0.034804162761333905, - "velocityY": 1.2528212236401832, - "timestamp": 1.3725525925819837 - }, - { - "x": 1.4388634248683685, - "y": 5.4287413805646505, - "heading": -3.3271667966009996e-18, - "angularVelocity": 2.8728066608973186e-17, - "velocityX": 0.058224903563684566, - "velocityY": 1.0025856232787074, - "timestamp": 1.453326209536095 - }, - { - "x": 1.44375686994876, - "y": 5.489423967756952, - "heading": -1.6082335369467641e-18, - "angularVelocity": 2.128087432366115e-17, - "velocityX": 0.06058222059667618, - "velocityY": 0.7512674246746076, - "timestamp": 1.5340998264902064 - }, - { - "x": 1.447728297610066, - "y": 5.529826730127948, - "heading": 1.5108104903999537e-18, - "angularVelocity": 3.861463726767174e-17, - "velocityX": 0.04916738670591753, - "velocityY": 0.5001975136814447, - "timestamp": 1.6148734434443177 + "heading": 8.000869341670443e-30, + "angularVelocity": 3.0755541014180874e-17, + "velocityX": -1.318691864562485, + "velocityY": 1.0912891192764178, + "timestamp": 0.7812540809004291 + }, + { + "x": 1.6678211787523816, + "y": 4.440113794273257, + "heading": 1.0337673199826826e-18, + "angularVelocity": 1.2792656650135126e-17, + "velocityX": -1.171678599002092, + "velocityY": 1.2952732528607998, + "timestamp": 0.8620635142288948 + }, + { + "x": 1.5861833542718362, + "y": 4.560362424925955, + "heading": 3.502553930956716e-18, + "angularVelocity": 3.055072217041457e-17, + "velocityX": -1.010251168928124, + "velocityY": 1.488051898145805, + "timestamp": 0.9428729475573605 + }, + { + "x": 1.5193061844335742, + "y": 4.694574430122835, + "heading": 3.349452477909152e-18, + "angularVelocity": -1.894598573168446e-18, + "velocityX": -0.8275911249967379, + "velocityY": 1.6608457660091864, + "timestamp": 1.0236823808858262 + }, + { + "x": 1.4697322615463966, + "y": 4.839437740666689, + "heading": 2.2003908510198267e-18, + "angularVelocity": -1.421939921772146e-17, + "velocityX": -0.6134670278607691, + "velocityY": 1.7926534635466616, + "timestamp": 1.1044918142142919 + }, + { + "x": 1.4402402800567775, + "y": 4.987394294503491, + "heading": 9.350345548651987e-19, + "angularVelocity": -1.5658521969411227e-17, + "velocityX": -0.3649571624857886, + "velocityY": 1.8309317086223476, + "timestamp": 1.1853012475427576 + }, + { + "x": 1.428656886409039, + "y": 5.125752153048063, + "heading": -1.0413562386131109e-20, + "angularVelocity": -1.1699724491291523e-17, + "velocityX": -0.14334209721081098, + "velocityY": 1.7121498424866368, + "timestamp": 1.2661106808712232 + }, + { + "x": 1.4274664912184116, + "y": 5.24665039601026, + "heading": 4.360728010528035e-19, + "angularVelocity": 5.525176154676822e-18, + "velocityX": -0.014730893925060436, + "velocityY": 1.496090715927136, + "timestamp": 1.346920114199689 + }, + { + "x": 1.4312682600903233, + "y": 5.3478526746786965, + "heading": 2.966726204203079e-19, + "angularVelocity": -1.7250483697434467e-18, + "velocityX": 0.0470461023606048, + "velocityY": 1.2523572372672604, + "timestamp": 1.4277295475281546 + }, + { + "x": 1.4369843193611713, + "y": 5.4288265506029205, + "heading": 6.544749795180092e-20, + "angularVelocity": -2.861363030181088e-18, + "velocityX": 0.07073504955260855, + "velocityY": 1.00203494306362, + "timestamp": 1.5085389808566203 + }, + { + "x": 1.4427588421957884, + "y": 5.48948173036258, + "heading": -2.7241750562008103e-19, + "angularVelocity": -4.181009397414832e-18, + "velocityX": 0.07145852404742657, + "velocityY": 0.7505952864828569, + "timestamp": 1.589348414185086 + }, + { + "x": 1.4473793665097352, + "y": 5.52985092603436, + "heading": -5.883333753074522e-19, + "angularVelocity": -3.909393399202573e-18, + "velocityX": 0.057178031364284086, + "velocityY": 0.4995604350769177, + "timestamp": 1.6701578475135517 }, { "x": 1.45, "y": 5.55, - "heading": 6.266703970621688e-28, - "angularVelocity": -1.8704256536060974e-17, - "velocityX": 0.028124311826708947, - "velocityY": 0.2497507309046547, - "timestamp": 1.695647060398429 + "heading": -1.4377702844560551e-28, + "angularVelocity": 7.280503460543119e-18, + "velocityX": 0.032429796654586486, + "velocityY": 0.249340616999313, + "timestamp": 1.7509672808420174 }, { "x": 1.45, "y": 5.55, - "heading": 3.044448816432048e-28, - "angularVelocity": -2.2012922013431035e-28, - "velocityX": 3.378728178111638e-19, - "velocityY": -4.966363669852347e-19, - "timestamp": 1.7764206773525404 + "heading": -7.062032350127263e-29, + "angularVelocity": 3.1387193567544073e-29, + "velocityX": 3.7137802104735376e-19, + "velocityY": -4.733660125526418e-19, + "timestamp": 1.831776714170483 } ], "trajectoryWaypoints": [ { "timestamp": 0, "isStopPoint": true, - "x": 2.5, + "x": 2.6, "y": 4.1, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 10 + "controlIntervalCount": 11 }, { - "timestamp": 0.7263636569490929, + "timestamp": 0.7812540809004291, "isStopPoint": false, "x": 1.7625038623809814, "y": 4.335443496704102, @@ -1222,7 +1267,7 @@ "controlIntervalCount": 13 }, { - "timestamp": 1.7764206773525404, + "timestamp": 1.831776714170483, "isStopPoint": true, "x": 1.45, "y": 5.55, @@ -1252,7 +1297,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": false + "isTrajectoryStale": true }, "MidNote": { "waypoints": [ diff --git a/src/main/deploy/choreo/ShootTop.traj b/src/main/deploy/choreo/ShootTop.traj index 97ad676..77ee333 100644 --- a/src/main/deploy/choreo/ShootTop.traj +++ b/src/main/deploy/choreo/ShootTop.traj @@ -235,6 +235,7 @@ "timestamp": 1.7900410246544713 }, { + "x": 1.4499999999999997, "x": 1.4499999999999997, "y": 5.55, "heading": 5.393496211799948e-29, diff --git a/src/main/java/frc/team5115/Constants.java b/src/main/java/frc/team5115/Constants.java index ffd7370..917f199 100644 --- a/src/main/java/frc/team5115/Constants.java +++ b/src/main/java/frc/team5115/Constants.java @@ -23,7 +23,7 @@ public static enum Mode { REPLAY } - public static final double CLOSE_SHOOT_DEGREES = 15.0; + public static final double CLOSE_SHOOT_DEGREES = 12.0; public static final byte INTAKE_MOTOR_ID = 32; public static final byte SNOWBLOWER_MOTOR_ID = 21; diff --git a/src/main/java/frc/team5115/commands/DriveCommands.java b/src/main/java/frc/team5115/commands/DriveCommands.java index 8b15a71..de07a0b 100644 --- a/src/main/java/frc/team5115/commands/DriveCommands.java +++ b/src/main/java/frc/team5115/commands/DriveCommands.java @@ -50,13 +50,13 @@ public static Command intakeUntilNote(Arm arm, Intake intake, Feeder feeder) { public static Command prepareAmp(Arm arm, Amper amper, Intake intake, Feeder feeder) { return Commands.sequence( - arm.goToAngle(Rotation2d.fromDegrees(98.0), 1), - amper.spinToAngle(new Rotation2d(3.25)), - intake.setSpeed(1), - feeder.setSpeeds(0.25), - Commands.waitSeconds(0.8), - feeder.stop(), - intake.setSpeed(-0.9)) + arm.goToAngle(Rotation2d.fromDegrees(98.0), 1), amper.spinToAngle(new Rotation2d(3.25)) + // ,intake.setSpeed(1), + // feeder.setSpeeds(0.25), + // Commands.waitSeconds(0.8), + // feeder.stop(), + // intake.setSpeed(-0.9) + ) .withInterruptBehavior(InterruptionBehavior.kCancelSelf); } @@ -70,7 +70,7 @@ public static Command triggerAmp(Arm arm, Amper amper, Intake intake, Feeder fee intake.stop(), feeder.stop(), Commands.waitSeconds(0.5), - amper.spinToAngle(new Rotation2d(0.2)).alongWith(arm.stow())) + amper.spinToAngle(new Rotation2d(0.2)).alongWith(arm.stow()).withTimeout(1.0)) .withInterruptBehavior(InterruptionBehavior.kCancelIncoming); }