diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 2aaab015..4d7f432f 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -5,7 +5,7 @@ "pathFolders": [], "autoFolders": [], "defaultMaxVel": 3.0, - "defaultMaxAccel": 3.0, + "defaultMaxAccel": 5.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "maxModuleSpeed": 4.5 diff --git a/.vscode/settings.json b/.vscode/settings.json index 2f1b9b39..6e590aac 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -26,5 +26,6 @@ }, ], "java.test.defaultConfig": "WPIlibUnitTests", - "java.compile.nullAnalysis.mode": "automatic" + "java.compile.nullAnalysis.mode": "automatic", + "wpilib.autoStartRioLog": true } diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index 6c78d01a..6534dffe 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,6 +1,6 @@ { "enableCppIntellisense": false, "currentLanguage": "java", - "projectYear": "2024beta", + "projectYear": "2024", "teamNumber": 8033 } \ No newline at end of file diff --git a/build.gradle b/build.gradle index 92b9b226..c908edc6 100644 --- a/build.gradle +++ b/build.gradle @@ -2,7 +2,7 @@ plugins { id "java" id "edu.wpi.first.GradleRIO" version "2024.1.1" id "com.peterabeles.gversion" version "1.10" - id "com.diffplug.spotless" version "6.12.0" + id "com.diffplug.spotless" version "6.20.0" } java { @@ -115,7 +115,11 @@ wpi.sim.addDriverstation() // in order to make them all available at runtime. Also adding the manifest so WPILib // knows where to look for our Robot Class. jar { - from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } } + from { + configurations.runtimeClasspath.collect { + it.isDirectory() ? it : zipTree(it) + } + } from sourceSets.main.allSource manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) duplicatesStrategy = DuplicatesStrategy.INCLUDE diff --git a/choreo.chor b/choreo.chor index eff8a597..6d6f1b3a 100644 --- a/choreo.chor +++ b/choreo.chor @@ -1,15 +1,15 @@ { - "version": "v0.2", + "version": "v0.2.1", "robotConfiguration": { - "mass": 37.04398850154597, - "rotationalInertia": 2.8, - "motorMaxTorque": 1.1526315789473685, - "motorMaxVelocity": 4864, - "gearing": 6.86, - "wheelbase": 0.5206997188221518, - "trackWidth": 0.5206997188221518, - "bumperLength": 0.8508995405142481, - "bumperWidth": 0.8508995405142481, + "mass": 74.08797700309194, + "rotationalInertia": 6, + "motorMaxTorque": 1.162295081967213, + "motorMaxVelocity": 4800, + "gearing": 6.75, + "wheelbase": 0.5778496879611685, + "trackWidth": 0.5778496879611685, + "bumperLength": 0.8762995267982555, + "bumperWidth": 0.8762995267982555, "wheelRadius": 0.050799972568014815 }, "paths": { @@ -22,16 +22,25 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 11 + "controlIntervalCount": 14 + }, + { + "x": 2.1557445526123047, + "y": 6.919295787811279, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 6 }, { - "x": 2.2610747814178467, - "y": 6.9050798416137695, + "x": 2.7185182571411133, + "y": 6.8797807693481445, "heading": 0.15617120010566338, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 8 + "controlIntervalCount": 11 }, { "x": 1.3818649053573608, @@ -40,16 +49,16 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 6 + "controlIntervalCount": 9 }, { - "x": 2.3054046630859375, - "y": 5.538240909576416, + "x": 2.59291672706604, + "y": 5.461223602294922, "heading": -0.020267736871838693, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 5 + "controlIntervalCount": 8 }, { "x": 1.3892531394958496, @@ -58,21 +67,21 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 10 + "controlIntervalCount": 13 }, { - "x": 2.4143476486206055, - "y": 4.51963472366333, + "x": 2.764192819595337, + "y": 4.415045738220215, "heading": -0.6391760227970993, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 9 + "controlIntervalCount": 12 }, { - "x": 0.7907993793487549, - "y": 4.415216445922852, - "heading": -1.0048134836944498, + "x": 0.7942578792572021, + "y": 4.357780456542969, + "heading": -0.9994586215071235, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -84,451 +93,667 @@ "x": 0.7470832467079163, "y": 6.625458717346191, "heading": 1.056495497952334, - "angularVelocity": 1.8216849722605286e-31, - "velocityX": 0, - "velocityY": 0, + "angularVelocity": -4.3186177103351096e-19, + "velocityX": 7.079282317781465e-18, + "velocityY": -1.220945161386867e-18, "timestamp": 0 }, { - "x": 0.7875510590073889, - "y": 6.647513625582565, - "heading": 0.9955054911508737, - "angularVelocity": -1.1259767405457384, - "velocityX": 0.7471029727592392, - "velocityY": 0.4071702069138493, - "timestamp": 0.05416631143899068 - }, - { - "x": 0.8691098883744304, - "y": 6.691702168377954, - "heading": 0.8785332104893699, - "angularVelocity": -2.159502420489772, - "velocityX": 1.505711339767041, - "velocityY": 0.8157938323926509, - "timestamp": 0.10833262287798136 - }, - { - "x": 0.9931586796399057, - "y": 6.758536729383142, - "heading": 0.7242571551382772, - "angularVelocity": -2.848192008142514, - "velocityX": 2.290146549949145, - "velocityY": 1.2338769103830776, - "timestamp": 0.16249893431697204 - }, - { - "x": 1.1542844409320208, - "y": 6.847841004747936, - "heading": 0.6582721283096681, - "angularVelocity": -1.2181930996525292, - "velocityX": 2.974648947133801, - "velocityY": 1.6487051267166433, - "timestamp": 0.21666524575596272 - }, - { - "x": 1.3362429767095947, - "y": 6.9407596540671515, - "heading": 0.6582720385538592, - "angularVelocity": -0.0000016570411839532803, - "velocityX": 3.3592565368332274, - "velocityY": 1.7154324680917685, - "timestamp": 0.2708315571949534 - }, - { - "x": 1.5182021069161247, - "y": 7.033677139333042, - "heading": 0.6582719487984567, - "angularVelocity": -0.0000016570336808247504, - "velocityX": 3.359267510978228, - "velocityY": 1.7154109777356332, - "timestamp": 0.3249978686339441 - }, - { - "x": 1.7157951910460691, - "y": 7.085636206134739, - "heading": 0.6582717690726799, - "angularVelocity": -0.0000033180361009936695, - "velocityX": 3.647896245482769, - "velocityY": 0.9592506009979895, - "timestamp": 0.3791641800729348 - }, - { - "x": 1.8967940454930174, - "y": 7.095126843263903, - "heading": 0.5939474144774982, - "angularVelocity": -1.1875343342814295, - "velocityX": 3.3415392268460664, - "velocityY": 0.1752129114394957, - "timestamp": 0.4333304915119255 - }, - { - "x": 2.0516314923077155, - "y": 7.067103472452243, - "heading": 0.46543090394591263, - "angularVelocity": -2.3726280619336353, - "velocityX": 2.8585562262089095, - "velocityY": -0.5173579309202905, - "timestamp": 0.4874968029509162 - }, - { - "x": 2.1749652200442315, - "y": 7.00220729212556, - "heading": 0.30618928904490067, - "angularVelocity": -2.9398644779490204, - "velocityX": 2.2769452905322995, - "velocityY": -1.198091186249191, - "timestamp": 0.5416631143899069 - }, - { - "x": 2.2610747814178467, - "y": 6.9050798416137695, + "x": 0.7613714426488418, + "y": 6.628614589829769, + "heading": 1.0345345122436784, + "angularVelocity": -0.5024975798156311, + "velocityX": 0.32693358920665033, + "velocityY": 0.07221070612415084, + "timestamp": 0.04370366463598273 + }, + { + "x": 0.7899910246485671, + "y": 6.635101738465351, + "heading": 0.9912579578255775, + "angularVelocity": -0.9902271303461743, + "velocityX": 0.6548554277565509, + "velocityY": 0.14843488960512216, + "timestamp": 0.08740732927196546 + }, + { + "x": 0.8329823079677272, + "y": 6.6450825712167525, + "heading": 0.9272173132517384, + "angularVelocity": -1.465338092520316, + "velocityX": 0.9836997349591596, + "velocityY": 0.22837519083431007, + "timestamp": 0.13111099390794817 + }, + { + "x": 0.8904118185530745, + "y": 6.658650997634064, + "heading": 0.8429907149485981, + "angularVelocity": -1.927220497518482, + "velocityX": 1.3140662473888705, + "velocityY": 0.3104642718253751, + "timestamp": 0.1748146585439309 + }, + { + "x": 0.962426123217969, + "y": 6.675814256462545, + "heading": 0.7397363294626891, + "angularVelocity": -2.362602457847324, + "velocityX": 1.6477864102408268, + "velocityY": 0.39271898527129734, + "timestamp": 0.21851832317991365 + }, + { + "x": 1.0493031568120121, + "y": 6.696537442602017, + "heading": 0.6202474925482772, + "angularVelocity": -2.7340690514092785, + "velocityX": 1.9878661049974005, + "velocityY": 0.47417502198223827, + "timestamp": 0.2622219878158964 + }, + { + "x": 1.1514382857503924, + "y": 6.720845269246716, + "heading": 0.4905752358986126, + "angularVelocity": -2.9670797112720706, + "velocityX": 2.3369923275104343, + "velocityY": 0.5561965305921692, + "timestamp": 0.30592565245187914 + }, + { + "x": 1.269055326004296, + "y": 6.748845004507445, + "heading": 0.3629054511567606, + "angularVelocity": -2.921260397846258, + "velocityX": 2.6912397675014597, + "velocityY": 0.6406724812196789, + "timestamp": 0.3496293170878619 + }, + { + "x": 1.400011708453663, + "y": 6.779996215891522, + "heading": 0.2672618294840015, + "angularVelocity": -2.188457706450791, + "velocityX": 2.996462277022566, + "velocityY": 0.7127825925706782, + "timestamp": 0.3933329817238446 + }, + { + "x": 1.5437969816492523, + "y": 6.812790254409381, + "heading": 0.2092081541239441, + "angularVelocity": -1.3283479965261176, + "velocityX": 3.2900049548066255, + "velocityY": 0.7503727385565244, + "timestamp": 0.43703664635982736 + }, + { + "x": 1.6999373892672376, + "y": 6.847694256712915, + "heading": 0.1915003494540967, + "angularVelocity": -0.40517894362723794, + "velocityX": 3.572707435829763, + "velocityY": 0.7986516140981349, + "timestamp": 0.4807403109958101 + }, + { + "x": 1.8624433107711578, + "y": 6.878112633593708, + "heading": 0.1915002067487222, + "angularVelocity": -0.0000032652953862886492, + "velocityX": 3.7183591549465618, + "velocityY": 0.6960143304721129, + "timestamp": 0.5244439756317928 + }, + { + "x": 2.015535426159542, + "y": 6.900266962585095, + "heading": 0.1561712001056634, + "angularVelocity": -0.8083762983569384, + "velocityX": 3.502958313988489, + "velocityY": 0.5069215402397582, + "timestamp": 0.5681476402677755 + }, + { + "x": 2.1557445526123047, + "y": 6.919295787811279, + "heading": 0.15617120010566338, + "angularVelocity": 8.052388519770604e-18, + "velocityX": 3.208177795171024, + "velocityY": 0.4354057121910402, + "timestamp": 0.6118513049037583 + }, + { + "x": 2.2979527331593355, + "y": 6.936286453257653, "heading": 0.15617120010566338, - "angularVelocity": -2.7695828819396024, - "velocityX": 1.5897254047029348, - "velocityY": -1.7931339227554435, - "timestamp": 0.5958294258288975 - }, - { - "x": 2.3021420961741277, - "y": 6.690129168178249, - "heading": -0.009663328888242836, - "angularVelocity": -2.0165575251161725, - "velocityX": 0.4993809377969598, - "velocityY": -2.6138127003762723, - "timestamp": 0.6780658742953757 - }, - { - "x": 2.248466001407306, - "y": 6.41546750147008, - "heading": -0.10873685342712126, - "angularVelocity": -1.204739825057738, - "velocityX": -0.6527044366306908, - "velocityY": -3.3399018541045957, - "timestamp": 0.7603023227618538 - }, - { - "x": 2.085165640858422, - "y": 6.151744237063873, - "heading": -0.10873683701731249, - "angularVelocity": 1.9954422992985155e-7, - "velocityX": -1.9857418893211336, - "velocityY": -3.2068902454330503, - "timestamp": 0.842538771228332 - }, - { - "x": 1.8574534262453974, - "y": 5.941116320042222, - "heading": -0.10873681487910887, - "angularVelocity": 2.692018447413834e-7, - "velocityX": -2.7689937838918954, - "velocityY": -2.5612477307736583, - "timestamp": 0.9247752196948101 - }, - { - "x": 1.629741023592729, - "y": 5.730488606312599, - "heading": -0.10873679274080142, - "angularVelocity": 2.6920310704244503e-7, - "velocityX": -2.76899607046491, - "velocityY": -2.5612452587307564, - "timestamp": 1.0070116681612884 - }, - { - "x": 1.4644902813281564, - "y": 5.577695792451846, - "heading": 0.0012781207724603802, - "angularVelocity": 1.337787751839788, - "velocityX": -2.009458644507651, - "velocityY": -1.8579695099921079, - "timestamp": 1.0892481166277665 + "angularVelocity": -1.1282442958129174e-19, + "velocityX": 2.796432452004192, + "velocityY": 0.33411051356290106, + "timestamp": 0.6627047344028709 + }, + { + "x": 2.4202451824897016, + "y": 6.945011127796279, + "heading": 0.15617120010566338, + "angularVelocity": -1.1282442959053082e-19, + "velocityX": 2.404802400445729, + "velocityY": 0.1715651161497845, + "timestamp": 0.7135581639019836 + }, + { + "x": 2.52317206188046, + "y": 6.94425224667938, + "heading": 0.15617120010566338, + "angularVelocity": -1.128244295905312e-19, + "velocityX": 2.0239909167296966, + "velocityY": -0.014922909317535413, + "timestamp": 0.7644115934010962 + }, + { + "x": 2.6070605614479327, + "y": 6.933368973549914, + "heading": 0.15617120010566338, + "angularVelocity": -1.1282442959052985e-19, + "velocityX": 1.6496134163171314, + "velocityY": -0.21401256978465483, + "timestamp": 0.8152650229002089 + }, + { + "x": 2.672125506615784, + "y": 6.911967284292252, + "heading": 0.15617120010566338, + "angularVelocity": -1.1282442956919517e-19, + "velocityX": 1.2794603197604928, + "velocityY": -0.4208504611874918, + "timestamp": 0.8661184523993215 + }, + { + "x": 2.7185182571411133, + "y": 6.8797807693481445, + "heading": 0.15617120010566338, + "angularVelocity": -1.0748383020939867e-19, + "velocityX": 0.9122836155256804, + "velocityY": -0.6329271252919907, + "timestamp": 0.9169718818984341 + }, + { + "x": 2.742166614394153, + "y": 6.787220144570856, + "heading": 0.15109105455160673, + "angularVelocity": -0.05590353076450862, + "velocityX": 0.26023401360416243, + "velocityY": -1.0185664327447723, + "timestamp": 1.0078453112611694 + }, + { + "x": 2.7063747461891747, + "y": 6.659930157895132, + "heading": 0.14109258501335736, + "angularVelocity": -0.11002632571879596, + "velocityX": -0.39386505446064113, + "velocityY": -1.4007393312694816, + "timestamp": 1.0987187406239047 + }, + { + "x": 2.6108392240840024, + "y": 6.498432494186991, + "heading": 0.12644294397632044, + "angularVelocity": -0.16120929010568202, + "velocityX": -1.0513031452112058, + "velocityY": -1.7771714442897992, + "timestamp": 1.18959216998664 + }, + { + "x": 2.4549786573639274, + "y": 6.303757740845773, + "heading": 0.10766682633359297, + "angularVelocity": -0.20661834569682533, + "velocityX": -1.715139043536396, + "velocityY": -2.1422626471390678, + "timestamp": 1.2804655993493754 + }, + { + "x": 2.237238983313492, + "y": 6.078890290802275, + "heading": 0.08626225710840621, + "angularVelocity": -0.23554265944719108, + "velocityX": -2.396076340217028, + "velocityY": -2.4745126448998374, + "timestamp": 1.3713390287121108 + }, + { + "x": 1.9563226558789764, + "y": 5.880741504295724, + "heading": 0.08626219926626796, + "angularVelocity": -6.365132100594054e-7, + "velocityX": -3.0912922446581543, + "velocityY": -2.1804920084573034, + "timestamp": 1.4622124580748461 + }, + { + "x": 1.7274737983405954, + "y": 5.727569361833653, + "heading": 0.0766627418429575, + "angularVelocity": -0.10563547002277199, + "velocityX": -2.5183253140463755, + "velocityY": -1.6855547714685755, + "timestamp": 1.5530858874375815 + }, + { + "x": 1.5549556420919104, + "y": 5.613995417389302, + "heading": 0.06722021492045382, + "angularVelocity": -0.10390855708563887, + "velocityX": -1.8984444348419232, + "velocityY": -1.2498036581298453, + "timestamp": 1.6439593168003168 + }, + { + "x": 1.4396298957459885, + "y": 5.538759046250953, + "heading": 0.06010941807852643, + "angularVelocity": -0.07824946072569235, + "velocityX": -1.269081041121286, + "velocityY": -0.8279248584097305, + "timestamp": 1.7348327461630522 }, { "x": 1.3818649053573608, "y": 5.5012993812561035, "heading": 0.05628563945524819, - "angularVelocity": 0.6688946289455666, - "velocityX": -1.0047293810903806, - "velocityY": -0.9289848068631428, - "timestamp": 1.1714845650942447 + "angularVelocity": -0.0420780711159847, + "velocityX": -0.6356642507464983, + "velocityY": -0.4122180185951169, + "timestamp": 1.8257061755257875 }, { "x": 1.3818649053573608, "y": 5.5012993812561035, "heading": 0.05628563945524819, - "angularVelocity": 8.108539434054397e-29, - "velocityX": -3.383351472481295e-31, - "velocityY": 2.5568610146599268e-30, - "timestamp": 1.2537210135607229 - }, - { - "x": 1.4659695573316451, - "y": 5.505361506655373, - "heading": 0.050154371824367786, - "angularVelocity": -0.0866134750258778, - "velocityX": 1.188106051128092, - "velocityY": 0.05738369583633677, - "timestamp": 1.3245098559325195 - }, - { - "x": 1.6341788550204006, - "y": 5.5134857557639885, - "heading": 0.03789188378734141, - "angularVelocity": -0.17322628293059847, - "velocityX": 2.3762120138268115, - "velocityY": 0.11476736779992304, - "timestamp": 1.395298698304316 - }, - { - "x": 1.8864927796012139, - "y": 5.525672125057411, - "heading": 0.019498324963128963, - "angularVelocity": -0.25983697724008453, - "velocityX": 3.5643177106303305, - "velocityY": 0.1721509899740557, - "timestamp": 1.4660875406761127 - }, - { - "x": 2.1102347064408926, - "y": 5.533923842143074, - "heading": 0.00011174721946555298, - "angularVelocity": -0.2738648789005671, - "velocityX": 3.160694812108141, - "velocityY": 0.11656804673148825, - "timestamp": 1.5368763830479093 - }, - { - "x": 2.249872007587195, - "y": 5.538113436672261, - "heading": -0.013143603106743272, - "angularVelocity": -0.18725197195045482, - "velocityX": 1.9725891322378306, - "velocityY": 0.0591843910539215, - "timestamp": 1.607665225419706 - }, - { - "x": 2.3054046630859375, - "y": 5.538240909576416, + "angularVelocity": 1.3994093358769211e-21, + "velocityX": -9.902038153093951e-18, + "velocityY": -5.706702901855187e-18, + "timestamp": 1.9165796048885229 + }, + { + "x": 1.4353793564287938, + "y": 5.499850139119754, + "heading": 0.05330774673729568, + "angularVelocity": -0.037162247354032396, + "velocityX": 0.6678270361261427, + "velocityY": -0.018085639694878305, + "timestamp": 1.9967118070554994 + }, + { + "x": 1.5424082559070336, + "y": 5.49695165466464, + "heading": 0.04735195633823623, + "angularVelocity": -0.07432455664514218, + "velocityX": 1.335654038999412, + "velocityY": -0.036171281666197166, + "timestamp": 2.076844009222476 + }, + { + "x": 1.7029515984698416, + "y": 5.492603927569323, + "heading": 0.03841826071733368, + "angularVelocity": -0.11148696003994531, + "velocityX": 2.003480975454455, + "velocityY": -0.05425692764886357, + "timestamp": 2.156976211389453 + }, + { + "x": 1.9170093681982305, + "y": 5.486806957092212, + "heading": 0.026506650941055532, + "angularVelocity": -0.1486494749196728, + "velocityX": 2.671307713250448, + "velocityY": -0.07234258288609449, + "timestamp": 2.2371084135564296 + }, + { + "x": 2.1592197171124052, + "y": 5.478791804392635, + "heading": 0.011195918948050757, + "angularVelocity": -0.19106840419911095, + "velocityX": 3.0226343762457084, + "velocityY": -0.10002411618334738, + "timestamp": 2.3172406157234064 + }, + { + "x": 2.3479156387807323, + "y": 5.472225891398255, + "heading": -0.0011368905306301479, + "angularVelocity": -0.1539057850049126, + "velocityX": 2.3548076374480407, + "velocityY": -0.08193850682774677, + "timestamp": 2.3973728178903833 + }, + { + "x": 2.483097117399865, + "y": 5.467109219705175, + "heading": -0.010491771187470106, + "angularVelocity": -0.11674308709682761, + "velocityX": 1.6869807014344356, + "velocityY": -0.06385287755374197, + "timestamp": 2.47750502005736 + }, + { + "x": 2.564764147705788, + "y": 5.463441789903776, + "heading": -0.01686872035025771, + "angularVelocity": -0.0795803558412077, + "velocityX": 1.0191536997292066, + "velocityY": -0.04576724091218098, + "timestamp": 2.557637222224337 + }, + { + "x": 2.59291672706604, + "y": 5.461223602294922, "heading": -0.020267736871838693, - "angularVelocity": -0.10063921836266371, - "velocityX": 0.784483170484331, - "velocityY": 0.0018007485344283746, - "timestamp": 1.6784540677915025 + "angularVelocity": -0.042417610269816795, + "velocityX": 0.3513266651724994, + "velocityY": -0.027681600516002228, + "timestamp": 2.6377694243913137 + }, + { + "x": 2.5441490516673415, + "y": 5.460719866140607, + "heading": -0.019850376221801108, + "angularVelocity": 0.004155972098076838, + "velocityX": -0.48561621280493705, + "velocityY": -0.0050160775864576445, + "timestamp": 2.7381937391336377 + }, + { + "x": 2.4113319639164366, + "y": 5.462492299555472, + "heading": -0.014755896628636694, + "angularVelocity": 0.050729543001973076, + "velocityX": -1.3225590644227596, + "velocityY": 0.017649444951779485, + "timestamp": 2.8386180538759618 + }, + { + "x": 2.194465469101637, + "y": 5.466540902467684, + "heading": -0.004984302193896615, + "angularVelocity": 0.09730307306365722, + "velocityX": -2.1595018633809024, + "velocityY": 0.040314966774733244, + "timestamp": 2.939042368618286 }, { - "x": 2.206836259942782, - "y": 5.529745448091516, - "heading": -0.017578724891835627, - "angularVelocity": 0.025983583153922206, - "velocityX": -0.9524540308729255, - "velocityY": -0.08209057139403513, - "timestamp": 1.7819429501397848 + "x": 1.8935495831026135, + "y": 5.472865675007542, + "heading": 0.009464396467489124, + "angularVelocity": 0.14387649742454536, + "velocityX": -2.9964445042133008, + "velocityY": 0.0629804898951702, + "timestamp": 3.03946668336061 }, { - "x": 1.9285141868544358, - "y": 5.512568166846942, - "heading": -0.0017855827044546864, - "angularVelocity": 0.15260713836130316, - "velocityX": -2.689391041558251, - "velocityY": -0.1659818992610695, - "timestamp": 1.885431832488067 + "x": 1.641401371888256, + "y": 5.479694173969964, + "heading": 0.023495626181635927, + "angularVelocity": 0.13971944693024915, + "velocityX": -2.510828297522737, + "velocityY": 0.06799647057530608, + "timestamp": 3.139890998102934 }, { - "x": 1.5690068282385219, - "y": 5.495204505355553, - "heading": 0.024422680398419737, - "angularVelocity": 0.25324713638971313, - "velocityX": -3.4738742023131053, - "velocityY": -0.16778286804715686, - "timestamp": 1.9889207148363492 + "x": 1.473302552055367, + "y": 5.484246507552045, + "heading": 0.03284977683623274, + "angularVelocity": 0.09314627317695277, + "velocityX": -1.6738856547262355, + "velocityY": 0.04533098974846174, + "timestamp": 3.240315312845258 }, { "x": 1.3892531394958496, "y": 5.486522674560547, "heading": 0.03752685285960867, - "angularVelocity": 0.12662396350062077, - "velocityX": -1.7369371923230166, - "velocityY": -0.08389143450007014, - "timestamp": 2.0924095971846315 + "angularVelocity": 0.04657314351984077, + "velocityX": -0.8369428536821739, + "velocityY": 0.02266549703966028, + "timestamp": 3.340739627587582 }, { "x": 1.3892531394958496, "y": 5.486522674560547, "heading": 0.03752685285960867, - "angularVelocity": 2.746210792067846e-31, - "velocityX": -7.082594657438039e-35, - "velocityY": 0, - "timestamp": 2.195898479532914 - }, - { - "x": 1.4239336560949027, - "y": 5.45678487802077, - "heading": -0.004824083437490689, - "angularVelocity": -0.7980299015185681, - "velocityX": 0.6534941530454559, - "velocityY": -0.5603571707963146, - "timestamp": 2.248967839736724 - }, - { - "x": 1.4936387312915242, - "y": 5.397060158439441, - "heading": -0.08394241765705696, - "angularVelocity": -1.4908477116686103, - "velocityX": 1.3134711805253199, - "velocityY": -1.1254086982009737, - "timestamp": 2.3020371999405342 - }, - { - "x": 1.5989756629525658, - "y": 5.306833356180067, - "heading": -0.18501626934212806, - "angularVelocity": -1.9045613381601734, - "velocityX": 1.9848916824416323, - "velocityY": -1.7001675149815771, - "timestamp": 2.3551065601443444 - }, - { - "x": 1.7389822594115079, - "y": 5.188268009617622, - "heading": -0.23253583212093973, - "angularVelocity": -0.8954236982755346, - "velocityX": 2.6381813521258652, - "velocityY": -2.234158205546495, - "timestamp": 2.4081759203481545 - }, - { - "x": 1.8903539316186226, - "y": 5.05728781912084, - "heading": -0.23253589895082247, - "angularVelocity": -0.0000012592931680036756, - "velocityX": 2.85233648240303, - "velocityY": -2.4680943955939854, - "timestamp": 2.4612452805519647 - }, - { - "x": 2.041725559967012, - "y": 4.926307577937187, - "heading": -0.23253596578056132, - "angularVelocity": -0.000001259290456718511, - "velocityX": 2.8523356559614563, - "velocityY": -2.468095350700103, - "timestamp": 2.514314640755775 - }, - { - "x": 2.186666927649336, - "y": 4.788245829195716, - "heading": -0.23253707199818868, - "angularVelocity": -0.000020844751532055486, - "velocityX": 2.731168552356477, - "velocityY": -2.601534071849594, - "timestamp": 2.567384000959585 - }, - { - "x": 2.299267843418341, - "y": 4.670864736575426, - "heading": -0.33873841006148575, - "angularVelocity": -2.0011799210587196, - "velocityX": 2.1217688575209404, - "velocityY": -2.2118429950821876, - "timestamp": 2.620453361163395 - }, - { - "x": 2.37528105951498, - "y": 4.58047733131235, - "heading": -0.49175658993248267, - "angularVelocity": -2.883362062088883, - "velocityX": 1.4323371490576546, - "velocityY": -1.7031937998865565, - "timestamp": 2.6735227213672053 - }, - { - "x": 2.4143476486206055, - "y": 4.51963472366333, + "angularVelocity": -2.6599524768911785e-21, + "velocityX": -1.3251439086588674e-20, + "velocityY": 5.237013291945624e-20, + "timestamp": 3.441163942329906 + }, + { + "x": 1.4165016264324646, + "y": 5.468726018878958, + "heading": 0.027669365250431073, + "angularVelocity": -0.15749827520188686, + "velocityX": 0.4353634378786766, + "velocityY": -0.2843465480598295, + "timestamp": 3.503751850651708 + }, + { + "x": 1.4710112662474708, + "y": 5.4331263299672, + "heading": 0.008563925885950884, + "angularVelocity": -0.3052576747931523, + "velocityX": 0.8709292461850596, + "velocityY": -0.5687949935747745, + "timestamp": 3.56633975897351 + }, + { + "x": 1.5527987232870373, + "y": 5.379715427763577, + "heading": -0.018924667024813987, + "angularVelocity": -0.4391997375823718, + "velocityX": 1.306761309533597, + "velocityY": -0.8533741362469829, + "timestamp": 3.628927667295312 + }, + { + "x": 1.6618865667976872, + "y": 5.308482777774649, + "heading": -0.05347261278820921, + "angularVelocity": -0.5519907389421553, + "velocityX": 1.7429539736296065, + "velocityY": -1.1381215940733804, + "timestamp": 3.691515575617114 + }, + { + "x": 1.7983053814757264, + "y": 5.219415946962974, + "heading": -0.09280768567650903, + "angularVelocity": -0.6284771922086734, + "velocityX": 2.1796353055390356, + "velocityY": -1.4230677010921848, + "timestamp": 3.754103483938916 + }, + { + "x": 1.9620858254779319, + "y": 5.112511494418258, + "heading": -0.13216035126284406, + "angularVelocity": -0.6287582800179152, + "velocityX": 2.6168064789785364, + "velocityY": -1.708068785348386, + "timestamp": 3.8166913922607177 + }, + { + "x": 2.1529952387272515, + "y": 4.987995425318902, + "heading": -0.15523721329619666, + "angularVelocity": -0.3687111880253399, + "velocityX": 3.0502603197368576, + "velocityY": -1.9894588657468077, + "timestamp": 3.8792793005825197 + }, + { + "x": 2.323053130467604, + "y": 4.850242258645187, + "heading": -0.20104196583241465, + "angularVelocity": -0.7318466739726868, + "velocityX": 2.7171045701988246, + "velocityY": -2.200954950682231, + "timestamp": 3.9418672089043216 + }, + { + "x": 2.466345532790285, + "y": 4.728187893870888, + "heading": -0.2844490729613583, + "angularVelocity": -1.3326393126943619, + "velocityX": 2.2894582382579274, + "velocityY": -1.9501269182338685, + "timestamp": 4.004455117226123 + }, + { + "x": 2.582053624940948, + "y": 4.6234869751422165, + "heading": -0.37568976157943984, + "angularVelocity": -1.4578005730589176, + "velocityX": 1.848729175542, + "velocityY": -1.6728617641340675, + "timestamp": 4.067043025547925 + }, + { + "x": 2.670225613669648, + "y": 4.536352850675129, + "heading": -0.4673001281858147, + "angularVelocity": -1.463707113127209, + "velocityX": 1.4087703374804497, + "velocityY": -1.3921878331366975, + "timestamp": 4.129630933869726 + }, + { + "x": 2.730923003974787, + "y": 4.4668604060655035, + "heading": -0.555803694848379, + "angularVelocity": -1.4140681328974087, + "velocityX": 0.9697941971963301, + "velocityY": -1.1103174155033886, + "timestamp": 4.192218842191528 + }, + { + "x": 2.764192819595337, + "y": 4.415045738220215, "heading": -0.6391760227970993, - "angularVelocity": -2.777863390447102, - "velocityX": 0.7361420781330721, - "velocityY": -1.146473358927959, - "timestamp": 2.7265920815710154 - }, - { - "x": 2.3701336666128094, - "y": 4.4395971700859445, - "heading": -0.8245153775471664, - "angularVelocity": -2.372308285003151, - "velocityX": -0.5659305114745719, - "velocityY": -1.0244653744427692, - "timestamp": 2.8047182492034923 - }, - { - "x": 2.2242072743116044, - "y": 4.369198833257615, - "heading": -0.9780363443258114, - "angularVelocity": -1.9650390058915468, - "velocityX": -1.8678299054380199, - "velocityY": -0.901085244056695, - "timestamp": 2.8828444168359693 - }, - { - "x": 1.9766118523863694, - "y": 4.308771886473381, - "heading": -1.099290083952399, - "angularVelocity": -1.5520246711313, - "velocityX": -3.169174035132216, - "velocityY": -0.7734533590395138, - "timestamp": 2.960970584468446 - }, - { - "x": 1.6829782303645726, - "y": 4.333642341587589, - "heading": -1.099290070338212, - "angularVelocity": 1.7425898802224296e-7, - "velocityX": -3.758454189166382, - "velocityY": 0.31833706769275627, - "timestamp": 3.039096752100923 - }, - { - "x": 1.3895177021759386, - "y": 4.36047817582874, - "heading": -1.0992900564673602, - "angularVelocity": 1.7754424736252278e-7, - "velocityX": -3.7562386212150933, - "velocityY": 0.34349354453672953, - "timestamp": 3.1172229197334 - }, - { - "x": 1.0960571742019716, - "y": 4.38731401241607, - "heading": -1.0992900425963372, - "angularVelocity": 1.7754644295782002e-7, - "velocityX": -3.756238618467393, - "velocityY": 0.34349357456735763, - "timestamp": 3.195349087365877 - }, - { - "x": 0.8925519822862651, - "y": 4.405915634394149, - "heading": -1.0363057213411215, - "angularVelocity": 0.8061872630368462, - "velocityX": -2.6048275255614968, - "velocityY": 0.23809720279105512, - "timestamp": 3.273475254998354 - }, - { - "x": 0.7907993793487549, - "y": 4.415216445922852, - "heading": -1.0048134836944498, - "angularVelocity": 0.4030946173477082, - "velocityX": -1.302413852119022, - "velocityY": 0.11904860830311666, - "timestamp": 3.351601422630831 - }, - { - "x": 0.7907993793487549, - "y": 4.415216445922852, - "heading": -1.0048134836944498, - "angularVelocity": -9.422070435540628e-32, - "velocityX": 1.9369819218763315e-32, - "velocityY": -1.162688749272308e-31, - "timestamp": 3.4297275902633078 + "angularVelocity": -1.3320836274005747, + "velocityX": 0.5315693799749596, + "velocityY": -0.8278702585630189, + "timestamp": 4.254806750513329 + }, + { + "x": 2.7480635723388045, + "y": 4.353298917368432, + "heading": -0.7461336511354502, + "angularVelocity": -1.226587505939294, + "velocityX": -0.18496981909961455, + "velocityY": -0.7081110544885852, + "timestamp": 4.3420060965204 + }, + { + "x": 2.669473842343889, + "y": 4.302035156263182, + "heading": -0.8427126143522478, + "angularVelocity": -1.1075652242731957, + "velocityX": -0.9012651309167219, + "velocityY": -0.5878915777773497, + "timestamp": 4.429205442527471 + }, + { + "x": 2.528459101717774, + "y": 4.261314542628117, + "heading": -0.927127909191312, + "angularVelocity": -0.9680725682531954, + "velocityX": -1.6171536494629242, + "velocityY": -0.46698301649834584, + "timestamp": 4.516404788534542 + }, + { + "x": 2.3250882453572066, + "y": 4.231236836483453, + "heading": -0.996366799797096, + "angularVelocity": -0.7940299299970593, + "velocityX": -2.3322520829924085, + "velocityY": -0.34493040971001, + "timestamp": 4.603604134541613 + }, + { + "x": 2.0595373245634314, + "y": 4.211999318818496, + "heading": -1.044279386733433, + "angularVelocity": -0.5494603931140937, + "velocityX": -3.0453315644390497, + "velocityY": -0.22061538928740462, + "timestamp": 4.690803480548684 + }, + { + "x": 1.7326671393645254, + "y": 4.204157960465748, + "heading": -1.0516982080702795, + "angularVelocity": -0.08507886442456468, + "velocityX": -3.7485394118942086, + "velocityY": -0.08992450874702791, + "timestamp": 4.778002826555755 + }, + { + "x": 1.419863969855018, + "y": 4.255364813771041, + "heading": -1.034285504034905, + "angularVelocity": 0.19968847053007022, + "velocityX": -3.587219214742072, + "velocityY": 0.5872389604979419, + "timestamp": 4.865202172562826 + }, + { + "x": 1.1696215071966642, + "y": 4.29633086503019, + "heading": -1.02035490887151, + "angularVelocity": 0.15975572984532924, + "velocityX": -2.8697745352133857, + "velocityY": 0.4697976892605051, + "timestamp": 4.952401518569897 + }, + { + "x": 0.9819396842688768, + "y": 4.327055591018277, + "heading": -1.009906819806218, + "angularVelocity": 0.11981843378096674, + "velocityX": -2.152330625421988, + "velocityY": 0.35235041769230013, + "timestamp": 5.039600864576968 + }, + { + "x": 0.8568184786877541, + "y": 4.347538817267099, + "heading": -1.002941368423543, + "angularVelocity": 0.07987962870856867, + "velocityX": -1.434886972328629, + "velocityY": 0.2349011453269456, + "timestamp": 5.126800210584039 + }, + { + "x": 0.7942578792572021, + "y": 4.357780456542969, + "heading": -0.9994586215071236, + "angularVelocity": 0.03994005776301401, + "velocityX": -0.7174434476318076, + "velocityY": 0.11745087256777795, + "timestamp": 5.21399955659111 + }, + { + "x": 0.7942578792572021, + "y": 4.357780456542969, + "heading": -0.9994586215071236, + "angularVelocity": -9.407654269861187e-21, + "velocityX": -1.531911723578323e-20, + "velocityY": 3.960115739846757e-19, + "timestamp": 5.301198902598181 } ], "constraints": [ @@ -537,42 +762,1202 @@ "first" ], "type": "StopPoint", - "uuid": "97aec77a-72c9-452d-b090-247d1b380727" + "uuid": "f5e2f7dc-14a2-456d-a6fe-7d777b7ff67b" }, { "scope": [ "last" ], "type": "StopPoint", - "uuid": "2e8488aa-d4fb-46f3-93c2-d32a1ab6c3d0" + "uuid": "ae918eb2-5f0e-4193-9c60-52e3bb50a1ba" }, { "scope": [ - 2 + 3 ], "type": "StopPoint", - "uuid": "daeb5c61-2b20-430d-951f-f2bdd4f2e4c0" + "uuid": "cf8e15e8-0cb0-4e98-8b57-a1df1fe7f549" }, { "scope": [ - 4 + 5 ], "type": "StopPoint", - "uuid": "96ea1bb1-66cd-462e-bee5-77ef8d7233f0" + "uuid": "3fc2d624-09ad-44f8-b3e5-99fb97f324b6" }, { "scope": [ - 5 + 6 ], "type": "WptVelocityDirection", - "uuid": "889873f8-2ca0-445b-af07-7765055f2cb4", + "uuid": "b6b3c210-d1c4-4502-b48c-cda9e58be917", "direction": -1 + }, + { + "scope": [ + 1, + 2 + ], + "type": "ZeroAngularVelocity", + "uuid": "203093d8-08ae-40dd-a312-c914dbd9735b" } ], "usesControlIntervalGuessing": true, "defaultControlIntervalCount": 40, "usesDefaultFieldObstacles": true, "circleObstacles": [] + }, + "test": { + "waypoints": [ + { + "x": 2.1199185848236084, + "y": 5.957952976226807, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + }, + { + "x": 4.337240695953369, + "y": 5.957952976226807, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + }, + { + "x": 3.1613881587982178, + "y": 4.647716999053955, + "heading": -1.5707963267948966, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + }, + { + "x": 2.131117343902588, + "y": 5.946754455566406, + "heading": -3.158540067224243, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 2.1199185848236084, + "y": 5.957952976226807, + "heading": -6.25322258455275e-30, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 2.157285981558088, + "y": 5.958401136866338, + "heading": 0.034313098562848664, + "angularVelocity": 0.4715592535645141, + "velocityX": 0.5135339695272734, + "velocityY": 0.006158997744474669, + "timestamp": 0.07276518974757916 + }, + { + "x": 2.2137097634050407, + "y": 5.959057625247641, + "heading": 0.03431310711765102, + "angularVelocity": 1.1756723768181273e-7, + "velocityX": 0.7754227267555416, + "velocityY": 0.009022011535733282, + "timestamp": 0.14553037949515832 + }, + { + "x": 2.270133545252147, + "y": 5.959714113615942, + "heading": 0.03431311567244465, + "angularVelocity": 1.1756711762076828e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058456, + "timestamp": 0.21829556924273746 + }, + { + "x": 2.3265573270992537, + "y": 5.960370601984243, + "heading": 0.03431312422723829, + "angularVelocity": 1.1756711788910916e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058367, + "timestamp": 0.29106075899031664 + }, + { + "x": 2.38298110894636, + "y": 5.961027090352544, + "heading": 0.0343131327820319, + "angularVelocity": 1.175671175035012e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058381, + "timestamp": 0.3638259487378958 + }, + { + "x": 2.4394048907934667, + "y": 5.961683578720844, + "heading": 0.034313141336825545, + "angularVelocity": 1.1756711790299388e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058397, + "timestamp": 0.436591138485475 + }, + { + "x": 2.495828672640573, + "y": 5.962340067089145, + "heading": 0.03431314989161917, + "angularVelocity": 1.1756711758579864e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.0090220113570584, + "timestamp": 0.5093563282330541 + }, + { + "x": 2.5522524544876797, + "y": 5.962996555457446, + "heading": 0.0343131584464128, + "angularVelocity": 1.175671177574276e-7, + "velocityX": 0.7754227267576639, + "velocityY": 0.009022011357058414, + "timestamp": 0.5821215179806333 + }, + { + "x": 2.6086762363347864, + "y": 5.963653043825747, + "heading": 0.034313167001206415, + "angularVelocity": 1.1756711751242314e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058426, + "timestamp": 0.6548867077282124 + }, + { + "x": 2.6651000181818927, + "y": 5.964309532194048, + "heading": 0.03431317555600002, + "angularVelocity": 1.1756711746464553e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058426, + "timestamp": 0.7276518974757916 + }, + { + "x": 2.7215238000289994, + "y": 5.964966020562349, + "heading": 0.03431318411079364, + "angularVelocity": 1.1756711761494635e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058443, + "timestamp": 0.8004170872233708 + }, + { + "x": 2.7779475818761057, + "y": 5.965622508930649, + "heading": 0.034313192665587286, + "angularVelocity": 1.1756711789914241e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058452, + "timestamp": 0.87318227697095 + }, + { + "x": 2.8343713637232124, + "y": 5.96627899729895, + "heading": 0.03431320122038093, + "angularVelocity": 1.1756711786542428e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058466, + "timestamp": 0.9459474667185291 + }, + { + "x": 2.8907951455703187, + "y": 5.966935485667251, + "heading": 0.03431320977517455, + "angularVelocity": 1.1756711758478168e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058475, + "timestamp": 1.0187126564661082 + }, + { + "x": 2.9472189274174254, + "y": 5.967591974035552, + "heading": 0.03431321832996816, + "angularVelocity": 1.1756711747039204e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058483, + "timestamp": 1.0914778462136874 + }, + { + "x": 3.0036427092645317, + "y": 5.968248462403853, + "heading": 0.034313226884761766, + "angularVelocity": 1.1756711737664598e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058489, + "timestamp": 1.1642430359612665 + }, + { + "x": 3.0600664911116384, + "y": 5.968904950772154, + "heading": 0.03431323543955538, + "angularVelocity": 1.1756711759331021e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058496, + "timestamp": 1.2370082257088457 + }, + { + "x": 3.116490272958745, + "y": 5.969561439140454, + "heading": 0.034313243994349, + "angularVelocity": 1.1756711756317827e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058518, + "timestamp": 1.309773415456425 + }, + { + "x": 3.1729140548058514, + "y": 5.970217927508755, + "heading": 0.03431325254914262, + "angularVelocity": 1.1756711755305748e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058523, + "timestamp": 1.382538605204004 + }, + { + "x": 3.229337836652958, + "y": 5.970874415877057, + "heading": 0.03431326110393624, + "angularVelocity": 1.175671175226974e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.00902201135705854, + "timestamp": 1.4553037949515832 + }, + { + "x": 3.2857616185000644, + "y": 5.971530904245357, + "heading": 0.03431326965872984, + "angularVelocity": 1.1756711738766866e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058546, + "timestamp": 1.5280689846991624 + }, + { + "x": 3.342185400347171, + "y": 5.972187392613658, + "heading": 0.03431327821352346, + "angularVelocity": 1.1756711754575316e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058565, + "timestamp": 1.6008341744467416 + }, + { + "x": 3.3986091821942774, + "y": 5.972843880981959, + "heading": 0.03431328676831708, + "angularVelocity": 1.1756711769845501e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058572, + "timestamp": 1.6735993641943208 + }, + { + "x": 3.455032964041384, + "y": 5.973500369350259, + "heading": 0.034313295323110705, + "angularVelocity": 1.175671175699485e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058579, + "timestamp": 1.7463645539419 + }, + { + "x": 3.5114567458884904, + "y": 5.97415685771856, + "heading": 0.03431330387790433, + "angularVelocity": 1.1756711766849268e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058603, + "timestamp": 1.819129743689479 + }, + { + "x": 3.567880527735597, + "y": 5.9748133460868615, + "heading": 0.03431331243269794, + "angularVelocity": 1.1756711756878859e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058601, + "timestamp": 1.8918949334370583 + }, + { + "x": 3.624304309582704, + "y": 5.975469834455162, + "heading": 0.03431332098749156, + "angularVelocity": 1.1756711758891969e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058617, + "timestamp": 1.9646601231846375 + }, + { + "x": 3.68072809142981, + "y": 5.976126322823463, + "heading": 0.03431332954228517, + "angularVelocity": 1.1756711749627294e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058617, + "timestamp": 2.0374253129322164 + }, + { + "x": 3.737151873276917, + "y": 5.976782811191764, + "heading": 0.03431333809707879, + "angularVelocity": 1.1756711754302844e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058624, + "timestamp": 2.1101905026797954 + }, + { + "x": 3.793575655124023, + "y": 5.977439299560064, + "heading": 0.03431334665187242, + "angularVelocity": 1.175671176839478e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058646, + "timestamp": 2.1829556924273743 + }, + { + "x": 3.84999943697113, + "y": 5.978095787928365, + "heading": 0.034313355206666014, + "angularVelocity": 1.1756711730438892e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.00902201135705865, + "timestamp": 2.2557208821749533 + }, + { + "x": 3.906423218818236, + "y": 5.9787522762966665, + "heading": 0.03431336376145962, + "angularVelocity": 1.1756711741363398e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058672, + "timestamp": 2.328486071922532 + }, + { + "x": 3.962847000665343, + "y": 5.979408764664967, + "heading": 0.03431337231625322, + "angularVelocity": 1.1756711736937194e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058674, + "timestamp": 2.401251261670111 + }, + { + "x": 4.019270782512449, + "y": 5.980065253033268, + "heading": 0.034313380871046816, + "angularVelocity": 1.17567117272878e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.00902201135705869, + "timestamp": 2.47401645141769 + }, + { + "x": 4.075694564359556, + "y": 5.9807217414015685, + "heading": 0.034313389425840425, + "angularVelocity": 1.1756711748988721e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058704, + "timestamp": 2.546781641165269 + }, + { + "x": 4.132118346206663, + "y": 5.981378229769869, + "heading": 0.034313397980634035, + "angularVelocity": 1.1756711734690936e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.00902201135705871, + "timestamp": 2.619546830912848 + }, + { + "x": 4.188542128053769, + "y": 5.98203471813817, + "heading": 0.03431340653542763, + "angularVelocity": 1.1756711733140912e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058714, + "timestamp": 2.692312020660427 + }, + { + "x": 4.244965909900876, + "y": 5.982691206506428, + "heading": 0.03431341509022124, + "angularVelocity": 1.1756711743485013e-7, + "velocityX": 0.7754227267576708, + "velocityY": 0.00902201135645878, + "timestamp": 2.765077210408006 + }, + { + "x": 4.30138969265204, + "y": 5.983347617167627, + "heading": 0.03431342364501558, + "angularVelocity": 1.1756712748463029e-7, + "velocityX": 0.7754227391819724, + "velocityY": 0.009020943441183957, + "timestamp": 2.837842400155585 + }, + { + "x": 4.337240695953369, + "y": 5.957952976226807, + "heading": -3.4992233566514893e-26, + "angularVelocity": -0.47156372111511136, + "velocityX": 0.4926944247063125, + "velocityY": -0.3489943615747365, + "timestamp": 2.910607589903164 + }, + { + "x": 4.337075774441293, + "y": 5.930386954424772, + "heading": -0.09015124251095946, + "angularVelocity": -1.224439774519884, + "velocityX": -0.002239974219267059, + "velocityY": -0.37440342007034494, + "timestamp": 2.9842341123234344 + }, + { + "x": 4.333264434450346, + "y": 5.912061346555676, + "heading": -0.20505051130417343, + "angularVelocity": -1.5605690044322955, + "velocityX": -0.05176585645578975, + "velocityY": -0.24889954416819945, + "timestamp": 3.057860634743705 + }, + { + "x": 4.325553986362593, + "y": 5.887325303672255, + "heading": -0.3021610086195407, + "angularVelocity": -1.31896080546962, + "velocityX": -0.10472378477610225, + "velocityY": -0.33596647064523955, + "timestamp": 3.1314871571639755 + }, + { + "x": 4.313628651056446, + "y": 5.857761645119009, + "heading": -0.3834128542416627, + "angularVelocity": -1.1035676132892005, + "velocityX": -0.16197064473690057, + "velocityY": -0.40153544648615047, + "timestamp": 3.205113679584246 + }, + { + "x": 4.297606686426737, + "y": 5.824666184248557, + "heading": -0.4508559142750475, + "angularVelocity": -0.9160158298447155, + "velocityX": -0.2176113186258289, + "velocityY": -0.4495046048968324, + "timestamp": 3.2787402020045167 + }, + { + "x": 4.277844157832715, + "y": 5.789044520818464, + "heading": -0.5065111263049437, + "angularVelocity": -0.7559125461910066, + "velocityX": -0.26841589069241406, + "velocityY": -0.4838156449486921, + "timestamp": 3.3523667244247872 + }, + { + "x": 4.254799811398479, + "y": 5.751649129927804, + "heading": -0.5522697074374715, + "angularVelocity": -0.6214958907244229, + "velocityX": -0.3129897444116032, + "velocityY": -0.5079065214733656, + "timestamp": 3.425993246845058 + }, + { + "x": 4.228951375108858, + "y": 5.713030269053256, + "heading": -0.5898366283888508, + "angularVelocity": -0.5102362534107197, + "velocityX": -0.35107506697211693, + "velocityY": -0.524523766776681, + "timestamp": 3.4996197692653284 + }, + { + "x": 4.2007493706625425, + "y": 5.673585962715614, + "heading": -0.6207093446974378, + "angularVelocity": -0.419315150216672, + "velocityX": -0.3830413758419039, + "velocityY": -0.5357350183197291, + "timestamp": 3.573246291685599 + }, + { + "x": 4.1705955025497214, + "y": 5.633604594294146, + "heading": -0.6461809126237779, + "angularVelocity": -0.34595641745707684, + "velocityX": -0.4095517093785753, + "velocityY": -0.5430294288958701, + "timestamp": 3.6468728141058695 + }, + { + "x": 4.138835739465679, + "y": 5.593298519659669, + "heading": -0.6673577962188777, + "angularVelocity": -0.28762574815389413, + "velocityX": -0.43136307460989803, + "velocityY": -0.5474396088464458, + "timestamp": 3.72049933652614 + }, + { + "x": 4.1057614035434415, + "y": 5.552829496334061, + "heading": -0.6851851845597893, + "angularVelocity": -0.2421326955950787, + "velocityX": -0.44921768453825217, + "velocityY": -0.549652788089118, + "timestamp": 3.7941258589464106 + }, + { + "x": 4.071614137002915, + "y": 5.512327581345269, + "heading": -0.700475200245641, + "angularVelocity": -0.20766994261353336, + "velocityX": -0.4637902948289247, + "velocityY": -0.5500995247011782, + "timestamp": 3.867752381366681 + }, + { + "x": 4.036592388923184, + "y": 5.471905240898815, + "heading": -0.7139354213530316, + "angularVelocity": -0.18281755901165134, + "velocityX": -0.47566755740304917, + "velocityY": -0.5490187383252648, + "timestamp": 3.9413789037869518 + }, + { + "x": 4.0008581978077, + "y": 5.431668194433558, + "heading": -0.7261965659955223, + "angularVelocity": -0.16653162799816065, + "velocityX": -0.4853440029600765, + "velocityY": -0.5465020639651851, + "timestamp": 4.015005426207223 + }, + { + "x": 3.964543737766948, + "y": 5.391724222217225, + "heading": -0.7378391147106159, + "angularVelocity": -0.15812981969508613, + "velocityX": -0.49322525153997593, + "velocityY": -0.5425215113152707, + "timestamp": 4.088631948627493 + }, + { + "x": 3.9277575195733885, + "y": 5.352190902296246, + "heading": -0.7494192294944804, + "angularVelocity": -0.15728183816374608, + "velocityX": -0.4996327000693894, + "velocityY": -0.536944006336704, + "timestamp": 4.162258471047764 + }, + { + "x": 3.89059042061129, + "y": 5.3132030416510085, + "heading": -0.761494708226674, + "angularVelocity": -0.16400990207394403, + "velocityX": -0.5048058463218383, + "velocityY": -0.5295355445784652, + "timestamp": 4.2358849934680345 + }, + { + "x": 3.8531219539867014, + "y": 5.274920426877835, + "heading": -0.7746519957947015, + "angularVelocity": -0.1787030968666938, + "velocityX": -0.5088990406298591, + "velocityY": -0.5199568513456381, + "timestamp": 4.309511515888305 + }, + { + "x": 3.8154274539330624, + "y": 5.23753642375436, + "heading": -0.7895355361070671, + "angularVelocity": -0.2021491688471734, + "velocityX": -0.5119690407007602, + "velocityY": -0.507751852112243, + "timestamp": 4.383138038308576 + }, + { + "x": 3.7775872299458775, + "y": 5.201287866945812, + "heading": -0.8068810412551409, + "angularVelocity": -0.23558772814317216, + "velocityX": -0.5139482722161929, + "velocityY": -0.49233014974734224, + "timestamp": 4.456764560728846 + }, + { + "x": 3.7396993230404707, + "y": 5.166466510716887, + "heading": -0.8275545372921488, + "angularVelocity": -0.2807887070776005, + "velocityX": -0.5145959045727775, + "velocityY": -0.4729458228402956, + "timestamp": 4.530391083149117 + }, + { + "x": 3.701898417706247, + "y": 5.133430805056498, + "heading": -0.8525964909440766, + "angularVelocity": -0.3401213697013267, + "velocityX": -0.5134142438298316, + "velocityY": -0.44869300592275085, + "timestamp": 4.604017605569387 + }, + { + "x": 3.664343129524347, + "y": 5.101026571041832, + "heading": -0.8792757408403825, + "angularVelocity": -0.36235922897480916, + "velocityX": -0.5100782564132129, + "velocityY": -0.44011631881374225, + "timestamp": 4.677644127989658 + }, + { + "x": 3.6270751479601078, + "y": 5.068774406382283, + "heading": -0.9064492634455257, + "angularVelocity": -0.3690724716024585, + "velocityX": -0.5061760400893075, + "velocityY": -0.4380509033884428, + "timestamp": 4.7512706504099285 + }, + { + "x": 3.590111595107405, + "y": 5.036682857394856, + "heading": -0.9341662862028731, + "angularVelocity": -0.37645432442313, + "velocityX": -0.5020412704230309, + "velocityY": -0.43586941135483154, + "timestamp": 4.824897172830199 + }, + { + "x": 3.55347222485786, + "y": 5.004762522170154, + "heading": -0.9624850330743058, + "angularVelocity": -0.38462697870999835, + "velocityX": -0.4976382021739686, + "velocityY": -0.4335439753964831, + "timestamp": 4.89852369525047 + }, + { + "x": 3.517180102788644, + "y": 4.973026350131823, + "heading": -0.9914744586380927, + "angularVelocity": -0.3937361783612594, + "velocityX": -0.49292185582330617, + "velocityY": -0.4310426595619462, + "timestamp": 4.97215021767074 + }, + { + "x": 3.4812625207112933, + "y": 4.941490315870616, + "heading": -1.0212172545129166, + "angularVelocity": -0.40396850071293, + "velocityX": -0.48783483039342795, + "velocityY": -0.42832437584372535, + "timestamp": 5.045776740091011 + }, + { + "x": 3.445752274995712, + "y": 4.9101744053392995, + "heading": -1.051814154831974, + "angularVelocity": -0.41556900031765764, + "velocityX": -0.4823023626307331, + "velocityY": -0.4253346416738305, + "timestamp": 5.119403262511281 + }, + { + "x": 3.4106895099559638, + "y": 4.879104067704057, + "heading": -1.0833902078498774, + "angularVelocity": -0.4288679130824993, + "velocityX": -0.4762246523013149, + "velocityY": -0.4219992553483426, + "timestamp": 5.193029784931552 + }, + { + "x": 3.376124474151927, + "y": 4.848312423976759, + "heading": -1.1161042305606605, + "angularVelocity": -0.44432388812345175, + "velocityX": -0.4694644629109969, + "velocityY": -0.4182140173826824, + "timestamp": 5.266656307351822 + }, + { + "x": 3.3421218274128583, + "y": 4.817843776695172, + "heading": -1.1501637008410697, + "angularVelocity": -0.4625978405715368, + "velocityX": -0.4618260597040901, + "velocityY": -0.4138270595977395, + "timestamp": 5.340282829772093 + }, + { + "x": 3.3087677521953345, + "y": 4.787759503770135, + "heading": -1.1858495459870186, + "angularVelocity": -0.48468736499938486, + "velocityX": -0.45301712101970637, + "velocityY": -0.40860646321595784, + "timestamp": 5.413909352192364 + }, + { + "x": 3.2761825599174124, + "y": 4.758148688152823, + "heading": -1.2235604524642407, + "angularVelocity": -0.5121918737647652, + "velocityX": -0.44257410518347007, + "velocityY": -0.4021759366589302, + "timestamp": 5.487535874612634 + }, + { + "x": 3.244545279648596, + "y": 4.729149218235016, + "heading": -1.2639000345369864, + "angularVelocity": -0.547894708953951, + "velocityX": -0.4296995054068451, + "velocityY": -0.3938726014013467, + "timestamp": 5.561162397032905 + }, + { + "x": 3.2141486367937797, + "y": 4.700995814255907, + "heading": -1.3078734515357762, + "angularVelocity": -0.5972496805944914, + "velocityX": -0.4128490910015826, + "velocityY": -0.382381281278044, + "timestamp": 5.634788919453175 + }, + { + "x": 3.185551521789466, + "y": 4.674155611334741, + "heading": -1.357437524753386, + "angularVelocity": -0.6731823205595795, + "velocityX": -0.38840779197850694, + "velocityY": -0.36454530295424753, + "timestamp": 5.708415441873446 + }, + { + "x": 3.161389111124608, + "y": 4.65130116728667, + "heading": -1.4227635315487523, + "angularVelocity": -0.8872618812888667, + "velocityX": -0.3281753622279693, + "velocityY": -0.3104104784090629, + "timestamp": 5.782041964293716 + }, + { + "x": 3.1613881587982178, + "y": 4.647716999053955, + "heading": -1.5707963267948966, + "angularVelocity": -2.010590618434374, + "velocityX": -0.00001293455617664302, + "velocityY": -0.048680395527252375, + "timestamp": 5.855668486713987 + }, + { + "x": 3.1590242751538056, + "y": 4.648068652143942, + "heading": -1.71822885135636, + "angularVelocity": -2.0416470093627317, + "velocityX": -0.032735083303029204, + "velocityY": 0.0048696953514198, + "timestamp": 5.927881030746015 + }, + { + "x": 3.147722843506569, + "y": 4.651261007949863, + "heading": -1.8460861775851984, + "angularVelocity": -1.7705694757427723, + "velocityX": -0.15650233347580403, + "velocityY": 0.04420777371456313, + "timestamp": 6.000093574778043 + }, + { + "x": 3.1294383579479246, + "y": 4.658664086881976, + "heading": -1.9555006668792347, + "angularVelocity": -1.5151728935835482, + "velocityX": -0.25320373078858216, + "velocityY": 0.10251790781432463, + "timestamp": 6.07230611881007 + }, + { + "x": 3.105950392035797, + "y": 4.670811298853881, + "heading": -2.048080530965875, + "angularVelocity": -1.2820468427975569, + "velocityX": -0.325261576461146, + "velocityY": 0.16821470749621126, + "timestamp": 6.144518662842098 + }, + { + "x": 3.078734489629141, + "y": 4.687681461713096, + "heading": -2.125691691683944, + "angularVelocity": -1.0747600954710448, + "velocityX": -0.37688607667090807, + "velocityY": 0.23361817652806605, + "timestamp": 6.216731206874126 + }, + { + "x": 3.048937913872785, + "y": 4.708930614799334, + "heading": -2.190285310241282, + "angularVelocity": -0.8944930472009076, + "velocityX": -0.41262326588495246, + "velocityY": 0.2942584750485111, + "timestamp": 6.288943750906154 + }, + { + "x": 3.0174119547024816, + "y": 4.734060139937329, + "heading": -2.243768219004051, + "angularVelocity": -0.7406318317638603, + "velocityX": -0.4365717839316291, + "velocityY": 0.347993904311833, + "timestamp": 6.361156294938182 + }, + { + "x": 2.9847672349131935, + "y": 4.762525988198673, + "heading": -2.2879169282753247, + "angularVelocity": -0.6113717479845765, + "velocityX": -0.4520643916770092, + "velocityY": 0.3941953388142518, + "timestamp": 6.4333688389702095 + }, + { + "x": 2.9514318737909195, + "y": 4.793802004717416, + "heading": -2.3243308683279778, + "angularVelocity": -0.5042605899122328, + "velocityX": -0.4616283994577112, + "velocityY": 0.4331105757037482, + "timestamp": 6.505581383002237 + }, + { + "x": 2.9177026495619596, + "y": 4.827411588267587, + "heading": -2.3544162803840454, + "angularVelocity": -0.4166230737242065, + "velocityX": -0.46708261952383673, + "velocityY": 0.46542583426037293, + "timestamp": 6.577793927034265 + }, + { + "x": 2.88378589769779, + "y": 4.862939604927141, + "heading": -2.379391061920221, + "angularVelocity": -0.3458510134347111, + "velocityX": -0.4696795040087044, + "velocityY": 0.4919923142964852, + "timestamp": 6.650006471066293 + }, + { + "x": 2.8498283989976017, + "y": 4.900033117168976, + "heading": -2.4003021659557313, + "angularVelocity": -0.289577168562792, + "velocityX": -0.47024376658337114, + "velocityY": 0.5136713120837215, + "timestamp": 6.722219015098321 + }, + { + "x": 2.8159399738054236, + "y": 4.938396448572123, + "heading": -2.4180494138871094, + "angularVelocity": -0.2457640589910084, + "velocityX": -0.46928723598420935, + "velocityY": 0.5312557799671519, + "timestamp": 6.794431559130349 + }, + { + "x": 2.7822098245267197, + "y": 4.977783840614619, + "heading": -2.4334117988726813, + "angularVelocity": -0.2127384541217516, + "velocityX": -0.4670954296215342, + "velocityY": 0.5454369814893605, + "timestamp": 6.866644103162376 + }, + { + "x": 2.7487184813842567, + "y": 5.0179914540575155, + "heading": -2.4470741047795617, + "angularVelocity": -0.18919574278978962, + "velocityX": -0.46378843996424124, + "velocityY": 0.5567954152821849, + "timestamp": 6.938856647194404 + }, + { + "x": 2.7155468563385656, + "y": 5.0588495367779895, + "heading": -2.459652889491748, + "angularVelocity": -0.17419113092882704, + "velocityX": -0.4593609807041143, + "velocityY": 0.5658031200556053, + "timestamp": 7.011069191226432 + }, + { + "x": 2.682783562068564, + "y": 5.100215023933873, + "heading": -2.471721683170927, + "angularVelocity": -0.16712877022898326, + "velocityX": -0.45370641221933317, + "velocityY": 0.5728296615271857, + "timestamp": 7.08328173525846 + }, + { + "x": 2.650531366839192, + "y": 5.141964497495229, + "heading": -2.4838357664761026, + "angularVelocity": -0.16775594140268155, + "velocityX": -0.44662870781934594, + "velocityY": 0.5781471089405039, + "timestamp": 7.155494279290488 + }, + { + "x": 2.618913440143494, + "y": 5.183987197473736, + "heading": -2.496557238457018, + "angularVelocity": -0.17616706559006176, + "velocityX": -0.4378453511023647, + "velocityY": 0.5819307509768665, + "timestamp": 7.2277068233225155 + }, + { + "x": 2.5880798824866296, + "y": 5.226177564560792, + "heading": -2.5104813496102354, + "angularVelocity": -0.1928212243435465, + "velocityX": -0.4269834011551906, + "velocityY": 0.5842526066986774, + "timestamp": 7.299919367354543 + }, + { + "x": 2.5582148956282778, + "y": 5.268426535906725, + "heading": -2.5262653286045555, + "angularVelocity": -0.21857669198470553, + "velocityX": -0.41357062347929907, + "velocityY": 0.585064159035808, + "timestamp": 7.372131911386571 + }, + { + "x": 2.5295447873325214, + "y": 5.31061044475372, + "heading": -2.5446612115646436, + "angularVelocity": -0.2547463630685689, + "velocityX": -0.39702393372321826, + "velocityY": 0.5841631729285931, + "timestamp": 7.444344455418599 + }, + { + "x": 2.5023467091367184, + "y": 5.352575812308809, + "heading": -2.566554449178874, + "angularVelocity": -0.3031777637486395, + "velocityX": -0.3766392468286359, + "velocityY": 0.5811368110293486, + "timestamp": 7.516556999450627 + }, + { + "x": 2.476956466389222, + "y": 5.394117632986223, + "heading": -2.593007817119188, + "angularVelocity": -0.36632649209230705, + "velocityX": -0.3516043242602833, + "velocityY": 0.5752715298188288, + "timestamp": 7.5887695434826545 + }, + { + "x": 2.4521989817386887, + "y": 5.435268589005595, + "heading": -2.621259955334882, + "angularVelocity": -0.39123588005934157, + "velocityX": -0.3428418840852991, + "velocityY": 0.5698588322981821, + "timestamp": 7.660982087514682 + }, + { + "x": 2.427559065081996, + "y": 5.476070836186979, + "heading": -2.6500802362174434, + "angularVelocity": -0.39910352514071706, + "velocityX": -0.34121380138282903, + "velocityY": 0.5650299089765801, + "timestamp": 7.73319463154671 + }, + { + "x": 2.403044029416898, + "y": 5.516501717159452, + "heading": -2.6795300674706377, + "angularVelocity": -0.407821544690809, + "velocityX": -0.3394844482175417, + "velocityY": 0.559887226165873, + "timestamp": 7.805407175578738 + }, + { + "x": 2.378663293884139, + "y": 5.556534603961008, + "heading": -2.7096829003029756, + "angularVelocity": -0.41755671727843513, + "velocityX": -0.33762465870120245, + "velocityY": 0.5543757990828, + "timestamp": 7.877619719610766 + }, + { + "x": 2.354428772799968, + "y": 5.59613781059984, + "heading": -2.74062708945702, + "angularVelocity": -0.42851542718561086, + "velocityX": -0.33559987961957327, + "velocityY": 0.5484255840822709, + "timestamp": 7.949832263642794 + }, + { + "x": 2.330355697409695, + "y": 5.635273029678329, + "heading": -2.77247058331099, + "angularVelocity": -0.4409690072662031, + "velocityX": -0.3333641781072918, + "velocityY": 0.5419448878733852, + "timestamp": 8.02204480767482 + }, + { + "x": 2.306463861338364, + "y": 5.673893057942382, + "heading": -2.8053478457859313, + "angularVelocity": -0.45528464501069815, + "velocityX": -0.33085437428619124, + "velocityY": 0.534810520550616, + "timestamp": 8.094257351706847 + }, + { + "x": 2.282779542094817, + "y": 5.711938365696433, + "heading": -2.839430372846894, + "angularVelocity": -0.47197516051846805, + "velocityX": -0.3279806792714956, + "velocityY": 0.5268517854346452, + "timestamp": 8.166469895738874 + }, + { + "x": 2.259338596376033, + "y": 5.749331681355029, + "heading": -2.8749434001407064, + "angularVelocity": -0.4917847414164131, + "velocityX": -0.3246104403742845, + "velocityY": 0.5178229926647014, + "timestamp": 8.238682439770901 + }, + { + "x": 2.2361917393720554, + "y": 5.785968937848207, + "heading": -2.9121940133772717, + "angularVelocity": -0.5158468481603996, + "velocityX": -0.3205378970405998, + "velocityY": 0.5073530781151929, + "timestamp": 8.310894983802928 + }, + { + "x": 2.2134142469672544, + "y": 5.821702966064213, + "heading": -2.9516221084722227, + "angularVelocity": -0.5460006377488126, + "velocityX": -0.3154229325406202, + "velocityY": 0.4948451642993865, + "timestamp": 8.383107527834955 + }, + { + "x": 2.191125686501144, + "y": 5.856311001786043, + "heading": -2.9939026154296866, + "angularVelocity": -0.5855008644856994, + "velocityX": -0.3086521983801695, + "velocityY": 0.4792524094772386, + "timestamp": 8.455320071866982 + }, + { + "x": 2.169536365545648, + "y": 5.8894197426408175, + "heading": -3.0401828782490323, + "angularVelocity": -0.6408895218927577, + "velocityX": -0.29896912295350214, + "velocityY": 0.4584901598272232, + "timestamp": 8.527532615899009 + }, + { + "x": 2.1490856813219974, + "y": 5.9202865649088325, + "heading": -3.092780352693255, + "angularVelocity": -0.7283703288571992, + "velocityX": -0.28320127060722483, + "velocityY": 0.42744404980837086, + "timestamp": 8.599745159931036 + }, + { + "x": 2.131117343902588, + "y": 5.946754455566406, + "heading": -3.158540067224243, + "angularVelocity": -0.9106411553901581, + "velocityX": -0.24882570833455084, + "velocityY": 0.3665276028197312, + "timestamp": 8.671957703963063 + }, + { + "x": 2.131117343902588, + "y": 5.946754455566406, + "heading": -3.158540067224243, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": 0, + "timestamp": 8.74417024799509 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint", + "uuid": "8bd4a7b9-3f58-4fe1-9239-10051f881fd7" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint", + "uuid": "d9bfd8e0-e225-46e1-8b92-adfae7d70f38" + } + ], + "usesControlIntervalGuessing": false, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] } - } + }, + "splitTrajectoriesAtStopPoints": true } \ No newline at end of file diff --git a/ctre_sim/Talon FX vers. C - 010 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 010 - 0 - ext.dat new file mode 100644 index 00000000..4e4eb2dc Binary files /dev/null and b/ctre_sim/Talon FX vers. C - 010 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 012 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 012 - 0 - ext.dat new file mode 100644 index 00000000..7b2f31b3 Binary files /dev/null and b/ctre_sim/Talon FX vers. C - 012 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 027 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 027 - 0 - ext.dat new file mode 100644 index 00000000..980135a5 Binary files /dev/null and b/ctre_sim/Talon FX vers. C - 027 - 0 - ext.dat differ diff --git a/deploy/choreo/amp 4 local.traj b/deploy/choreo/amp 4 local.traj new file mode 100644 index 00000000..6dfc86f9 --- /dev/null +++ b/deploy/choreo/amp 4 local.traj @@ -0,0 +1,760 @@ +{ + "samples": [ + { + "x": 0.7470832465334709, + "y": 6.625458718233556, + "heading": 1.0564954979067924, + "angularVelocity": -5.684301329861994e-12, + "velocityX": -3.602456269610706e-10, + "velocityY": 4.1137821262407583e-10, + "timestamp": 0 + }, + { + "x": 0.7589569772775385, + "y": 6.629936126338657, + "heading": 1.0434265097431823, + "angularVelocity": -0.12778392744309977, + "velocityX": 0.11609712676013798, + "velocityY": 0.04377849867759475, + "timestamp": 0.10227411521599863 + }, + { + "x": 0.7822022838432713, + "y": 6.638629979602176, + "heading": 1.0183346599429852, + "angularVelocity": -0.2453392014361959, + "velocityX": 0.2272843592055021, + "velocityY": 0.08500540295845989, + "timestamp": 0.20454823043199727 + }, + { + "x": 0.816345318524229, + "y": 6.6512661070932495, + "heading": 0.9822355978698938, + "angularVelocity": -0.35296381641965074, + "velocityX": 0.33383847695279084, + "velocityY": 0.12355156118457596, + "timestamp": 0.30682234564799593 + }, + { + "x": 0.8609432271767217, + "y": 6.667555740683275, + "heading": 0.9361120278121537, + "angularVelocity": -0.45097989764859636, + "velocityX": 0.4360625240585109, + "velocityY": 0.15927424713723962, + "timestamp": 0.40909646086399454 + }, + { + "x": 0.9155883048461597, + "y": 6.687191502411657, + "heading": 0.8809291072748399, + "angularVelocity": -0.5395590113414345, + "velocityX": 0.5343001768187209, + "velocityY": 0.19199150048411856, + "timestamp": 0.5113705760799931 + }, + { + "x": 0.9799143577111046, + "y": 6.709840447255909, + "heading": 0.817662056557047, + "angularVelocity": -0.6186027670698295, + "velocityX": 0.6289573163604619, + "velocityY": 0.22145333556813593, + "timestamp": 0.6136446912959918 + }, + { + "x": 1.0536065024635548, + "y": 6.735133328172977, + "heading": 0.7473295527982347, + "angularVelocity": -0.687686259272288, + "velocityX": 0.7205356386732681, + "velocityY": 0.24730480411634378, + "timestamp": 0.7159188065119904 + }, + { + "x": 1.1364169197016813, + "y": 6.762648211314195, + "heading": 0.6710254557943266, + "angularVelocity": -0.7460743786173882, + "velocityX": 0.8096908708297301, + "velocityY": 0.2690307532427302, + "timestamp": 0.818192921727989 + }, + { + "x": 1.2281932228229642, + "y": 6.7918845460908575, + "heading": 0.5899378478266369, + "angularVelocity": -0.7928458510838684, + "velocityX": 0.8973561208709863, + "velocityY": 0.28586249636547506, + "timestamp": 0.9204670369439876 + }, + { + "x": 1.3289409008958384, + "y": 6.822217824239773, + "heading": 0.5053285615301966, + "angularVelocity": -0.8272795718879136, + "velocityX": 0.985075041075245, + "velocityY": 0.296588019337818, + "timestamp": 1.0227411521599863 + }, + { + "x": 1.4390031913745853, + "y": 6.852792823991799, + "heading": 0.4183893727045359, + "angularVelocity": -0.8500605319021087, + "velocityX": 1.076150015287458, + "velocityY": 0.29895148738224403, + "timestamp": 1.125015267375985 + }, + { + "x": 1.5597885498551032, + "y": 6.882059112916124, + "heading": 0.32979940511385675, + "angularVelocity": -0.8662012607776253, + "velocityX": 1.1809963684348201, + "velocityY": 0.28615537738935176, + "timestamp": 1.2272893825919837 + }, + { + "x": 1.700662064049805, + "y": 6.9025972281907375, + "heading": 0.2406449492427404, + "angularVelocity": -0.8717206268168672, + "velocityX": 1.3774112254273, + "velocityY": 0.20081439319892427, + "timestamp": 1.3295634978079824 + }, + { + "x": 1.8510164004103928, + "y": 6.913603640101383, + "heading": 0.18435642050200896, + "angularVelocity": -0.5503692556839603, + "velocityX": 1.4701113396826178, + "velocityY": 0.10761678064700267, + "timestamp": 1.4318376130239812 + }, + { + "x": 2.0039488544462833, + "y": 6.918843013673937, + "heading": 0.15617120081925578, + "angularVelocity": -0.2755850738942481, + "velocityX": 1.4953192592999736, + "velocityY": 0.051228726583383746, + "timestamp": 1.5341117282399799 + }, + { + "x": 2.1557445524327394, + "y": 6.919295783765447, + "heading": 0.15617120073466711, + "angularVelocity": -8.01993499988118e-11, + "velocityX": 1.484204462099971, + "velocityY": 0.0044270165813428694, + "timestamp": 1.6363858434559786 + }, + { + "x": 2.232501376452394, + "y": 6.918644981823818, + "heading": 0.15617120066422163, + "angularVelocity": -2.7881719949913897e-12, + "velocityX": 1.4631774376072004, + "velocityY": -0.012405855819009865, + "timestamp": 1.6888448428916174 + }, + { + "x": 2.3078780299226054, + "y": 6.916920762093343, + "heading": 0.1561712005937278, + "angularVelocity": -3.698975819730093e-12, + "velocityX": 1.436867928003674, + "velocityY": -0.032867889033463694, + "timestamp": 1.7413038423272562 + }, + { + "x": 2.381659462849091, + "y": 6.91403766609857, + "heading": 0.15617120052323402, + "angularVelocity": -3.687857737825465e-12, + "velocityX": 1.4064590265589882, + "velocityY": -0.05495897496281437, + "timestamp": 1.793762841762895 + }, + { + "x": 2.453627858185915, + "y": 6.909923503320222, + "heading": 0.15617120045273983, + "angularVelocity": -3.68806627753873e-12, + "velocityX": 1.3718979863339387, + "velocityY": -0.07842619311071317, + "timestamp": 1.8462218411985338 + }, + { + "x": 2.5235476514141553, + "y": 6.90450568335544, + "heading": 0.15617120038224525, + "angularVelocity": -3.688143550349055e-12, + "velocityX": 1.3328464960153679, + "velocityY": -0.10327716626043287, + "timestamp": 1.8986808406341726 + }, + { + "x": 2.5911718313337544, + "y": 6.897715977103231, + "heading": 0.1561712003117504, + "angularVelocity": -3.687560329325201e-12, + "velocityX": 1.2890863531735766, + "velocityY": -0.12942875706821344, + "timestamp": 1.9511398400698114 + }, + { + "x": 2.65624736130467, + "y": 6.889492702059127, + "heading": 0.15617120024125528, + "angularVelocity": -3.68944362324301e-12, + "velocityX": 1.240502697821755, + "velocityY": -0.1567561709974853, + "timestamp": 2.0035988395054503 + }, + { + "x": 2.7185182573768354, + "y": 6.879780772610566, + "heading": 0.1561712001707086, + "angularVelocity": -4.669569996251489e-12, + "velocityX": 1.187039346844362, + "velocityY": -0.1851336547556834, + "timestamp": 2.056057838941089 + }, + { + "x": 2.844727229024539, + "y": 6.826831262995891, + "heading": 0.36522912798339696, + "angularVelocity": 1.2815424227266763, + "velocityX": 0.7736714562084378, + "velocityY": -0.32458488264739044, + "timestamp": 2.219187770584854 + }, + { + "x": 2.905891887894138, + "y": 6.746880408353911, + "heading": 0.5636352419280429, + "angularVelocity": 1.2162459209838357, + "velocityX": 0.3749444284785087, + "velocityY": -0.490105365340026, + "timestamp": 2.382317702228619 + }, + { + "x": 2.909090532344001, + "y": 6.6442122814929245, + "heading": 0.7377210107974022, + "angularVelocity": 1.0671601901772823, + "velocityX": 0.0196079562833389, + "velocityY": -0.6293641266849203, + "timestamp": 2.5454476338723837 + }, + { + "x": 2.8528015320306115, + "y": 6.520167648469734, + "heading": 0.8617887991289785, + "angularVelocity": 0.7605458243522792, + "velocityX": -0.34505623601923857, + "velocityY": -0.7604038809886424, + "timestamp": 2.7085775655161486 + }, + { + "x": 2.6673775887021587, + "y": 6.358333521535233, + "heading": 0.8824380102910534, + "angularVelocity": 0.1265813757217392, + "velocityX": -1.1366641384675562, + "velocityY": -0.9920566103835737, + "timestamp": 2.8717074971599135 + }, + { + "x": 2.433334548523251, + "y": 6.191208743378355, + "heading": 0.7641593769894643, + "angularVelocity": -0.7250578241718747, + "velocityX": -1.4347032313583656, + "velocityY": -1.0244887408273557, + "timestamp": 3.0348374288036783 + }, + { + "x": 2.190845809692594, + "y": 6.024313186516238, + "heading": 0.6297541443819642, + "angularVelocity": -0.823915214372992, + "velocityX": -1.486476064011715, + "velocityY": -1.0230835952941921, + "timestamp": 3.1979673604474432 + }, + { + "x": 1.9580404786548902, + "y": 5.870547470854263, + "heading": 0.47066977289213696, + "angularVelocity": -0.9752003809508628, + "velocityX": -1.4271159717504125, + "velocityY": -0.9425965806313563, + "timestamp": 3.361097292091208 + }, + { + "x": 1.7498273467354928, + "y": 5.73486661370809, + "heading": 0.32338313207816166, + "angularVelocity": -0.9028793141986351, + "velocityX": -1.2763637531323493, + "velocityY": -0.831734899187721, + "timestamp": 3.524227223734973 + }, + { + "x": 1.577014249967444, + "y": 5.624033816048722, + "heading": 0.19882585300104294, + "angularVelocity": -0.7635464431083702, + "velocityX": -1.0593586039568081, + "velocityY": -0.6794142353068856, + "timestamp": 3.687357155378738 + }, + { + "x": 1.4506486184747536, + "y": 5.5441849844920235, + "heading": 0.1067515452077215, + "angularVelocity": -0.5644231371867715, + "velocityX": -0.7746317925903302, + "velocityY": -0.4894799553714506, + "timestamp": 3.8504870870225028 + }, + { + "x": 1.3818649056319274, + "y": 5.50129938101995, + "heading": 0.05628563946839357, + "angularVelocity": -0.30936018439404245, + "velocityX": -0.4216498592955241, + "velocityY": -0.2628923039688523, + "timestamp": 4.013617018666268 + }, + { + "x": 1.3818649054659777, + "y": 5.501299381110093, + "heading": 0.05628563946121549, + "angularVelocity": -8.20711890061584e-12, + "velocityX": -2.918186949691226e-10, + "velocityY": -8.516677331367793e-11, + "timestamp": 4.176746950310032 + }, + { + "x": 1.4254749911225055, + "y": 5.4996771535618185, + "heading": 0.052286234874463405, + "angularVelocity": -0.03818204740477268, + "velocityX": 0.41634256351427257, + "velocityY": -0.015487297221703413, + "timestamp": 4.281492632082638 + }, + { + "x": 1.5054277602383004, + "y": 5.496460245033646, + "heading": 0.04534031368940726, + "angularVelocity": -0.06631224378557163, + "velocityX": 0.763303725485265, + "velocityY": -0.030711609603001202, + "timestamp": 4.386238313855244 + }, + { + "x": 1.614706352195534, + "y": 5.491742221647794, + "heading": 0.036622661203774276, + "angularVelocity": -0.08322684370600701, + "velocityX": 1.0432753896642522, + "velocityY": -0.045042652489690874, + "timestamp": 4.4909839956278494 + }, + { + "x": 1.7468754648697162, + "y": 5.485736649446459, + "heading": 0.02746720464328704, + "angularVelocity": -0.08740652985101428, + "velocityX": 1.2618096559898304, + "velocityY": -0.05733479469253905, + "timestamp": 4.595729677400455 + }, + { + "x": 1.8974421226741423, + "y": 5.478814465512338, + "heading": 0.0192537076414664, + "angularVelocity": -0.07841370510897432, + "velocityX": 1.43744978567254, + "velocityY": -0.06608562544064354, + "timestamp": 4.700475359173061 + }, + { + "x": 2.0478771487383725, + "y": 5.472018862260595, + "heading": 0.011842428389913316, + "angularVelocity": -0.07075498604111852, + "velocityX": 1.436193106275662, + "velocityY": -0.06487716815402857, + "timestamp": 4.805221040945667 + }, + { + "x": 2.1991998312667134, + "y": 5.465297323381209, + "heading": 0.007319920190768939, + "angularVelocity": -0.04317608251475354, + "velocityX": 1.4446675030153622, + "velocityY": -0.06417008055763802, + "timestamp": 4.909966722718273 + }, + { + "x": 2.337086103631991, + "y": 5.4645313180146555, + "heading": 0.0021217678335590726, + "angularVelocity": -0.04962641198543019, + "velocityX": 1.316390996187784, + "velocityY": -0.007313001468104138, + "timestamp": 5.014712404490878 + }, + { + "x": 2.45341531313919, + "y": 5.464492524135499, + "heading": -0.0044756440905307565, + "angularVelocity": -0.06298504924092899, + "velocityX": 1.1105871626237722, + "velocityY": -0.0003703621622037101, + "timestamp": 5.119458086263484 + }, + { + "x": 2.541111145262716, + "y": 5.463618987693384, + "heading": -0.012133903650681083, + "angularVelocity": -0.07311289048562652, + "velocityX": 0.8372262287970834, + "velocityY": -0.008339593435811936, + "timestamp": 5.22420376803609 + }, + { + "x": 2.592916727068705, + "y": 5.461223602405139, + "heading": -0.020267736871702392, + "angularVelocity": -0.07765316033563659, + "velocityX": 0.4945844156915881, + "velocityY": -0.022868582319897673, + "timestamp": 5.328949449808696 + }, + { + "x": 2.573983182200651, + "y": 5.456183859404588, + "heading": -0.02941680850548689, + "angularVelocity": -0.07029420270393813, + "velocityX": -0.14547032678251723, + "velocityY": -0.03872138453206129, + "timestamp": 5.459103449457666 + }, + { + "x": 2.4879155076147748, + "y": 5.449739339031148, + "heading": -0.03668510065770142, + "angularVelocity": -0.05584378637679435, + "velocityX": -0.6612756796636089, + "velocityY": -0.04951457856894505, + "timestamp": 5.589257449106635 + }, + { + "x": 2.35046961428428, + "y": 5.4436438239165845, + "heading": -0.03977633600135786, + "angularVelocity": -0.02375059815495546, + "velocityX": -1.056025121733672, + "velocityY": -0.046833099245860266, + "timestamp": 5.719411448755605 + }, + { + "x": 2.1764781877651598, + "y": 5.443212327064482, + "heading": -0.032264347817817784, + "angularVelocity": 0.05771615319957162, + "velocityX": -1.336811984121226, + "velocityY": -0.0033152797571240995, + "timestamp": 5.849565448404575 + }, + { + "x": 1.9752868983122995, + "y": 5.4534701667830054, + "heading": -0.009494098721117665, + "angularVelocity": 0.174948516049147, + "velocityX": -1.5457941360881122, + "velocityY": 0.07881309585526655, + "timestamp": 5.979719448053545 + }, + { + "x": 1.7725080662079153, + "y": 5.4634789711909475, + "heading": 0.004869525328448035, + "angularVelocity": 0.11035868346800264, + "velocityX": -1.5579915534229911, + "velocityY": 0.07689970624561784, + "timestamp": 6.1098734477025145 + }, + { + "x": 1.5970859275969194, + "y": 5.473760403676058, + "heading": 0.019510408429697427, + "angularVelocity": 0.11248892189634652, + "velocityX": -1.347804439948399, + "velocityY": 0.07899436394267238, + "timestamp": 6.240027447351484 + }, + { + "x": 1.463998994938607, + "y": 5.481958295434183, + "heading": 0.031093440957313423, + "angularVelocity": 0.0889948258110792, + "velocityX": -1.0225343286428261, + "velocityY": 0.06298609118419185, + "timestamp": 6.370181447000454 + }, + { + "x": 1.3892531402609951, + "y": 5.486522674152253, + "heading": 0.03752685288213878, + "angularVelocity": 0.049429229542960515, + "velocityX": -0.5742878042328909, + "velocityY": 0.03506906174326123, + "timestamp": 6.500335446649424 + }, + { + "x": 1.3892531401370203, + "y": 5.486522674374907, + "heading": 0.03752685288891443, + "angularVelocity": 5.018347137854837e-11, + "velocityX": -8.981333628450582e-10, + "velocityY": 1.1863234183511445e-9, + "timestamp": 6.630489446298394 + }, + { + "x": 1.410785208000178, + "y": 5.466798705442517, + "heading": 0.02538794622123859, + "angularVelocity": -0.12074981514388294, + "velocityX": 0.21418676347263918, + "velocityY": -0.1962009963268651, + "timestamp": 6.731018848208977 + }, + { + "x": 1.4512665581597533, + "y": 5.429718184658836, + "heading": 0.002445244265967334, + "angularVelocity": -0.2282188250282345, + "velocityX": 0.4026816906907998, + "velocityY": -0.36885249450974994, + "timestamp": 6.8315482501195595 + }, + { + "x": 1.5081514675148215, + "y": 5.3776444456377, + "heading": -0.029986935931297812, + "angularVelocity": -0.3226138782330449, + "velocityX": 0.5658534482437384, + "velocityY": -0.5179951130124598, + "timestamp": 6.932077652030142 + }, + { + "x": 1.578941487701644, + "y": 5.312948064530918, + "heading": -0.07059739560458454, + "angularVelocity": -0.4039659933419245, + "velocityX": 0.7041722939494359, + "velocityY": -0.6435568078574709, + "timestamp": 7.032607053940725 + }, + { + "x": 1.6611946188865518, + "y": 5.238029568056684, + "heading": -0.11800108662062794, + "angularVelocity": -0.4715405656932785, + "velocityX": 0.8181997404499256, + "velocityY": -0.7452396506196601, + "timestamp": 7.133136455851308 + }, + { + "x": 1.7524893345353523, + "y": 5.155358397604862, + "heading": -0.17044488274189246, + "angularVelocity": -0.521676197753198, + "velocityX": 0.9081394425873396, + "velocityY": -0.8223581237255294, + "timestamp": 7.233665857761891 + }, + { + "x": 1.8499500532222615, + "y": 5.067431436588914, + "heading": -0.22499900927916092, + "angularVelocity": -0.5426683689609221, + "velocityX": 0.9694747626122975, + "velocityY": -0.8746392520744162, + "timestamp": 7.334195259672474 + }, + { + "x": 1.9473638091809122, + "y": 4.979075974351683, + "heading": -0.27982409115325835, + "angularVelocity": -0.5453636534436577, + "velocityX": 0.9690076084526664, + "velocityY": -0.8789016988127862, + "timestamp": 7.434724661583057 + }, + { + "x": 2.05818821772957, + "y": 4.854821291737014, + "heading": -0.34544281347225186, + "angularVelocity": -0.6527316496725523, + "velocityX": 1.1024079106043232, + "velocityY": -1.2360033993617228, + "timestamp": 7.53525406349364 + }, + { + "x": 2.1664425710584037, + "y": 4.732231232720307, + "heading": -0.4041393246178968, + "angularVelocity": -0.5838740711290391, + "velocityX": 1.076842701113489, + "velocityY": -1.2194448247922516, + "timestamp": 7.635783465404223 + }, + { + "x": 2.2666201121554423, + "y": 4.61627032113389, + "heading": -0.4602072656334145, + "angularVelocity": -0.557726794034664, + "velocityX": 0.996499915039965, + "velocityY": -1.153502450677793, + "timestamp": 7.736312867314806 + }, + { + "x": 2.356480498307365, + "y": 4.5087945921361055, + "heading": -0.5124957847583047, + "angularVelocity": -0.5201316047807618, + "velocityX": 0.8938716814183492, + "velocityY": -1.0690974663909212, + "timestamp": 7.8368422692253885 + }, + { + "x": 2.433798199341425, + "y": 4.411691494217003, + "heading": -0.5602485683833155, + "angularVelocity": -0.47501310796660834, + "velocityX": 0.7691053456052521, + "velocityY": -0.9659173928845837, + "timestamp": 7.937371671135971 + }, + { + "x": 2.4961951429135376, + "y": 4.327016530758609, + "heading": -0.6027268742617791, + "angularVelocity": -0.4225460918867537, + "velocityX": 0.620683518971687, + "velocityY": -0.8422905312450144, + "timestamp": 8.037901073046553 + }, + { + "x": 2.541198492257727, + "y": 4.256927967764492, + "heading": -0.6391760227839379, + "angularVelocity": -0.3625720221944257, + "velocityX": 0.4476635477259144, + "velocityY": -0.6971946668803632, + "timestamp": 8.138430474957136 + }, + { + "x": 2.5331758624705114, + "y": 4.192320334785442, + "heading": -0.6763224470307224, + "angularVelocity": -0.2449990301925222, + "velocityX": -0.052913214713167914, + "velocityY": -0.426119279886695, + "timestamp": 8.290049133403663 + }, + { + "x": 2.46411262676954, + "y": 4.160473976285577, + "heading": -0.7018758155970488, + "angularVelocity": -0.16853709734646205, + "velocityX": -0.45550618406914944, + "velocityY": -0.21004248027074415, + "timestamp": 8.44166779185019 + }, + { + "x": 2.348779581051416, + "y": 4.153307846262027, + "heading": -0.7202693675207003, + "angularVelocity": -0.121314567375086, + "velocityX": -0.7606784530447123, + "velocityY": -0.04726417363495295, + "timestamp": 8.593286450296716 + }, + { + "x": 2.201596779448434, + "y": 4.163036990055964, + "heading": -0.7333684069992377, + "angularVelocity": -0.0863946406365267, + "velocityX": -0.9707433369527995, + "velocityY": 0.06416850879734587, + "timestamp": 8.744905108743243 + }, + { + "x": 2.03723470076713, + "y": 4.1831659239379135, + "heading": -0.7500897294060416, + "angularVelocity": -0.11028538710126731, + "velocityX": -1.0840491612778367, + "velocityY": 0.13276026521150996, + "timestamp": 8.89652376718977 + }, + { + "x": 1.8800877783569956, + "y": 4.195672966501764, + "heading": -0.7289968148061016, + "angularVelocity": 0.13911819814698503, + "velocityX": -1.0364616391016628, + "velocityY": 0.08249012434493122, + "timestamp": 9.048142425636296 + }, + { + "x": 1.7046442157621393, + "y": 4.233321160679635, + "heading": -0.7238359462014516, + "angularVelocity": 0.03403847932720532, + "velocityX": -1.1571370254440554, + "velocityY": 0.24830844691554524, + "timestamp": 9.199761084082823 + }, + { + "x": 1.572815192920781, + "y": 4.2658136358994225, + "heading": -0.7241682787394389, + "angularVelocity": -0.002191897597681109, + "velocityX": -0.8694775764003041, + "velocityY": 0.21430393183203963, + "timestamp": 9.35137974252935 + }, + { + "x": 1.4994455554717883, + "y": 4.286480902556393, + "heading": -0.7254698893818832, + "angularVelocity": -0.00858476582372283, + "velocityX": -0.4839090323218549, + "velocityY": 0.13631083596916982, + "timestamp": 9.502998400975876 + }, + { + "x": 1.4994455567524874, + "y": 4.286480903049883, + "heading": -0.7254698893413497, + "angularVelocity": 3.8000232610425283e-11, + "velocityX": 2.8946950656209713e-9, + "velocityY": -5.415769760526697e-10, + "timestamp": 9.654617059422403 + } + ] +} \ No newline at end of file diff --git a/deploy/choreo/test.traj b/deploy/choreo/test.traj new file mode 100644 index 00000000..ddadedba --- /dev/null +++ b/deploy/choreo/test.traj @@ -0,0 +1,1093 @@ +{ + "samples": [ + { + "x": 2.1199185848236084, + "y": 5.957952976226807, + "heading": -6.25322258455275e-30, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 2.157285981558088, + "y": 5.958401136866338, + "heading": 0.034313098562848664, + "angularVelocity": 0.4715592535645141, + "velocityX": 0.5135339695272734, + "velocityY": 0.006158997744474669, + "timestamp": 0.07276518974757916 + }, + { + "x": 2.2137097634050407, + "y": 5.959057625247641, + "heading": 0.03431310711765102, + "angularVelocity": 1.1756723768181273e-7, + "velocityX": 0.7754227267555416, + "velocityY": 0.009022011535733282, + "timestamp": 0.14553037949515832 + }, + { + "x": 2.270133545252147, + "y": 5.959714113615942, + "heading": 0.03431311567244465, + "angularVelocity": 1.1756711762076828e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058456, + "timestamp": 0.21829556924273746 + }, + { + "x": 2.3265573270992537, + "y": 5.960370601984243, + "heading": 0.03431312422723829, + "angularVelocity": 1.1756711788910916e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058367, + "timestamp": 0.29106075899031664 + }, + { + "x": 2.38298110894636, + "y": 5.961027090352544, + "heading": 0.0343131327820319, + "angularVelocity": 1.175671175035012e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058381, + "timestamp": 0.3638259487378958 + }, + { + "x": 2.4394048907934667, + "y": 5.961683578720844, + "heading": 0.034313141336825545, + "angularVelocity": 1.1756711790299388e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058397, + "timestamp": 0.436591138485475 + }, + { + "x": 2.495828672640573, + "y": 5.962340067089145, + "heading": 0.03431314989161917, + "angularVelocity": 1.1756711758579864e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.0090220113570584, + "timestamp": 0.5093563282330541 + }, + { + "x": 2.5522524544876797, + "y": 5.962996555457446, + "heading": 0.0343131584464128, + "angularVelocity": 1.175671177574276e-7, + "velocityX": 0.7754227267576639, + "velocityY": 0.009022011357058414, + "timestamp": 0.5821215179806333 + }, + { + "x": 2.6086762363347864, + "y": 5.963653043825747, + "heading": 0.034313167001206415, + "angularVelocity": 1.1756711751242314e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058426, + "timestamp": 0.6548867077282124 + }, + { + "x": 2.6651000181818927, + "y": 5.964309532194048, + "heading": 0.03431317555600002, + "angularVelocity": 1.1756711746464553e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058426, + "timestamp": 0.7276518974757916 + }, + { + "x": 2.7215238000289994, + "y": 5.964966020562349, + "heading": 0.03431318411079364, + "angularVelocity": 1.1756711761494635e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058443, + "timestamp": 0.8004170872233708 + }, + { + "x": 2.7779475818761057, + "y": 5.965622508930649, + "heading": 0.034313192665587286, + "angularVelocity": 1.1756711789914241e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058452, + "timestamp": 0.87318227697095 + }, + { + "x": 2.8343713637232124, + "y": 5.96627899729895, + "heading": 0.03431320122038093, + "angularVelocity": 1.1756711786542428e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058466, + "timestamp": 0.9459474667185291 + }, + { + "x": 2.8907951455703187, + "y": 5.966935485667251, + "heading": 0.03431320977517455, + "angularVelocity": 1.1756711758478168e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058475, + "timestamp": 1.0187126564661082 + }, + { + "x": 2.9472189274174254, + "y": 5.967591974035552, + "heading": 0.03431321832996816, + "angularVelocity": 1.1756711747039204e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058483, + "timestamp": 1.0914778462136874 + }, + { + "x": 3.0036427092645317, + "y": 5.968248462403853, + "heading": 0.034313226884761766, + "angularVelocity": 1.1756711737664598e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058489, + "timestamp": 1.1642430359612665 + }, + { + "x": 3.0600664911116384, + "y": 5.968904950772154, + "heading": 0.03431323543955538, + "angularVelocity": 1.1756711759331021e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058496, + "timestamp": 1.2370082257088457 + }, + { + "x": 3.116490272958745, + "y": 5.969561439140454, + "heading": 0.034313243994349, + "angularVelocity": 1.1756711756317827e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058518, + "timestamp": 1.309773415456425 + }, + { + "x": 3.1729140548058514, + "y": 5.970217927508755, + "heading": 0.03431325254914262, + "angularVelocity": 1.1756711755305748e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058523, + "timestamp": 1.382538605204004 + }, + { + "x": 3.229337836652958, + "y": 5.970874415877057, + "heading": 0.03431326110393624, + "angularVelocity": 1.175671175226974e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.00902201135705854, + "timestamp": 1.4553037949515832 + }, + { + "x": 3.2857616185000644, + "y": 5.971530904245357, + "heading": 0.03431326965872984, + "angularVelocity": 1.1756711738766866e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058546, + "timestamp": 1.5280689846991624 + }, + { + "x": 3.342185400347171, + "y": 5.972187392613658, + "heading": 0.03431327821352346, + "angularVelocity": 1.1756711754575316e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058565, + "timestamp": 1.6008341744467416 + }, + { + "x": 3.3986091821942774, + "y": 5.972843880981959, + "heading": 0.03431328676831708, + "angularVelocity": 1.1756711769845501e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058572, + "timestamp": 1.6735993641943208 + }, + { + "x": 3.455032964041384, + "y": 5.973500369350259, + "heading": 0.034313295323110705, + "angularVelocity": 1.175671175699485e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058579, + "timestamp": 1.7463645539419 + }, + { + "x": 3.5114567458884904, + "y": 5.97415685771856, + "heading": 0.03431330387790433, + "angularVelocity": 1.1756711766849268e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058603, + "timestamp": 1.819129743689479 + }, + { + "x": 3.567880527735597, + "y": 5.9748133460868615, + "heading": 0.03431331243269794, + "angularVelocity": 1.1756711756878859e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058601, + "timestamp": 1.8918949334370583 + }, + { + "x": 3.624304309582704, + "y": 5.975469834455162, + "heading": 0.03431332098749156, + "angularVelocity": 1.1756711758891969e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058617, + "timestamp": 1.9646601231846375 + }, + { + "x": 3.68072809142981, + "y": 5.976126322823463, + "heading": 0.03431332954228517, + "angularVelocity": 1.1756711749627294e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058617, + "timestamp": 2.0374253129322164 + }, + { + "x": 3.737151873276917, + "y": 5.976782811191764, + "heading": 0.03431333809707879, + "angularVelocity": 1.1756711754302844e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058624, + "timestamp": 2.1101905026797954 + }, + { + "x": 3.793575655124023, + "y": 5.977439299560064, + "heading": 0.03431334665187242, + "angularVelocity": 1.175671176839478e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058646, + "timestamp": 2.1829556924273743 + }, + { + "x": 3.84999943697113, + "y": 5.978095787928365, + "heading": 0.034313355206666014, + "angularVelocity": 1.1756711730438892e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.00902201135705865, + "timestamp": 2.2557208821749533 + }, + { + "x": 3.906423218818236, + "y": 5.9787522762966665, + "heading": 0.03431336376145962, + "angularVelocity": 1.1756711741363398e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058672, + "timestamp": 2.328486071922532 + }, + { + "x": 3.962847000665343, + "y": 5.979408764664967, + "heading": 0.03431337231625322, + "angularVelocity": 1.1756711736937194e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058674, + "timestamp": 2.401251261670111 + }, + { + "x": 4.019270782512449, + "y": 5.980065253033268, + "heading": 0.034313380871046816, + "angularVelocity": 1.17567117272878e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.00902201135705869, + "timestamp": 2.47401645141769 + }, + { + "x": 4.075694564359556, + "y": 5.9807217414015685, + "heading": 0.034313389425840425, + "angularVelocity": 1.1756711748988721e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058704, + "timestamp": 2.546781641165269 + }, + { + "x": 4.132118346206663, + "y": 5.981378229769869, + "heading": 0.034313397980634035, + "angularVelocity": 1.1756711734690936e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.00902201135705871, + "timestamp": 2.619546830912848 + }, + { + "x": 4.188542128053769, + "y": 5.98203471813817, + "heading": 0.03431340653542763, + "angularVelocity": 1.1756711733140912e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058714, + "timestamp": 2.692312020660427 + }, + { + "x": 4.244965909900876, + "y": 5.982691206506428, + "heading": 0.03431341509022124, + "angularVelocity": 1.1756711743485013e-7, + "velocityX": 0.7754227267576708, + "velocityY": 0.00902201135645878, + "timestamp": 2.765077210408006 + }, + { + "x": 4.30138969265204, + "y": 5.983347617167627, + "heading": 0.03431342364501558, + "angularVelocity": 1.1756712748463029e-7, + "velocityX": 0.7754227391819724, + "velocityY": 0.009020943441183957, + "timestamp": 2.837842400155585 + }, + { + "x": 4.337240695953369, + "y": 5.957952976226807, + "heading": -3.4992233566514893e-26, + "angularVelocity": -0.47156372111511136, + "velocityX": 0.4926944247063125, + "velocityY": -0.3489943615747365, + "timestamp": 2.910607589903164 + }, + { + "x": 4.337075774441293, + "y": 5.930386954424772, + "heading": -0.09015124251095946, + "angularVelocity": -1.224439774519884, + "velocityX": -0.002239974219267059, + "velocityY": -0.37440342007034494, + "timestamp": 2.9842341123234344 + }, + { + "x": 4.333264434450346, + "y": 5.912061346555676, + "heading": -0.20505051130417343, + "angularVelocity": -1.5605690044322955, + "velocityX": -0.05176585645578975, + "velocityY": -0.24889954416819945, + "timestamp": 3.057860634743705 + }, + { + "x": 4.325553986362593, + "y": 5.887325303672255, + "heading": -0.3021610086195407, + "angularVelocity": -1.31896080546962, + "velocityX": -0.10472378477610225, + "velocityY": -0.33596647064523955, + "timestamp": 3.1314871571639755 + }, + { + "x": 4.313628651056446, + "y": 5.857761645119009, + "heading": -0.3834128542416627, + "angularVelocity": -1.1035676132892005, + "velocityX": -0.16197064473690057, + "velocityY": -0.40153544648615047, + "timestamp": 3.205113679584246 + }, + { + "x": 4.297606686426737, + "y": 5.824666184248557, + "heading": -0.4508559142750475, + "angularVelocity": -0.9160158298447155, + "velocityX": -0.2176113186258289, + "velocityY": -0.4495046048968324, + "timestamp": 3.2787402020045167 + }, + { + "x": 4.277844157832715, + "y": 5.789044520818464, + "heading": -0.5065111263049437, + "angularVelocity": -0.7559125461910066, + "velocityX": -0.26841589069241406, + "velocityY": -0.4838156449486921, + "timestamp": 3.3523667244247872 + }, + { + "x": 4.254799811398479, + "y": 5.751649129927804, + "heading": -0.5522697074374715, + "angularVelocity": -0.6214958907244229, + "velocityX": -0.3129897444116032, + "velocityY": -0.5079065214733656, + "timestamp": 3.425993246845058 + }, + { + "x": 4.228951375108858, + "y": 5.713030269053256, + "heading": -0.5898366283888508, + "angularVelocity": -0.5102362534107197, + "velocityX": -0.35107506697211693, + "velocityY": -0.524523766776681, + "timestamp": 3.4996197692653284 + }, + { + "x": 4.2007493706625425, + "y": 5.673585962715614, + "heading": -0.6207093446974378, + "angularVelocity": -0.419315150216672, + "velocityX": -0.3830413758419039, + "velocityY": -0.5357350183197291, + "timestamp": 3.573246291685599 + }, + { + "x": 4.1705955025497214, + "y": 5.633604594294146, + "heading": -0.6461809126237779, + "angularVelocity": -0.34595641745707684, + "velocityX": -0.4095517093785753, + "velocityY": -0.5430294288958701, + "timestamp": 3.6468728141058695 + }, + { + "x": 4.138835739465679, + "y": 5.593298519659669, + "heading": -0.6673577962188777, + "angularVelocity": -0.28762574815389413, + "velocityX": -0.43136307460989803, + "velocityY": -0.5474396088464458, + "timestamp": 3.72049933652614 + }, + { + "x": 4.1057614035434415, + "y": 5.552829496334061, + "heading": -0.6851851845597893, + "angularVelocity": -0.2421326955950787, + "velocityX": -0.44921768453825217, + "velocityY": -0.549652788089118, + "timestamp": 3.7941258589464106 + }, + { + "x": 4.071614137002915, + "y": 5.512327581345269, + "heading": -0.700475200245641, + "angularVelocity": -0.20766994261353336, + "velocityX": -0.4637902948289247, + "velocityY": -0.5500995247011782, + "timestamp": 3.867752381366681 + }, + { + "x": 4.036592388923184, + "y": 5.471905240898815, + "heading": -0.7139354213530316, + "angularVelocity": -0.18281755901165134, + "velocityX": -0.47566755740304917, + "velocityY": -0.5490187383252648, + "timestamp": 3.9413789037869518 + }, + { + "x": 4.0008581978077, + "y": 5.431668194433558, + "heading": -0.7261965659955223, + "angularVelocity": -0.16653162799816065, + "velocityX": -0.4853440029600765, + "velocityY": -0.5465020639651851, + "timestamp": 4.015005426207223 + }, + { + "x": 3.964543737766948, + "y": 5.391724222217225, + "heading": -0.7378391147106159, + "angularVelocity": -0.15812981969508613, + "velocityX": -0.49322525153997593, + "velocityY": -0.5425215113152707, + "timestamp": 4.088631948627493 + }, + { + "x": 3.9277575195733885, + "y": 5.352190902296246, + "heading": -0.7494192294944804, + "angularVelocity": -0.15728183816374608, + "velocityX": -0.4996327000693894, + "velocityY": -0.536944006336704, + "timestamp": 4.162258471047764 + }, + { + "x": 3.89059042061129, + "y": 5.3132030416510085, + "heading": -0.761494708226674, + "angularVelocity": -0.16400990207394403, + "velocityX": -0.5048058463218383, + "velocityY": -0.5295355445784652, + "timestamp": 4.2358849934680345 + }, + { + "x": 3.8531219539867014, + "y": 5.274920426877835, + "heading": -0.7746519957947015, + "angularVelocity": -0.1787030968666938, + "velocityX": -0.5088990406298591, + "velocityY": -0.5199568513456381, + "timestamp": 4.309511515888305 + }, + { + "x": 3.8154274539330624, + "y": 5.23753642375436, + "heading": -0.7895355361070671, + "angularVelocity": -0.2021491688471734, + "velocityX": -0.5119690407007602, + "velocityY": -0.507751852112243, + "timestamp": 4.383138038308576 + }, + { + "x": 3.7775872299458775, + "y": 5.201287866945812, + "heading": -0.8068810412551409, + "angularVelocity": -0.23558772814317216, + "velocityX": -0.5139482722161929, + "velocityY": -0.49233014974734224, + "timestamp": 4.456764560728846 + }, + { + "x": 3.7396993230404707, + "y": 5.166466510716887, + "heading": -0.8275545372921488, + "angularVelocity": -0.2807887070776005, + "velocityX": -0.5145959045727775, + "velocityY": -0.4729458228402956, + "timestamp": 4.530391083149117 + }, + { + "x": 3.701898417706247, + "y": 5.133430805056498, + "heading": -0.8525964909440766, + "angularVelocity": -0.3401213697013267, + "velocityX": -0.5134142438298316, + "velocityY": -0.44869300592275085, + "timestamp": 4.604017605569387 + }, + { + "x": 3.664343129524347, + "y": 5.101026571041832, + "heading": -0.8792757408403825, + "angularVelocity": -0.36235922897480916, + "velocityX": -0.5100782564132129, + "velocityY": -0.44011631881374225, + "timestamp": 4.677644127989658 + }, + { + "x": 3.6270751479601078, + "y": 5.068774406382283, + "heading": -0.9064492634455257, + "angularVelocity": -0.3690724716024585, + "velocityX": -0.5061760400893075, + "velocityY": -0.4380509033884428, + "timestamp": 4.7512706504099285 + }, + { + "x": 3.590111595107405, + "y": 5.036682857394856, + "heading": -0.9341662862028731, + "angularVelocity": -0.37645432442313, + "velocityX": -0.5020412704230309, + "velocityY": -0.43586941135483154, + "timestamp": 4.824897172830199 + }, + { + "x": 3.55347222485786, + "y": 5.004762522170154, + "heading": -0.9624850330743058, + "angularVelocity": -0.38462697870999835, + "velocityX": -0.4976382021739686, + "velocityY": -0.4335439753964831, + "timestamp": 4.89852369525047 + }, + { + "x": 3.517180102788644, + "y": 4.973026350131823, + "heading": -0.9914744586380927, + "angularVelocity": -0.3937361783612594, + "velocityX": -0.49292185582330617, + "velocityY": -0.4310426595619462, + "timestamp": 4.97215021767074 + }, + { + "x": 3.4812625207112933, + "y": 4.941490315870616, + "heading": -1.0212172545129166, + "angularVelocity": -0.40396850071293, + "velocityX": -0.48783483039342795, + "velocityY": -0.42832437584372535, + "timestamp": 5.045776740091011 + }, + { + "x": 3.445752274995712, + "y": 4.9101744053392995, + "heading": -1.051814154831974, + "angularVelocity": -0.41556900031765764, + "velocityX": -0.4823023626307331, + "velocityY": -0.4253346416738305, + "timestamp": 5.119403262511281 + }, + { + "x": 3.4106895099559638, + "y": 4.879104067704057, + "heading": -1.0833902078498774, + "angularVelocity": -0.4288679130824993, + "velocityX": -0.4762246523013149, + "velocityY": -0.4219992553483426, + "timestamp": 5.193029784931552 + }, + { + "x": 3.376124474151927, + "y": 4.848312423976759, + "heading": -1.1161042305606605, + "angularVelocity": -0.44432388812345175, + "velocityX": -0.4694644629109969, + "velocityY": -0.4182140173826824, + "timestamp": 5.266656307351822 + }, + { + "x": 3.3421218274128583, + "y": 4.817843776695172, + "heading": -1.1501637008410697, + "angularVelocity": -0.4625978405715368, + "velocityX": -0.4618260597040901, + "velocityY": -0.4138270595977395, + "timestamp": 5.340282829772093 + }, + { + "x": 3.3087677521953345, + "y": 4.787759503770135, + "heading": -1.1858495459870186, + "angularVelocity": -0.48468736499938486, + "velocityX": -0.45301712101970637, + "velocityY": -0.40860646321595784, + "timestamp": 5.413909352192364 + }, + { + "x": 3.2761825599174124, + "y": 4.758148688152823, + "heading": -1.2235604524642407, + "angularVelocity": -0.5121918737647652, + "velocityX": -0.44257410518347007, + "velocityY": -0.4021759366589302, + "timestamp": 5.487535874612634 + }, + { + "x": 3.244545279648596, + "y": 4.729149218235016, + "heading": -1.2639000345369864, + "angularVelocity": -0.547894708953951, + "velocityX": -0.4296995054068451, + "velocityY": -0.3938726014013467, + "timestamp": 5.561162397032905 + }, + { + "x": 3.2141486367937797, + "y": 4.700995814255907, + "heading": -1.3078734515357762, + "angularVelocity": -0.5972496805944914, + "velocityX": -0.4128490910015826, + "velocityY": -0.382381281278044, + "timestamp": 5.634788919453175 + }, + { + "x": 3.185551521789466, + "y": 4.674155611334741, + "heading": -1.357437524753386, + "angularVelocity": -0.6731823205595795, + "velocityX": -0.38840779197850694, + "velocityY": -0.36454530295424753, + "timestamp": 5.708415441873446 + }, + { + "x": 3.161389111124608, + "y": 4.65130116728667, + "heading": -1.4227635315487523, + "angularVelocity": -0.8872618812888667, + "velocityX": -0.3281753622279693, + "velocityY": -0.3104104784090629, + "timestamp": 5.782041964293716 + }, + { + "x": 3.1613881587982178, + "y": 4.647716999053955, + "heading": -1.5707963267948966, + "angularVelocity": -2.010590618434374, + "velocityX": -0.00001293455617664302, + "velocityY": -0.048680395527252375, + "timestamp": 5.855668486713987 + }, + { + "x": 3.1590242751538056, + "y": 4.648068652143942, + "heading": -1.71822885135636, + "angularVelocity": -2.0416470093627317, + "velocityX": -0.032735083303029204, + "velocityY": 0.0048696953514198, + "timestamp": 5.927881030746015 + }, + { + "x": 3.147722843506569, + "y": 4.651261007949863, + "heading": -1.8460861775851984, + "angularVelocity": -1.7705694757427723, + "velocityX": -0.15650233347580403, + "velocityY": 0.04420777371456313, + "timestamp": 6.000093574778043 + }, + { + "x": 3.1294383579479246, + "y": 4.658664086881976, + "heading": -1.9555006668792347, + "angularVelocity": -1.5151728935835482, + "velocityX": -0.25320373078858216, + "velocityY": 0.10251790781432463, + "timestamp": 6.07230611881007 + }, + { + "x": 3.105950392035797, + "y": 4.670811298853881, + "heading": -2.048080530965875, + "angularVelocity": -1.2820468427975569, + "velocityX": -0.325261576461146, + "velocityY": 0.16821470749621126, + "timestamp": 6.144518662842098 + }, + { + "x": 3.078734489629141, + "y": 4.687681461713096, + "heading": -2.125691691683944, + "angularVelocity": -1.0747600954710448, + "velocityX": -0.37688607667090807, + "velocityY": 0.23361817652806605, + "timestamp": 6.216731206874126 + }, + { + "x": 3.048937913872785, + "y": 4.708930614799334, + "heading": -2.190285310241282, + "angularVelocity": -0.8944930472009076, + "velocityX": -0.41262326588495246, + "velocityY": 0.2942584750485111, + "timestamp": 6.288943750906154 + }, + { + "x": 3.0174119547024816, + "y": 4.734060139937329, + "heading": -2.243768219004051, + "angularVelocity": -0.7406318317638603, + "velocityX": -0.4365717839316291, + "velocityY": 0.347993904311833, + "timestamp": 6.361156294938182 + }, + { + "x": 2.9847672349131935, + "y": 4.762525988198673, + "heading": -2.2879169282753247, + "angularVelocity": -0.6113717479845765, + "velocityX": -0.4520643916770092, + "velocityY": 0.3941953388142518, + "timestamp": 6.4333688389702095 + }, + { + "x": 2.9514318737909195, + "y": 4.793802004717416, + "heading": -2.3243308683279778, + "angularVelocity": -0.5042605899122328, + "velocityX": -0.4616283994577112, + "velocityY": 0.4331105757037482, + "timestamp": 6.505581383002237 + }, + { + "x": 2.9177026495619596, + "y": 4.827411588267587, + "heading": -2.3544162803840454, + "angularVelocity": -0.4166230737242065, + "velocityX": -0.46708261952383673, + "velocityY": 0.46542583426037293, + "timestamp": 6.577793927034265 + }, + { + "x": 2.88378589769779, + "y": 4.862939604927141, + "heading": -2.379391061920221, + "angularVelocity": -0.3458510134347111, + "velocityX": -0.4696795040087044, + "velocityY": 0.4919923142964852, + "timestamp": 6.650006471066293 + }, + { + "x": 2.8498283989976017, + "y": 4.900033117168976, + "heading": -2.4003021659557313, + "angularVelocity": -0.289577168562792, + "velocityX": -0.47024376658337114, + "velocityY": 0.5136713120837215, + "timestamp": 6.722219015098321 + }, + { + "x": 2.8159399738054236, + "y": 4.938396448572123, + "heading": -2.4180494138871094, + "angularVelocity": -0.2457640589910084, + "velocityX": -0.46928723598420935, + "velocityY": 0.5312557799671519, + "timestamp": 6.794431559130349 + }, + { + "x": 2.7822098245267197, + "y": 4.977783840614619, + "heading": -2.4334117988726813, + "angularVelocity": -0.2127384541217516, + "velocityX": -0.4670954296215342, + "velocityY": 0.5454369814893605, + "timestamp": 6.866644103162376 + }, + { + "x": 2.7487184813842567, + "y": 5.0179914540575155, + "heading": -2.4470741047795617, + "angularVelocity": -0.18919574278978962, + "velocityX": -0.46378843996424124, + "velocityY": 0.5567954152821849, + "timestamp": 6.938856647194404 + }, + { + "x": 2.7155468563385656, + "y": 5.0588495367779895, + "heading": -2.459652889491748, + "angularVelocity": -0.17419113092882704, + "velocityX": -0.4593609807041143, + "velocityY": 0.5658031200556053, + "timestamp": 7.011069191226432 + }, + { + "x": 2.682783562068564, + "y": 5.100215023933873, + "heading": -2.471721683170927, + "angularVelocity": -0.16712877022898326, + "velocityX": -0.45370641221933317, + "velocityY": 0.5728296615271857, + "timestamp": 7.08328173525846 + }, + { + "x": 2.650531366839192, + "y": 5.141964497495229, + "heading": -2.4838357664761026, + "angularVelocity": -0.16775594140268155, + "velocityX": -0.44662870781934594, + "velocityY": 0.5781471089405039, + "timestamp": 7.155494279290488 + }, + { + "x": 2.618913440143494, + "y": 5.183987197473736, + "heading": -2.496557238457018, + "angularVelocity": -0.17616706559006176, + "velocityX": -0.4378453511023647, + "velocityY": 0.5819307509768665, + "timestamp": 7.2277068233225155 + }, + { + "x": 2.5880798824866296, + "y": 5.226177564560792, + "heading": -2.5104813496102354, + "angularVelocity": -0.1928212243435465, + "velocityX": -0.4269834011551906, + "velocityY": 0.5842526066986774, + "timestamp": 7.299919367354543 + }, + { + "x": 2.5582148956282778, + "y": 5.268426535906725, + "heading": -2.5262653286045555, + "angularVelocity": -0.21857669198470553, + "velocityX": -0.41357062347929907, + "velocityY": 0.585064159035808, + "timestamp": 7.372131911386571 + }, + { + "x": 2.5295447873325214, + "y": 5.31061044475372, + "heading": -2.5446612115646436, + "angularVelocity": -0.2547463630685689, + "velocityX": -0.39702393372321826, + "velocityY": 0.5841631729285931, + "timestamp": 7.444344455418599 + }, + { + "x": 2.5023467091367184, + "y": 5.352575812308809, + "heading": -2.566554449178874, + "angularVelocity": -0.3031777637486395, + "velocityX": -0.3766392468286359, + "velocityY": 0.5811368110293486, + "timestamp": 7.516556999450627 + }, + { + "x": 2.476956466389222, + "y": 5.394117632986223, + "heading": -2.593007817119188, + "angularVelocity": -0.36632649209230705, + "velocityX": -0.3516043242602833, + "velocityY": 0.5752715298188288, + "timestamp": 7.5887695434826545 + }, + { + "x": 2.4521989817386887, + "y": 5.435268589005595, + "heading": -2.621259955334882, + "angularVelocity": -0.39123588005934157, + "velocityX": -0.3428418840852991, + "velocityY": 0.5698588322981821, + "timestamp": 7.660982087514682 + }, + { + "x": 2.427559065081996, + "y": 5.476070836186979, + "heading": -2.6500802362174434, + "angularVelocity": -0.39910352514071706, + "velocityX": -0.34121380138282903, + "velocityY": 0.5650299089765801, + "timestamp": 7.73319463154671 + }, + { + "x": 2.403044029416898, + "y": 5.516501717159452, + "heading": -2.6795300674706377, + "angularVelocity": -0.407821544690809, + "velocityX": -0.3394844482175417, + "velocityY": 0.559887226165873, + "timestamp": 7.805407175578738 + }, + { + "x": 2.378663293884139, + "y": 5.556534603961008, + "heading": -2.7096829003029756, + "angularVelocity": -0.41755671727843513, + "velocityX": -0.33762465870120245, + "velocityY": 0.5543757990828, + "timestamp": 7.877619719610766 + }, + { + "x": 2.354428772799968, + "y": 5.59613781059984, + "heading": -2.74062708945702, + "angularVelocity": -0.42851542718561086, + "velocityX": -0.33559987961957327, + "velocityY": 0.5484255840822709, + "timestamp": 7.949832263642794 + }, + { + "x": 2.330355697409695, + "y": 5.635273029678329, + "heading": -2.77247058331099, + "angularVelocity": -0.4409690072662031, + "velocityX": -0.3333641781072918, + "velocityY": 0.5419448878733852, + "timestamp": 8.02204480767482 + }, + { + "x": 2.306463861338364, + "y": 5.673893057942382, + "heading": -2.8053478457859313, + "angularVelocity": -0.45528464501069815, + "velocityX": -0.33085437428619124, + "velocityY": 0.534810520550616, + "timestamp": 8.094257351706847 + }, + { + "x": 2.282779542094817, + "y": 5.711938365696433, + "heading": -2.839430372846894, + "angularVelocity": -0.47197516051846805, + "velocityX": -0.3279806792714956, + "velocityY": 0.5268517854346452, + "timestamp": 8.166469895738874 + }, + { + "x": 2.259338596376033, + "y": 5.749331681355029, + "heading": -2.8749434001407064, + "angularVelocity": -0.4917847414164131, + "velocityX": -0.3246104403742845, + "velocityY": 0.5178229926647014, + "timestamp": 8.238682439770901 + }, + { + "x": 2.2361917393720554, + "y": 5.785968937848207, + "heading": -2.9121940133772717, + "angularVelocity": -0.5158468481603996, + "velocityX": -0.3205378970405998, + "velocityY": 0.5073530781151929, + "timestamp": 8.310894983802928 + }, + { + "x": 2.2134142469672544, + "y": 5.821702966064213, + "heading": -2.9516221084722227, + "angularVelocity": -0.5460006377488126, + "velocityX": -0.3154229325406202, + "velocityY": 0.4948451642993865, + "timestamp": 8.383107527834955 + }, + { + "x": 2.191125686501144, + "y": 5.856311001786043, + "heading": -2.9939026154296866, + "angularVelocity": -0.5855008644856994, + "velocityX": -0.3086521983801695, + "velocityY": 0.4792524094772386, + "timestamp": 8.455320071866982 + }, + { + "x": 2.169536365545648, + "y": 5.8894197426408175, + "heading": -3.0401828782490323, + "angularVelocity": -0.6408895218927577, + "velocityX": -0.29896912295350214, + "velocityY": 0.4584901598272232, + "timestamp": 8.527532615899009 + }, + { + "x": 2.1490856813219974, + "y": 5.9202865649088325, + "heading": -3.092780352693255, + "angularVelocity": -0.7283703288571992, + "velocityX": -0.28320127060722483, + "velocityY": 0.42744404980837086, + "timestamp": 8.599745159931036 + }, + { + "x": 2.131117343902588, + "y": 5.946754455566406, + "heading": -3.158540067224243, + "angularVelocity": -0.9106411553901581, + "velocityX": -0.24882570833455084, + "velocityY": 0.3665276028197312, + "timestamp": 8.671957703963063 + }, + { + "x": 2.131117343902588, + "y": 5.946754455566406, + "heading": -3.158540067224243, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": 0, + "timestamp": 8.74417024799509 + } + ] +} \ No newline at end of file diff --git a/simgui.json b/simgui.json index b30b8d3b..d405821f 100644 --- a/simgui.json +++ b/simgui.json @@ -1,7 +1,9 @@ { "NTProvider": { "types": { - "/FMSInfo": "FMSInfo" + "/FMSInfo": "FMSInfo", + "/SmartDashboard/reset Kicker value": "Command", + "/SmartDashboard/reset Pivot value": "Command" } }, "NetworkTables": { @@ -29,6 +31,7 @@ }, "Clients": { "open": true - } + }, + "visible": true } } diff --git a/src/main/deploy/choreo/amp 4 local sgmt 1.traj b/src/main/deploy/choreo/amp 4 local sgmt 1.traj index ddf10560..36ca3a7c 100644 --- a/src/main/deploy/choreo/amp 4 local sgmt 1.traj +++ b/src/main/deploy/choreo/amp 4 local sgmt 1.traj @@ -4,172 +4,280 @@ "x": 0.7470832467079163, "y": 6.625458717346191, "heading": 1.056495497952334, - "angularVelocity": 1.8216849722605286e-31, - "velocityX": 0, - "velocityY": 0, + "angularVelocity": -4.3186177103351096e-19, + "velocityX": 7.079282317781465e-18, + "velocityY": -1.220945161386867e-18, "timestamp": 0 }, { - "x": 0.7875510590073889, - "y": 6.647513625582565, - "heading": 0.9955054911508737, - "angularVelocity": -1.1259767405457384, - "velocityX": 0.7471029727592392, - "velocityY": 0.4071702069138493, - "timestamp": 0.05416631143899068 - }, - { - "x": 0.8691098883744304, - "y": 6.691702168377954, - "heading": 0.8785332104893699, - "angularVelocity": -2.159502420489772, - "velocityX": 1.505711339767041, - "velocityY": 0.8157938323926509, - "timestamp": 0.10833262287798136 - }, - { - "x": 0.9931586796399057, - "y": 6.758536729383142, - "heading": 0.7242571551382772, - "angularVelocity": -2.848192008142514, - "velocityX": 2.290146549949145, - "velocityY": 1.2338769103830776, - "timestamp": 0.16249893431697204 - }, - { - "x": 1.1542844409320208, - "y": 6.847841004747936, - "heading": 0.6582721283096681, - "angularVelocity": -1.2181930996525292, - "velocityX": 2.974648947133801, - "velocityY": 1.6487051267166433, - "timestamp": 0.21666524575596272 - }, - { - "x": 1.3362429767095947, - "y": 6.9407596540671515, - "heading": 0.6582720385538592, - "angularVelocity": -0.0000016570411839532803, - "velocityX": 3.3592565368332274, - "velocityY": 1.7154324680917685, - "timestamp": 0.2708315571949534 - }, - { - "x": 1.5182021069161247, - "y": 7.033677139333042, - "heading": 0.6582719487984567, - "angularVelocity": -0.0000016570336808247504, - "velocityX": 3.359267510978228, - "velocityY": 1.7154109777356332, - "timestamp": 0.3249978686339441 - }, - { - "x": 1.7157951910460691, - "y": 7.085636206134739, - "heading": 0.6582717690726799, - "angularVelocity": -0.0000033180361009936695, - "velocityX": 3.647896245482769, - "velocityY": 0.9592506009979895, - "timestamp": 0.3791641800729348 - }, - { - "x": 1.8967940454930174, - "y": 7.095126843263903, - "heading": 0.5939474144774982, - "angularVelocity": -1.1875343342814295, - "velocityX": 3.3415392268460664, - "velocityY": 0.1752129114394957, - "timestamp": 0.4333304915119255 - }, - { - "x": 2.0516314923077155, - "y": 7.067103472452243, - "heading": 0.46543090394591263, - "angularVelocity": -2.3726280619336353, - "velocityX": 2.8585562262089095, - "velocityY": -0.5173579309202905, - "timestamp": 0.4874968029509162 - }, - { - "x": 2.1749652200442315, - "y": 7.00220729212556, - "heading": 0.30618928904490067, - "angularVelocity": -2.9398644779490204, - "velocityX": 2.2769452905322995, - "velocityY": -1.198091186249191, - "timestamp": 0.5416631143899069 - }, - { - "x": 2.2610747814178467, - "y": 6.9050798416137695, + "x": 0.7613714426488418, + "y": 6.628614589829769, + "heading": 1.0345345122436784, + "angularVelocity": -0.5024975798156311, + "velocityX": 0.32693358920665033, + "velocityY": 0.07221070612415084, + "timestamp": 0.04370366463598273 + }, + { + "x": 0.7899910246485671, + "y": 6.635101738465351, + "heading": 0.9912579578255775, + "angularVelocity": -0.9902271303461743, + "velocityX": 0.6548554277565509, + "velocityY": 0.14843488960512216, + "timestamp": 0.08740732927196546 + }, + { + "x": 0.8329823079677272, + "y": 6.6450825712167525, + "heading": 0.9272173132517384, + "angularVelocity": -1.465338092520316, + "velocityX": 0.9836997349591596, + "velocityY": 0.22837519083431007, + "timestamp": 0.13111099390794817 + }, + { + "x": 0.8904118185530745, + "y": 6.658650997634064, + "heading": 0.8429907149485981, + "angularVelocity": -1.927220497518482, + "velocityX": 1.3140662473888705, + "velocityY": 0.3104642718253751, + "timestamp": 0.1748146585439309 + }, + { + "x": 0.962426123217969, + "y": 6.675814256462545, + "heading": 0.7397363294626891, + "angularVelocity": -2.362602457847324, + "velocityX": 1.6477864102408268, + "velocityY": 0.39271898527129734, + "timestamp": 0.21851832317991365 + }, + { + "x": 1.0493031568120121, + "y": 6.696537442602017, + "heading": 0.6202474925482772, + "angularVelocity": -2.7340690514092785, + "velocityX": 1.9878661049974005, + "velocityY": 0.47417502198223827, + "timestamp": 0.2622219878158964 + }, + { + "x": 1.1514382857503924, + "y": 6.720845269246716, + "heading": 0.4905752358986126, + "angularVelocity": -2.9670797112720706, + "velocityX": 2.3369923275104343, + "velocityY": 0.5561965305921692, + "timestamp": 0.30592565245187914 + }, + { + "x": 1.269055326004296, + "y": 6.748845004507445, + "heading": 0.3629054511567606, + "angularVelocity": -2.921260397846258, + "velocityX": 2.6912397675014597, + "velocityY": 0.6406724812196789, + "timestamp": 0.3496293170878619 + }, + { + "x": 1.400011708453663, + "y": 6.779996215891522, + "heading": 0.2672618294840015, + "angularVelocity": -2.188457706450791, + "velocityX": 2.996462277022566, + "velocityY": 0.7127825925706782, + "timestamp": 0.3933329817238446 + }, + { + "x": 1.5437969816492523, + "y": 6.812790254409381, + "heading": 0.2092081541239441, + "angularVelocity": -1.3283479965261176, + "velocityX": 3.2900049548066255, + "velocityY": 0.7503727385565244, + "timestamp": 0.43703664635982736 + }, + { + "x": 1.6999373892672376, + "y": 6.847694256712915, + "heading": 0.1915003494540967, + "angularVelocity": -0.40517894362723794, + "velocityX": 3.572707435829763, + "velocityY": 0.7986516140981349, + "timestamp": 0.4807403109958101 + }, + { + "x": 1.8624433107711578, + "y": 6.878112633593708, + "heading": 0.1915002067487222, + "angularVelocity": -0.0000032652953862886492, + "velocityX": 3.7183591549465618, + "velocityY": 0.6960143304721129, + "timestamp": 0.5244439756317928 + }, + { + "x": 2.015535426159542, + "y": 6.900266962585095, + "heading": 0.1561712001056634, + "angularVelocity": -0.8083762983569384, + "velocityX": 3.502958313988489, + "velocityY": 0.5069215402397582, + "timestamp": 0.5681476402677755 + }, + { + "x": 2.1557445526123047, + "y": 6.919295787811279, + "heading": 0.15617120010566338, + "angularVelocity": 8.052388519770604e-18, + "velocityX": 3.208177795171024, + "velocityY": 0.4354057121910402, + "timestamp": 0.6118513049037583 + }, + { + "x": 2.2979527331593355, + "y": 6.936286453257653, + "heading": 0.15617120010566338, + "angularVelocity": -1.1282442958129174e-19, + "velocityX": 2.796432452004192, + "velocityY": 0.33411051356290106, + "timestamp": 0.6627047344028709 + }, + { + "x": 2.4202451824897016, + "y": 6.945011127796279, + "heading": 0.15617120010566338, + "angularVelocity": -1.1282442959053082e-19, + "velocityX": 2.404802400445729, + "velocityY": 0.1715651161497845, + "timestamp": 0.7135581639019836 + }, + { + "x": 2.52317206188046, + "y": 6.94425224667938, + "heading": 0.15617120010566338, + "angularVelocity": -1.128244295905312e-19, + "velocityX": 2.0239909167296966, + "velocityY": -0.014922909317535413, + "timestamp": 0.7644115934010962 + }, + { + "x": 2.6070605614479327, + "y": 6.933368973549914, + "heading": 0.15617120010566338, + "angularVelocity": -1.1282442959052985e-19, + "velocityX": 1.6496134163171314, + "velocityY": -0.21401256978465483, + "timestamp": 0.8152650229002089 + }, + { + "x": 2.672125506615784, + "y": 6.911967284292252, + "heading": 0.15617120010566338, + "angularVelocity": -1.1282442956919517e-19, + "velocityX": 1.2794603197604928, + "velocityY": -0.4208504611874918, + "timestamp": 0.8661184523993215 + }, + { + "x": 2.7185182571411133, + "y": 6.8797807693481445, "heading": 0.15617120010566338, - "angularVelocity": -2.7695828819396024, - "velocityX": 1.5897254047029348, - "velocityY": -1.7931339227554435, - "timestamp": 0.5958294258288975 - }, - { - "x": 2.3021420961741277, - "y": 6.690129168178249, - "heading": -0.009663328888242836, - "angularVelocity": -2.0165575251161725, - "velocityX": 0.4993809377969598, - "velocityY": -2.6138127003762723, - "timestamp": 0.6780658742953757 - }, - { - "x": 2.248466001407306, - "y": 6.41546750147008, - "heading": -0.10873685342712126, - "angularVelocity": -1.204739825057738, - "velocityX": -0.6527044366306908, - "velocityY": -3.3399018541045957, - "timestamp": 0.7603023227618538 - }, - { - "x": 2.085165640858422, - "y": 6.151744237063873, - "heading": -0.10873683701731249, - "angularVelocity": 1.9954422992985155e-7, - "velocityX": -1.9857418893211336, - "velocityY": -3.2068902454330503, - "timestamp": 0.842538771228332 - }, - { - "x": 1.8574534262453974, - "y": 5.941116320042222, - "heading": -0.10873681487910887, - "angularVelocity": 2.692018447413834e-7, - "velocityX": -2.7689937838918954, - "velocityY": -2.5612477307736583, - "timestamp": 0.9247752196948101 - }, - { - "x": 1.629741023592729, - "y": 5.730488606312599, - "heading": -0.10873679274080142, - "angularVelocity": 2.6920310704244503e-7, - "velocityX": -2.76899607046491, - "velocityY": -2.5612452587307564, - "timestamp": 1.0070116681612884 - }, - { - "x": 1.4644902813281564, - "y": 5.577695792451846, - "heading": 0.0012781207724603802, - "angularVelocity": 1.337787751839788, - "velocityX": -2.009458644507651, - "velocityY": -1.8579695099921079, - "timestamp": 1.0892481166277665 + "angularVelocity": -1.0748383020939867e-19, + "velocityX": 0.9122836155256804, + "velocityY": -0.6329271252919907, + "timestamp": 0.9169718818984341 + }, + { + "x": 2.742166614394153, + "y": 6.787220144570856, + "heading": 0.15109105455160673, + "angularVelocity": -0.05590353076450862, + "velocityX": 0.26023401360416243, + "velocityY": -1.0185664327447723, + "timestamp": 1.0078453112611694 + }, + { + "x": 2.7063747461891747, + "y": 6.659930157895132, + "heading": 0.14109258501335736, + "angularVelocity": -0.11002632571879596, + "velocityX": -0.39386505446064113, + "velocityY": -1.4007393312694816, + "timestamp": 1.0987187406239047 + }, + { + "x": 2.6108392240840024, + "y": 6.498432494186991, + "heading": 0.12644294397632044, + "angularVelocity": -0.16120929010568202, + "velocityX": -1.0513031452112058, + "velocityY": -1.7771714442897992, + "timestamp": 1.18959216998664 + }, + { + "x": 2.4549786573639274, + "y": 6.303757740845773, + "heading": 0.10766682633359297, + "angularVelocity": -0.20661834569682533, + "velocityX": -1.715139043536396, + "velocityY": -2.1422626471390678, + "timestamp": 1.2804655993493754 + }, + { + "x": 2.237238983313492, + "y": 6.078890290802275, + "heading": 0.08626225710840621, + "angularVelocity": -0.23554265944719108, + "velocityX": -2.396076340217028, + "velocityY": -2.4745126448998374, + "timestamp": 1.3713390287121108 + }, + { + "x": 1.9563226558789764, + "y": 5.880741504295724, + "heading": 0.08626219926626796, + "angularVelocity": -6.365132100594054e-7, + "velocityX": -3.0912922446581543, + "velocityY": -2.1804920084573034, + "timestamp": 1.4622124580748461 + }, + { + "x": 1.7274737983405954, + "y": 5.727569361833653, + "heading": 0.0766627418429575, + "angularVelocity": -0.10563547002277199, + "velocityX": -2.5183253140463755, + "velocityY": -1.6855547714685755, + "timestamp": 1.5530858874375815 + }, + { + "x": 1.5549556420919104, + "y": 5.613995417389302, + "heading": 0.06722021492045382, + "angularVelocity": -0.10390855708563887, + "velocityX": -1.8984444348419232, + "velocityY": -1.2498036581298453, + "timestamp": 1.6439593168003168 + }, + { + "x": 1.4396298957459885, + "y": 5.538759046250953, + "heading": 0.06010941807852643, + "angularVelocity": -0.07824946072569235, + "velocityX": -1.269081041121286, + "velocityY": -0.8279248584097305, + "timestamp": 1.7348327461630522 }, { "x": 1.3818649053573608, "y": 5.5012993812561035, "heading": 0.05628563945524819, - "angularVelocity": 0.6688946289455666, - "velocityX": -1.0047293810903806, - "velocityY": -0.9289848068631428, - "timestamp": 1.1714845650942447 + "angularVelocity": -0.0420780711159847, + "velocityX": -0.6356642507464983, + "velocityY": -0.4122180185951169, + "timestamp": 1.8257061755257875 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/amp 4 local sgmt 2.traj b/src/main/deploy/choreo/amp 4 local sgmt 2.traj index 88093dbe..822513bf 100644 --- a/src/main/deploy/choreo/amp 4 local sgmt 2.traj +++ b/src/main/deploy/choreo/amp 4 local sgmt 2.traj @@ -4,100 +4,154 @@ "x": 1.3818649053573608, "y": 5.5012993812561035, "heading": 0.05628563945524819, - "angularVelocity": 8.108539434054397e-29, - "velocityX": -3.383351472481295e-31, - "velocityY": 2.5568610146599268e-30, - "timestamp": 1.2537210135607229 - }, - { - "x": 1.4659695573316451, - "y": 5.505361506655373, - "heading": 0.050154371824367786, - "angularVelocity": -0.0866134750258778, - "velocityX": 1.188106051128092, - "velocityY": 0.05738369583633677, - "timestamp": 1.3245098559325195 - }, - { - "x": 1.6341788550204006, - "y": 5.5134857557639885, - "heading": 0.03789188378734141, - "angularVelocity": -0.17322628293059847, - "velocityX": 2.3762120138268115, - "velocityY": 0.11476736779992304, - "timestamp": 1.395298698304316 - }, - { - "x": 1.8864927796012139, - "y": 5.525672125057411, - "heading": 0.019498324963128963, - "angularVelocity": -0.25983697724008453, - "velocityX": 3.5643177106303305, - "velocityY": 0.1721509899740557, - "timestamp": 1.4660875406761127 - }, - { - "x": 2.1102347064408926, - "y": 5.533923842143074, - "heading": 0.00011174721946555298, - "angularVelocity": -0.2738648789005671, - "velocityX": 3.160694812108141, - "velocityY": 0.11656804673148825, - "timestamp": 1.5368763830479093 - }, - { - "x": 2.249872007587195, - "y": 5.538113436672261, - "heading": -0.013143603106743272, - "angularVelocity": -0.18725197195045482, - "velocityX": 1.9725891322378306, - "velocityY": 0.0591843910539215, - "timestamp": 1.607665225419706 - }, - { - "x": 2.3054046630859375, - "y": 5.538240909576416, + "angularVelocity": 1.3994093358769211e-21, + "velocityX": -9.902038153093951e-18, + "velocityY": -5.706702901855187e-18, + "timestamp": 0 + }, + { + "x": 1.4353793564287938, + "y": 5.499850139119754, + "heading": 0.05330774673729568, + "angularVelocity": -0.037162247354032396, + "velocityX": 0.6678270361261427, + "velocityY": -0.018085639694878305, + "timestamp": 0.08013220216697658 + }, + { + "x": 1.5424082559070336, + "y": 5.49695165466464, + "heading": 0.04735195633823623, + "angularVelocity": -0.07432455664514218, + "velocityX": 1.335654038999412, + "velocityY": -0.036171281666197166, + "timestamp": 0.16026440433395317 + }, + { + "x": 1.7029515984698416, + "y": 5.492603927569323, + "heading": 0.03841826071733368, + "angularVelocity": -0.11148696003994531, + "velocityX": 2.003480975454455, + "velocityY": -0.05425692764886357, + "timestamp": 0.24039660650092998 + }, + { + "x": 1.9170093681982305, + "y": 5.486806957092212, + "heading": 0.026506650941055532, + "angularVelocity": -0.1486494749196728, + "velocityX": 2.671307713250448, + "velocityY": -0.07234258288609449, + "timestamp": 0.3205288086679068 + }, + { + "x": 2.1592197171124052, + "y": 5.478791804392635, + "heading": 0.011195918948050757, + "angularVelocity": -0.19106840419911095, + "velocityX": 3.0226343762457084, + "velocityY": -0.10002411618334738, + "timestamp": 0.4006610108348836 + }, + { + "x": 2.3479156387807323, + "y": 5.472225891398255, + "heading": -0.0011368905306301479, + "angularVelocity": -0.1539057850049126, + "velocityX": 2.3548076374480407, + "velocityY": -0.08193850682774677, + "timestamp": 0.4807932130018604 + }, + { + "x": 2.483097117399865, + "y": 5.467109219705175, + "heading": -0.010491771187470106, + "angularVelocity": -0.11674308709682761, + "velocityX": 1.6869807014344356, + "velocityY": -0.06385287755374197, + "timestamp": 0.5609254151688372 + }, + { + "x": 2.564764147705788, + "y": 5.463441789903776, + "heading": -0.01686872035025771, + "angularVelocity": -0.0795803558412077, + "velocityX": 1.0191536997292066, + "velocityY": -0.04576724091218098, + "timestamp": 0.641057617335814 + }, + { + "x": 2.59291672706604, + "y": 5.461223602294922, "heading": -0.020267736871838693, - "angularVelocity": -0.10063921836266371, - "velocityX": 0.784483170484331, - "velocityY": 0.0018007485344283746, - "timestamp": 1.6784540677915025 + "angularVelocity": -0.042417610269816795, + "velocityX": 0.3513266651724994, + "velocityY": -0.027681600516002228, + "timestamp": 0.7211898195027908 + }, + { + "x": 2.5441490516673415, + "y": 5.460719866140607, + "heading": -0.019850376221801108, + "angularVelocity": 0.004155972098076838, + "velocityX": -0.48561621280493705, + "velocityY": -0.0050160775864576445, + "timestamp": 0.8216141342451149 + }, + { + "x": 2.4113319639164366, + "y": 5.462492299555472, + "heading": -0.014755896628636694, + "angularVelocity": 0.050729543001973076, + "velocityX": -1.3225590644227596, + "velocityY": 0.017649444951779485, + "timestamp": 0.9220384489874389 + }, + { + "x": 2.194465469101637, + "y": 5.466540902467684, + "heading": -0.004984302193896615, + "angularVelocity": 0.09730307306365722, + "velocityX": -2.1595018633809024, + "velocityY": 0.040314966774733244, + "timestamp": 1.022462763729763 }, { - "x": 2.206836259942782, - "y": 5.529745448091516, - "heading": -0.017578724891835627, - "angularVelocity": 0.025983583153922206, - "velocityX": -0.9524540308729255, - "velocityY": -0.08209057139403513, - "timestamp": 1.7819429501397848 + "x": 1.8935495831026135, + "y": 5.472865675007542, + "heading": 0.009464396467489124, + "angularVelocity": 0.14387649742454536, + "velocityX": -2.9964445042133008, + "velocityY": 0.0629804898951702, + "timestamp": 1.122887078472087 }, { - "x": 1.9285141868544358, - "y": 5.512568166846942, - "heading": -0.0017855827044546864, - "angularVelocity": 0.15260713836130316, - "velocityX": -2.689391041558251, - "velocityY": -0.1659818992610695, - "timestamp": 1.885431832488067 + "x": 1.641401371888256, + "y": 5.479694173969964, + "heading": 0.023495626181635927, + "angularVelocity": 0.13971944693024915, + "velocityX": -2.510828297522737, + "velocityY": 0.06799647057530608, + "timestamp": 1.223311393214411 }, { - "x": 1.5690068282385219, - "y": 5.495204505355553, - "heading": 0.024422680398419737, - "angularVelocity": 0.25324713638971313, - "velocityX": -3.4738742023131053, - "velocityY": -0.16778286804715686, - "timestamp": 1.9889207148363492 + "x": 1.473302552055367, + "y": 5.484246507552045, + "heading": 0.03284977683623274, + "angularVelocity": 0.09314627317695277, + "velocityX": -1.6738856547262355, + "velocityY": 0.04533098974846174, + "timestamp": 1.3237357079567351 }, { "x": 1.3892531394958496, "y": 5.486522674560547, "heading": 0.03752685285960867, - "angularVelocity": 0.12662396350062077, - "velocityX": -1.7369371923230166, - "velocityY": -0.08389143450007014, - "timestamp": 2.0924095971846315 + "angularVelocity": 0.04657314351984077, + "velocityX": -0.8369428536821739, + "velocityY": 0.02266549703966028, + "timestamp": 1.4241600226990592 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/amp 4 local sgmt 3.traj b/src/main/deploy/choreo/amp 4 local sgmt 3.traj index 3c66e297..7833c650 100644 --- a/src/main/deploy/choreo/amp 4 local sgmt 3.traj +++ b/src/main/deploy/choreo/amp 4 local sgmt 3.traj @@ -4,172 +4,226 @@ "x": 1.3892531394958496, "y": 5.486522674560547, "heading": 0.03752685285960867, - "angularVelocity": 2.746210792067846e-31, - "velocityX": -7.082594657438039e-35, - "velocityY": 0, - "timestamp": 2.195898479532914 - }, - { - "x": 1.4239336560949027, - "y": 5.45678487802077, - "heading": -0.004824083437490689, - "angularVelocity": -0.7980299015185681, - "velocityX": 0.6534941530454559, - "velocityY": -0.5603571707963146, - "timestamp": 2.248967839736724 - }, - { - "x": 1.4936387312915242, - "y": 5.397060158439441, - "heading": -0.08394241765705696, - "angularVelocity": -1.4908477116686103, - "velocityX": 1.3134711805253199, - "velocityY": -1.1254086982009737, - "timestamp": 2.3020371999405342 - }, - { - "x": 1.5989756629525658, - "y": 5.306833356180067, - "heading": -0.18501626934212806, - "angularVelocity": -1.9045613381601734, - "velocityX": 1.9848916824416323, - "velocityY": -1.7001675149815771, - "timestamp": 2.3551065601443444 - }, - { - "x": 1.7389822594115079, - "y": 5.188268009617622, - "heading": -0.23253583212093973, - "angularVelocity": -0.8954236982755346, - "velocityX": 2.6381813521258652, - "velocityY": -2.234158205546495, - "timestamp": 2.4081759203481545 - }, - { - "x": 1.8903539316186226, - "y": 5.05728781912084, - "heading": -0.23253589895082247, - "angularVelocity": -0.0000012592931680036756, - "velocityX": 2.85233648240303, - "velocityY": -2.4680943955939854, - "timestamp": 2.4612452805519647 - }, - { - "x": 2.041725559967012, - "y": 4.926307577937187, - "heading": -0.23253596578056132, - "angularVelocity": -0.000001259290456718511, - "velocityX": 2.8523356559614563, - "velocityY": -2.468095350700103, - "timestamp": 2.514314640755775 - }, - { - "x": 2.186666927649336, - "y": 4.788245829195716, - "heading": -0.23253707199818868, - "angularVelocity": -0.000020844751532055486, - "velocityX": 2.731168552356477, - "velocityY": -2.601534071849594, - "timestamp": 2.567384000959585 - }, - { - "x": 2.299267843418341, - "y": 4.670864736575426, - "heading": -0.33873841006148575, - "angularVelocity": -2.0011799210587196, - "velocityX": 2.1217688575209404, - "velocityY": -2.2118429950821876, - "timestamp": 2.620453361163395 - }, - { - "x": 2.37528105951498, - "y": 4.58047733131235, - "heading": -0.49175658993248267, - "angularVelocity": -2.883362062088883, - "velocityX": 1.4323371490576546, - "velocityY": -1.7031937998865565, - "timestamp": 2.6735227213672053 - }, - { - "x": 2.4143476486206055, - "y": 4.51963472366333, + "angularVelocity": -2.6599524768911785e-21, + "velocityX": -1.3251439086588674e-20, + "velocityY": 5.237013291945624e-20, + "timestamp": 0 + }, + { + "x": 1.4165016264324646, + "y": 5.468726018878958, + "heading": 0.027669365250431073, + "angularVelocity": -0.15749827520188686, + "velocityX": 0.4353634378786766, + "velocityY": -0.2843465480598295, + "timestamp": 0.06258790832180194 + }, + { + "x": 1.4710112662474708, + "y": 5.4331263299672, + "heading": 0.008563925885950884, + "angularVelocity": -0.3052576747931523, + "velocityX": 0.8709292461850596, + "velocityY": -0.5687949935747745, + "timestamp": 0.1251758166436039 + }, + { + "x": 1.5527987232870373, + "y": 5.379715427763577, + "heading": -0.018924667024813987, + "angularVelocity": -0.4391997375823718, + "velocityX": 1.306761309533597, + "velocityY": -0.8533741362469829, + "timestamp": 0.18776372496540583 + }, + { + "x": 1.6618865667976872, + "y": 5.308482777774649, + "heading": -0.05347261278820921, + "angularVelocity": -0.5519907389421553, + "velocityX": 1.7429539736296065, + "velocityY": -1.1381215940733804, + "timestamp": 0.2503516332872078 + }, + { + "x": 1.7983053814757264, + "y": 5.219415946962974, + "heading": -0.09280768567650903, + "angularVelocity": -0.6284771922086734, + "velocityX": 2.1796353055390356, + "velocityY": -1.4230677010921848, + "timestamp": 0.3129395416090097 + }, + { + "x": 1.9620858254779319, + "y": 5.112511494418258, + "heading": -0.13216035126284406, + "angularVelocity": -0.6287582800179152, + "velocityX": 2.6168064789785364, + "velocityY": -1.708068785348386, + "timestamp": 0.37552744993081166 + }, + { + "x": 2.1529952387272515, + "y": 4.987995425318902, + "heading": -0.15523721329619666, + "angularVelocity": -0.3687111880253399, + "velocityX": 3.0502603197368576, + "velocityY": -1.9894588657468077, + "timestamp": 0.4381153582526136 + }, + { + "x": 2.323053130467604, + "y": 4.850242258645187, + "heading": -0.20104196583241465, + "angularVelocity": -0.7318466739726868, + "velocityX": 2.7171045701988246, + "velocityY": -2.200954950682231, + "timestamp": 0.5007032665744156 + }, + { + "x": 2.466345532790285, + "y": 4.728187893870888, + "heading": -0.2844490729613583, + "angularVelocity": -1.3326393126943619, + "velocityX": 2.2894582382579274, + "velocityY": -1.9501269182338685, + "timestamp": 0.563291174896217 + }, + { + "x": 2.582053624940948, + "y": 4.6234869751422165, + "heading": -0.37568976157943984, + "angularVelocity": -1.4578005730589176, + "velocityX": 1.848729175542, + "velocityY": -1.6728617641340675, + "timestamp": 0.6258790832180186 + }, + { + "x": 2.670225613669648, + "y": 4.536352850675129, + "heading": -0.4673001281858147, + "angularVelocity": -1.463707113127209, + "velocityX": 1.4087703374804497, + "velocityY": -1.3921878331366975, + "timestamp": 0.68846699153982 + }, + { + "x": 2.730923003974787, + "y": 4.4668604060655035, + "heading": -0.555803694848379, + "angularVelocity": -1.4140681328974087, + "velocityX": 0.9697941971963301, + "velocityY": -1.1103174155033886, + "timestamp": 0.7510548998616215 + }, + { + "x": 2.764192819595337, + "y": 4.415045738220215, "heading": -0.6391760227970993, - "angularVelocity": -2.777863390447102, - "velocityX": 0.7361420781330721, - "velocityY": -1.146473358927959, - "timestamp": 2.7265920815710154 - }, - { - "x": 2.3701336666128094, - "y": 4.4395971700859445, - "heading": -0.8245153775471664, - "angularVelocity": -2.372308285003151, - "velocityX": -0.5659305114745719, - "velocityY": -1.0244653744427692, - "timestamp": 2.8047182492034923 - }, - { - "x": 2.2242072743116044, - "y": 4.369198833257615, - "heading": -0.9780363443258114, - "angularVelocity": -1.9650390058915468, - "velocityX": -1.8678299054380199, - "velocityY": -0.901085244056695, - "timestamp": 2.8828444168359693 - }, - { - "x": 1.9766118523863694, - "y": 4.308771886473381, - "heading": -1.099290083952399, - "angularVelocity": -1.5520246711313, - "velocityX": -3.169174035132216, - "velocityY": -0.7734533590395138, - "timestamp": 2.960970584468446 - }, - { - "x": 1.6829782303645726, - "y": 4.333642341587589, - "heading": -1.099290070338212, - "angularVelocity": 1.7425898802224296e-7, - "velocityX": -3.758454189166382, - "velocityY": 0.31833706769275627, - "timestamp": 3.039096752100923 - }, - { - "x": 1.3895177021759386, - "y": 4.36047817582874, - "heading": -1.0992900564673602, - "angularVelocity": 1.7754424736252278e-7, - "velocityX": -3.7562386212150933, - "velocityY": 0.34349354453672953, - "timestamp": 3.1172229197334 - }, - { - "x": 1.0960571742019716, - "y": 4.38731401241607, - "heading": -1.0992900425963372, - "angularVelocity": 1.7754644295782002e-7, - "velocityX": -3.756238618467393, - "velocityY": 0.34349357456735763, - "timestamp": 3.195349087365877 - }, - { - "x": 0.8925519822862651, - "y": 4.405915634394149, - "heading": -1.0363057213411215, - "angularVelocity": 0.8061872630368462, - "velocityX": -2.6048275255614968, - "velocityY": 0.23809720279105512, - "timestamp": 3.273475254998354 - }, - { - "x": 0.7907993793487549, - "y": 4.415216445922852, - "heading": -1.0048134836944498, - "angularVelocity": 0.4030946173477082, - "velocityX": -1.302413852119022, - "velocityY": 0.11904860830311666, - "timestamp": 3.351601422630831 + "angularVelocity": -1.3320836274005747, + "velocityX": 0.5315693799749596, + "velocityY": -0.8278702585630189, + "timestamp": 0.813642808183423 + }, + { + "x": 2.7480635723388045, + "y": 4.353298917368432, + "heading": -0.7461336511354502, + "angularVelocity": -1.226587505939294, + "velocityX": -0.18496981909961455, + "velocityY": -0.7081110544885852, + "timestamp": 0.9008421541904941 + }, + { + "x": 2.669473842343889, + "y": 4.302035156263182, + "heading": -0.8427126143522478, + "angularVelocity": -1.1075652242731957, + "velocityX": -0.9012651309167219, + "velocityY": -0.5878915777773497, + "timestamp": 0.9880415001975651 + }, + { + "x": 2.528459101717774, + "y": 4.261314542628117, + "heading": -0.927127909191312, + "angularVelocity": -0.9680725682531954, + "velocityX": -1.6171536494629242, + "velocityY": -0.46698301649834584, + "timestamp": 1.0752408462046361 + }, + { + "x": 2.3250882453572066, + "y": 4.231236836483453, + "heading": -0.996366799797096, + "angularVelocity": -0.7940299299970593, + "velocityX": -2.3322520829924085, + "velocityY": -0.34493040971001, + "timestamp": 1.1624401922117071 + }, + { + "x": 2.0595373245634314, + "y": 4.211999318818496, + "heading": -1.044279386733433, + "angularVelocity": -0.5494603931140937, + "velocityX": -3.0453315644390497, + "velocityY": -0.22061538928740462, + "timestamp": 1.2496395382187782 + }, + { + "x": 1.7326671393645254, + "y": 4.204157960465748, + "heading": -1.0516982080702795, + "angularVelocity": -0.08507886442456468, + "velocityX": -3.7485394118942086, + "velocityY": -0.08992450874702791, + "timestamp": 1.3368388842258492 + }, + { + "x": 1.419863969855018, + "y": 4.255364813771041, + "heading": -1.034285504034905, + "angularVelocity": 0.19968847053007022, + "velocityX": -3.587219214742072, + "velocityY": 0.5872389604979419, + "timestamp": 1.4240382302329202 + }, + { + "x": 1.1696215071966642, + "y": 4.29633086503019, + "heading": -1.02035490887151, + "angularVelocity": 0.15975572984532924, + "velocityX": -2.8697745352133857, + "velocityY": 0.4697976892605051, + "timestamp": 1.5112375762399912 + }, + { + "x": 0.9819396842688768, + "y": 4.327055591018277, + "heading": -1.009906819806218, + "angularVelocity": 0.11981843378096674, + "velocityX": -2.152330625421988, + "velocityY": 0.35235041769230013, + "timestamp": 1.5984369222470622 + }, + { + "x": 0.8568184786877541, + "y": 4.347538817267099, + "heading": -1.002941368423543, + "angularVelocity": 0.07987962870856867, + "velocityX": -1.434886972328629, + "velocityY": 0.2349011453269456, + "timestamp": 1.6856362682541333 + }, + { + "x": 0.7942578792572021, + "y": 4.357780456542969, + "heading": -0.9994586215071236, + "angularVelocity": 0.03994005776301401, + "velocityX": -0.7174434476318076, + "velocityY": 0.11745087256777795, + "timestamp": 1.7728356142612043 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/amp 4 local.traj b/src/main/deploy/choreo/amp 4 local.traj index 2864d9ec..6dfc86f9 100644 --- a/src/main/deploy/choreo/amp 4 local.traj +++ b/src/main/deploy/choreo/amp 4 local.traj @@ -1,454 +1,760 @@ { "samples": [ { - "x": 0.7470832467079163, - "y": 6.625458717346191, - "heading": 1.056495497952334, - "angularVelocity": 1.8216849722605286e-31, - "velocityX": 0, - "velocityY": 0, + "x": 0.7470832465334709, + "y": 6.625458718233556, + "heading": 1.0564954979067924, + "angularVelocity": -5.684301329861994e-12, + "velocityX": -3.602456269610706e-10, + "velocityY": 4.1137821262407583e-10, "timestamp": 0 }, { - "x": 0.7875510590073889, - "y": 6.647513625582565, - "heading": 0.9955054911508737, - "angularVelocity": -1.1259767405457384, - "velocityX": 0.7471029727592392, - "velocityY": 0.4071702069138493, - "timestamp": 0.05416631143899068 - }, - { - "x": 0.8691098883744304, - "y": 6.691702168377954, - "heading": 0.8785332104893699, - "angularVelocity": -2.159502420489772, - "velocityX": 1.505711339767041, - "velocityY": 0.8157938323926509, - "timestamp": 0.10833262287798136 - }, - { - "x": 0.9931586796399057, - "y": 6.758536729383142, - "heading": 0.7242571551382772, - "angularVelocity": -2.848192008142514, - "velocityX": 2.290146549949145, - "velocityY": 1.2338769103830776, - "timestamp": 0.16249893431697204 - }, - { - "x": 1.1542844409320208, - "y": 6.847841004747936, - "heading": 0.6582721283096681, - "angularVelocity": -1.2181930996525292, - "velocityX": 2.974648947133801, - "velocityY": 1.6487051267166433, - "timestamp": 0.21666524575596272 - }, - { - "x": 1.3362429767095947, - "y": 6.9407596540671515, - "heading": 0.6582720385538592, - "angularVelocity": -0.0000016570411839532803, - "velocityX": 3.3592565368332274, - "velocityY": 1.7154324680917685, - "timestamp": 0.2708315571949534 - }, - { - "x": 1.5182021069161247, - "y": 7.033677139333042, - "heading": 0.6582719487984567, - "angularVelocity": -0.0000016570336808247504, - "velocityX": 3.359267510978228, - "velocityY": 1.7154109777356332, - "timestamp": 0.3249978686339441 - }, - { - "x": 1.7157951910460691, - "y": 7.085636206134739, - "heading": 0.6582717690726799, - "angularVelocity": -0.0000033180361009936695, - "velocityX": 3.647896245482769, - "velocityY": 0.9592506009979895, - "timestamp": 0.3791641800729348 - }, - { - "x": 1.8967940454930174, - "y": 7.095126843263903, - "heading": 0.5939474144774982, - "angularVelocity": -1.1875343342814295, - "velocityX": 3.3415392268460664, - "velocityY": 0.1752129114394957, - "timestamp": 0.4333304915119255 - }, - { - "x": 2.0516314923077155, - "y": 7.067103472452243, - "heading": 0.46543090394591263, - "angularVelocity": -2.3726280619336353, - "velocityX": 2.8585562262089095, - "velocityY": -0.5173579309202905, - "timestamp": 0.4874968029509162 - }, - { - "x": 2.1749652200442315, - "y": 7.00220729212556, - "heading": 0.30618928904490067, - "angularVelocity": -2.9398644779490204, - "velocityX": 2.2769452905322995, - "velocityY": -1.198091186249191, - "timestamp": 0.5416631143899069 - }, - { - "x": 2.2610747814178467, - "y": 6.9050798416137695, - "heading": 0.15617120010566338, - "angularVelocity": -2.7695828819396024, - "velocityX": 1.5897254047029348, - "velocityY": -1.7931339227554435, - "timestamp": 0.5958294258288975 - }, - { - "x": 2.3021420961741277, - "y": 6.690129168178249, - "heading": -0.009663328888242836, - "angularVelocity": -2.0165575251161725, - "velocityX": 0.4993809377969598, - "velocityY": -2.6138127003762723, - "timestamp": 0.6780658742953757 - }, - { - "x": 2.248466001407306, - "y": 6.41546750147008, - "heading": -0.10873685342712126, - "angularVelocity": -1.204739825057738, - "velocityX": -0.6527044366306908, - "velocityY": -3.3399018541045957, - "timestamp": 0.7603023227618538 - }, - { - "x": 2.085165640858422, - "y": 6.151744237063873, - "heading": -0.10873683701731249, - "angularVelocity": 1.9954422992985155e-7, - "velocityX": -1.9857418893211336, - "velocityY": -3.2068902454330503, - "timestamp": 0.842538771228332 - }, - { - "x": 1.8574534262453974, - "y": 5.941116320042222, - "heading": -0.10873681487910887, - "angularVelocity": 2.692018447413834e-7, - "velocityX": -2.7689937838918954, - "velocityY": -2.5612477307736583, - "timestamp": 0.9247752196948101 - }, - { - "x": 1.629741023592729, - "y": 5.730488606312599, - "heading": -0.10873679274080142, - "angularVelocity": 2.6920310704244503e-7, - "velocityX": -2.76899607046491, - "velocityY": -2.5612452587307564, - "timestamp": 1.0070116681612884 - }, - { - "x": 1.4644902813281564, - "y": 5.577695792451846, - "heading": 0.0012781207724603802, - "angularVelocity": 1.337787751839788, - "velocityX": -2.009458644507651, - "velocityY": -1.8579695099921079, - "timestamp": 1.0892481166277665 - }, - { - "x": 1.3818649053573608, - "y": 5.5012993812561035, - "heading": 0.05628563945524819, - "angularVelocity": 0.6688946289455666, - "velocityX": -1.0047293810903806, - "velocityY": -0.9289848068631428, - "timestamp": 1.1714845650942447 - }, - { - "x": 1.3818649053573608, - "y": 5.5012993812561035, - "heading": 0.05628563945524819, - "angularVelocity": 8.108539434054397e-29, - "velocityX": -3.383351472481295e-31, - "velocityY": 2.5568610146599268e-30, - "timestamp": 1.2537210135607229 - }, - { - "x": 1.4659695573316451, - "y": 5.505361506655373, - "heading": 0.050154371824367786, - "angularVelocity": -0.0866134750258778, - "velocityX": 1.188106051128092, - "velocityY": 0.05738369583633677, - "timestamp": 1.3245098559325195 - }, - { - "x": 1.6341788550204006, - "y": 5.5134857557639885, - "heading": 0.03789188378734141, - "angularVelocity": -0.17322628293059847, - "velocityX": 2.3762120138268115, - "velocityY": 0.11476736779992304, - "timestamp": 1.395298698304316 - }, - { - "x": 1.8864927796012139, - "y": 5.525672125057411, - "heading": 0.019498324963128963, - "angularVelocity": -0.25983697724008453, - "velocityX": 3.5643177106303305, - "velocityY": 0.1721509899740557, - "timestamp": 1.4660875406761127 - }, - { - "x": 2.1102347064408926, - "y": 5.533923842143074, - "heading": 0.00011174721946555298, - "angularVelocity": -0.2738648789005671, - "velocityX": 3.160694812108141, - "velocityY": 0.11656804673148825, - "timestamp": 1.5368763830479093 - }, - { - "x": 2.249872007587195, - "y": 5.538113436672261, - "heading": -0.013143603106743272, - "angularVelocity": -0.18725197195045482, - "velocityX": 1.9725891322378306, - "velocityY": 0.0591843910539215, - "timestamp": 1.607665225419706 - }, - { - "x": 2.3054046630859375, - "y": 5.538240909576416, - "heading": -0.020267736871838693, - "angularVelocity": -0.10063921836266371, - "velocityX": 0.784483170484331, - "velocityY": 0.0018007485344283746, - "timestamp": 1.6784540677915025 - }, - { - "x": 2.206836259942782, - "y": 5.529745448091516, - "heading": -0.017578724891835627, - "angularVelocity": 0.025983583153922206, - "velocityX": -0.9524540308729255, - "velocityY": -0.08209057139403513, - "timestamp": 1.7819429501397848 - }, - { - "x": 1.9285141868544358, - "y": 5.512568166846942, - "heading": -0.0017855827044546864, - "angularVelocity": 0.15260713836130316, - "velocityX": -2.689391041558251, - "velocityY": -0.1659818992610695, - "timestamp": 1.885431832488067 - }, - { - "x": 1.5690068282385219, - "y": 5.495204505355553, - "heading": 0.024422680398419737, - "angularVelocity": 0.25324713638971313, - "velocityX": -3.4738742023131053, - "velocityY": -0.16778286804715686, - "timestamp": 1.9889207148363492 - }, - { - "x": 1.3892531394958496, - "y": 5.486522674560547, - "heading": 0.03752685285960867, - "angularVelocity": 0.12662396350062077, - "velocityX": -1.7369371923230166, - "velocityY": -0.08389143450007014, - "timestamp": 2.0924095971846315 - }, - { - "x": 1.3892531394958496, - "y": 5.486522674560547, - "heading": 0.03752685285960867, - "angularVelocity": 2.746210792067846e-31, - "velocityX": -7.082594657438039e-35, - "velocityY": 0, - "timestamp": 2.195898479532914 - }, - { - "x": 1.4239336560949027, - "y": 5.45678487802077, - "heading": -0.004824083437490689, - "angularVelocity": -0.7980299015185681, - "velocityX": 0.6534941530454559, - "velocityY": -0.5603571707963146, - "timestamp": 2.248967839736724 - }, - { - "x": 1.4936387312915242, - "y": 5.397060158439441, - "heading": -0.08394241765705696, - "angularVelocity": -1.4908477116686103, - "velocityX": 1.3134711805253199, - "velocityY": -1.1254086982009737, - "timestamp": 2.3020371999405342 - }, - { - "x": 1.5989756629525658, - "y": 5.306833356180067, - "heading": -0.18501626934212806, - "angularVelocity": -1.9045613381601734, - "velocityX": 1.9848916824416323, - "velocityY": -1.7001675149815771, - "timestamp": 2.3551065601443444 - }, - { - "x": 1.7389822594115079, - "y": 5.188268009617622, - "heading": -0.23253583212093973, - "angularVelocity": -0.8954236982755346, - "velocityX": 2.6381813521258652, - "velocityY": -2.234158205546495, - "timestamp": 2.4081759203481545 - }, - { - "x": 1.8903539316186226, - "y": 5.05728781912084, - "heading": -0.23253589895082247, - "angularVelocity": -0.0000012592931680036756, - "velocityX": 2.85233648240303, - "velocityY": -2.4680943955939854, - "timestamp": 2.4612452805519647 - }, - { - "x": 2.041725559967012, - "y": 4.926307577937187, - "heading": -0.23253596578056132, - "angularVelocity": -0.000001259290456718511, - "velocityX": 2.8523356559614563, - "velocityY": -2.468095350700103, - "timestamp": 2.514314640755775 - }, - { - "x": 2.186666927649336, - "y": 4.788245829195716, - "heading": -0.23253707199818868, - "angularVelocity": -0.000020844751532055486, - "velocityX": 2.731168552356477, - "velocityY": -2.601534071849594, - "timestamp": 2.567384000959585 - }, - { - "x": 2.299267843418341, - "y": 4.670864736575426, - "heading": -0.33873841006148575, - "angularVelocity": -2.0011799210587196, - "velocityX": 2.1217688575209404, - "velocityY": -2.2118429950821876, - "timestamp": 2.620453361163395 - }, - { - "x": 2.37528105951498, - "y": 4.58047733131235, - "heading": -0.49175658993248267, - "angularVelocity": -2.883362062088883, - "velocityX": 1.4323371490576546, - "velocityY": -1.7031937998865565, - "timestamp": 2.6735227213672053 - }, - { - "x": 2.4143476486206055, - "y": 4.51963472366333, - "heading": -0.6391760227970993, - "angularVelocity": -2.777863390447102, - "velocityX": 0.7361420781330721, - "velocityY": -1.146473358927959, - "timestamp": 2.7265920815710154 - }, - { - "x": 2.3701336666128094, - "y": 4.4395971700859445, - "heading": -0.8245153775471664, - "angularVelocity": -2.372308285003151, - "velocityX": -0.5659305114745719, - "velocityY": -1.0244653744427692, - "timestamp": 2.8047182492034923 - }, - { - "x": 2.2242072743116044, - "y": 4.369198833257615, - "heading": -0.9780363443258114, - "angularVelocity": -1.9650390058915468, - "velocityX": -1.8678299054380199, - "velocityY": -0.901085244056695, - "timestamp": 2.8828444168359693 - }, - { - "x": 1.9766118523863694, - "y": 4.308771886473381, - "heading": -1.099290083952399, - "angularVelocity": -1.5520246711313, - "velocityX": -3.169174035132216, - "velocityY": -0.7734533590395138, - "timestamp": 2.960970584468446 - }, - { - "x": 1.6829782303645726, - "y": 4.333642341587589, - "heading": -1.099290070338212, - "angularVelocity": 1.7425898802224296e-7, - "velocityX": -3.758454189166382, - "velocityY": 0.31833706769275627, - "timestamp": 3.039096752100923 - }, - { - "x": 1.3895177021759386, - "y": 4.36047817582874, - "heading": -1.0992900564673602, - "angularVelocity": 1.7754424736252278e-7, - "velocityX": -3.7562386212150933, - "velocityY": 0.34349354453672953, - "timestamp": 3.1172229197334 - }, - { - "x": 1.0960571742019716, - "y": 4.38731401241607, - "heading": -1.0992900425963372, - "angularVelocity": 1.7754644295782002e-7, - "velocityX": -3.756238618467393, - "velocityY": 0.34349357456735763, - "timestamp": 3.195349087365877 - }, - { - "x": 0.8925519822862651, - "y": 4.405915634394149, - "heading": -1.0363057213411215, - "angularVelocity": 0.8061872630368462, - "velocityX": -2.6048275255614968, - "velocityY": 0.23809720279105512, - "timestamp": 3.273475254998354 - }, - { - "x": 0.7907993793487549, - "y": 4.415216445922852, - "heading": -1.0048134836944498, - "angularVelocity": 0.4030946173477082, - "velocityX": -1.302413852119022, - "velocityY": 0.11904860830311666, - "timestamp": 3.351601422630831 - }, - { - "x": 0.7907993793487549, - "y": 4.415216445922852, - "heading": -1.0048134836944498, - "angularVelocity": -9.422070435540628e-32, - "velocityX": 1.9369819218763315e-32, - "velocityY": -1.162688749272308e-31, - "timestamp": 3.4297275902633078 + "x": 0.7589569772775385, + "y": 6.629936126338657, + "heading": 1.0434265097431823, + "angularVelocity": -0.12778392744309977, + "velocityX": 0.11609712676013798, + "velocityY": 0.04377849867759475, + "timestamp": 0.10227411521599863 + }, + { + "x": 0.7822022838432713, + "y": 6.638629979602176, + "heading": 1.0183346599429852, + "angularVelocity": -0.2453392014361959, + "velocityX": 0.2272843592055021, + "velocityY": 0.08500540295845989, + "timestamp": 0.20454823043199727 + }, + { + "x": 0.816345318524229, + "y": 6.6512661070932495, + "heading": 0.9822355978698938, + "angularVelocity": -0.35296381641965074, + "velocityX": 0.33383847695279084, + "velocityY": 0.12355156118457596, + "timestamp": 0.30682234564799593 + }, + { + "x": 0.8609432271767217, + "y": 6.667555740683275, + "heading": 0.9361120278121537, + "angularVelocity": -0.45097989764859636, + "velocityX": 0.4360625240585109, + "velocityY": 0.15927424713723962, + "timestamp": 0.40909646086399454 + }, + { + "x": 0.9155883048461597, + "y": 6.687191502411657, + "heading": 0.8809291072748399, + "angularVelocity": -0.5395590113414345, + "velocityX": 0.5343001768187209, + "velocityY": 0.19199150048411856, + "timestamp": 0.5113705760799931 + }, + { + "x": 0.9799143577111046, + "y": 6.709840447255909, + "heading": 0.817662056557047, + "angularVelocity": -0.6186027670698295, + "velocityX": 0.6289573163604619, + "velocityY": 0.22145333556813593, + "timestamp": 0.6136446912959918 + }, + { + "x": 1.0536065024635548, + "y": 6.735133328172977, + "heading": 0.7473295527982347, + "angularVelocity": -0.687686259272288, + "velocityX": 0.7205356386732681, + "velocityY": 0.24730480411634378, + "timestamp": 0.7159188065119904 + }, + { + "x": 1.1364169197016813, + "y": 6.762648211314195, + "heading": 0.6710254557943266, + "angularVelocity": -0.7460743786173882, + "velocityX": 0.8096908708297301, + "velocityY": 0.2690307532427302, + "timestamp": 0.818192921727989 + }, + { + "x": 1.2281932228229642, + "y": 6.7918845460908575, + "heading": 0.5899378478266369, + "angularVelocity": -0.7928458510838684, + "velocityX": 0.8973561208709863, + "velocityY": 0.28586249636547506, + "timestamp": 0.9204670369439876 + }, + { + "x": 1.3289409008958384, + "y": 6.822217824239773, + "heading": 0.5053285615301966, + "angularVelocity": -0.8272795718879136, + "velocityX": 0.985075041075245, + "velocityY": 0.296588019337818, + "timestamp": 1.0227411521599863 + }, + { + "x": 1.4390031913745853, + "y": 6.852792823991799, + "heading": 0.4183893727045359, + "angularVelocity": -0.8500605319021087, + "velocityX": 1.076150015287458, + "velocityY": 0.29895148738224403, + "timestamp": 1.125015267375985 + }, + { + "x": 1.5597885498551032, + "y": 6.882059112916124, + "heading": 0.32979940511385675, + "angularVelocity": -0.8662012607776253, + "velocityX": 1.1809963684348201, + "velocityY": 0.28615537738935176, + "timestamp": 1.2272893825919837 + }, + { + "x": 1.700662064049805, + "y": 6.9025972281907375, + "heading": 0.2406449492427404, + "angularVelocity": -0.8717206268168672, + "velocityX": 1.3774112254273, + "velocityY": 0.20081439319892427, + "timestamp": 1.3295634978079824 + }, + { + "x": 1.8510164004103928, + "y": 6.913603640101383, + "heading": 0.18435642050200896, + "angularVelocity": -0.5503692556839603, + "velocityX": 1.4701113396826178, + "velocityY": 0.10761678064700267, + "timestamp": 1.4318376130239812 + }, + { + "x": 2.0039488544462833, + "y": 6.918843013673937, + "heading": 0.15617120081925578, + "angularVelocity": -0.2755850738942481, + "velocityX": 1.4953192592999736, + "velocityY": 0.051228726583383746, + "timestamp": 1.5341117282399799 + }, + { + "x": 2.1557445524327394, + "y": 6.919295783765447, + "heading": 0.15617120073466711, + "angularVelocity": -8.01993499988118e-11, + "velocityX": 1.484204462099971, + "velocityY": 0.0044270165813428694, + "timestamp": 1.6363858434559786 + }, + { + "x": 2.232501376452394, + "y": 6.918644981823818, + "heading": 0.15617120066422163, + "angularVelocity": -2.7881719949913897e-12, + "velocityX": 1.4631774376072004, + "velocityY": -0.012405855819009865, + "timestamp": 1.6888448428916174 + }, + { + "x": 2.3078780299226054, + "y": 6.916920762093343, + "heading": 0.1561712005937278, + "angularVelocity": -3.698975819730093e-12, + "velocityX": 1.436867928003674, + "velocityY": -0.032867889033463694, + "timestamp": 1.7413038423272562 + }, + { + "x": 2.381659462849091, + "y": 6.91403766609857, + "heading": 0.15617120052323402, + "angularVelocity": -3.687857737825465e-12, + "velocityX": 1.4064590265589882, + "velocityY": -0.05495897496281437, + "timestamp": 1.793762841762895 + }, + { + "x": 2.453627858185915, + "y": 6.909923503320222, + "heading": 0.15617120045273983, + "angularVelocity": -3.68806627753873e-12, + "velocityX": 1.3718979863339387, + "velocityY": -0.07842619311071317, + "timestamp": 1.8462218411985338 + }, + { + "x": 2.5235476514141553, + "y": 6.90450568335544, + "heading": 0.15617120038224525, + "angularVelocity": -3.688143550349055e-12, + "velocityX": 1.3328464960153679, + "velocityY": -0.10327716626043287, + "timestamp": 1.8986808406341726 + }, + { + "x": 2.5911718313337544, + "y": 6.897715977103231, + "heading": 0.1561712003117504, + "angularVelocity": -3.687560329325201e-12, + "velocityX": 1.2890863531735766, + "velocityY": -0.12942875706821344, + "timestamp": 1.9511398400698114 + }, + { + "x": 2.65624736130467, + "y": 6.889492702059127, + "heading": 0.15617120024125528, + "angularVelocity": -3.68944362324301e-12, + "velocityX": 1.240502697821755, + "velocityY": -0.1567561709974853, + "timestamp": 2.0035988395054503 + }, + { + "x": 2.7185182573768354, + "y": 6.879780772610566, + "heading": 0.1561712001707086, + "angularVelocity": -4.669569996251489e-12, + "velocityX": 1.187039346844362, + "velocityY": -0.1851336547556834, + "timestamp": 2.056057838941089 + }, + { + "x": 2.844727229024539, + "y": 6.826831262995891, + "heading": 0.36522912798339696, + "angularVelocity": 1.2815424227266763, + "velocityX": 0.7736714562084378, + "velocityY": -0.32458488264739044, + "timestamp": 2.219187770584854 + }, + { + "x": 2.905891887894138, + "y": 6.746880408353911, + "heading": 0.5636352419280429, + "angularVelocity": 1.2162459209838357, + "velocityX": 0.3749444284785087, + "velocityY": -0.490105365340026, + "timestamp": 2.382317702228619 + }, + { + "x": 2.909090532344001, + "y": 6.6442122814929245, + "heading": 0.7377210107974022, + "angularVelocity": 1.0671601901772823, + "velocityX": 0.0196079562833389, + "velocityY": -0.6293641266849203, + "timestamp": 2.5454476338723837 + }, + { + "x": 2.8528015320306115, + "y": 6.520167648469734, + "heading": 0.8617887991289785, + "angularVelocity": 0.7605458243522792, + "velocityX": -0.34505623601923857, + "velocityY": -0.7604038809886424, + "timestamp": 2.7085775655161486 + }, + { + "x": 2.6673775887021587, + "y": 6.358333521535233, + "heading": 0.8824380102910534, + "angularVelocity": 0.1265813757217392, + "velocityX": -1.1366641384675562, + "velocityY": -0.9920566103835737, + "timestamp": 2.8717074971599135 + }, + { + "x": 2.433334548523251, + "y": 6.191208743378355, + "heading": 0.7641593769894643, + "angularVelocity": -0.7250578241718747, + "velocityX": -1.4347032313583656, + "velocityY": -1.0244887408273557, + "timestamp": 3.0348374288036783 + }, + { + "x": 2.190845809692594, + "y": 6.024313186516238, + "heading": 0.6297541443819642, + "angularVelocity": -0.823915214372992, + "velocityX": -1.486476064011715, + "velocityY": -1.0230835952941921, + "timestamp": 3.1979673604474432 + }, + { + "x": 1.9580404786548902, + "y": 5.870547470854263, + "heading": 0.47066977289213696, + "angularVelocity": -0.9752003809508628, + "velocityX": -1.4271159717504125, + "velocityY": -0.9425965806313563, + "timestamp": 3.361097292091208 + }, + { + "x": 1.7498273467354928, + "y": 5.73486661370809, + "heading": 0.32338313207816166, + "angularVelocity": -0.9028793141986351, + "velocityX": -1.2763637531323493, + "velocityY": -0.831734899187721, + "timestamp": 3.524227223734973 + }, + { + "x": 1.577014249967444, + "y": 5.624033816048722, + "heading": 0.19882585300104294, + "angularVelocity": -0.7635464431083702, + "velocityX": -1.0593586039568081, + "velocityY": -0.6794142353068856, + "timestamp": 3.687357155378738 + }, + { + "x": 1.4506486184747536, + "y": 5.5441849844920235, + "heading": 0.1067515452077215, + "angularVelocity": -0.5644231371867715, + "velocityX": -0.7746317925903302, + "velocityY": -0.4894799553714506, + "timestamp": 3.8504870870225028 + }, + { + "x": 1.3818649056319274, + "y": 5.50129938101995, + "heading": 0.05628563946839357, + "angularVelocity": -0.30936018439404245, + "velocityX": -0.4216498592955241, + "velocityY": -0.2628923039688523, + "timestamp": 4.013617018666268 + }, + { + "x": 1.3818649054659777, + "y": 5.501299381110093, + "heading": 0.05628563946121549, + "angularVelocity": -8.20711890061584e-12, + "velocityX": -2.918186949691226e-10, + "velocityY": -8.516677331367793e-11, + "timestamp": 4.176746950310032 + }, + { + "x": 1.4254749911225055, + "y": 5.4996771535618185, + "heading": 0.052286234874463405, + "angularVelocity": -0.03818204740477268, + "velocityX": 0.41634256351427257, + "velocityY": -0.015487297221703413, + "timestamp": 4.281492632082638 + }, + { + "x": 1.5054277602383004, + "y": 5.496460245033646, + "heading": 0.04534031368940726, + "angularVelocity": -0.06631224378557163, + "velocityX": 0.763303725485265, + "velocityY": -0.030711609603001202, + "timestamp": 4.386238313855244 + }, + { + "x": 1.614706352195534, + "y": 5.491742221647794, + "heading": 0.036622661203774276, + "angularVelocity": -0.08322684370600701, + "velocityX": 1.0432753896642522, + "velocityY": -0.045042652489690874, + "timestamp": 4.4909839956278494 + }, + { + "x": 1.7468754648697162, + "y": 5.485736649446459, + "heading": 0.02746720464328704, + "angularVelocity": -0.08740652985101428, + "velocityX": 1.2618096559898304, + "velocityY": -0.05733479469253905, + "timestamp": 4.595729677400455 + }, + { + "x": 1.8974421226741423, + "y": 5.478814465512338, + "heading": 0.0192537076414664, + "angularVelocity": -0.07841370510897432, + "velocityX": 1.43744978567254, + "velocityY": -0.06608562544064354, + "timestamp": 4.700475359173061 + }, + { + "x": 2.0478771487383725, + "y": 5.472018862260595, + "heading": 0.011842428389913316, + "angularVelocity": -0.07075498604111852, + "velocityX": 1.436193106275662, + "velocityY": -0.06487716815402857, + "timestamp": 4.805221040945667 + }, + { + "x": 2.1991998312667134, + "y": 5.465297323381209, + "heading": 0.007319920190768939, + "angularVelocity": -0.04317608251475354, + "velocityX": 1.4446675030153622, + "velocityY": -0.06417008055763802, + "timestamp": 4.909966722718273 + }, + { + "x": 2.337086103631991, + "y": 5.4645313180146555, + "heading": 0.0021217678335590726, + "angularVelocity": -0.04962641198543019, + "velocityX": 1.316390996187784, + "velocityY": -0.007313001468104138, + "timestamp": 5.014712404490878 + }, + { + "x": 2.45341531313919, + "y": 5.464492524135499, + "heading": -0.0044756440905307565, + "angularVelocity": -0.06298504924092899, + "velocityX": 1.1105871626237722, + "velocityY": -0.0003703621622037101, + "timestamp": 5.119458086263484 + }, + { + "x": 2.541111145262716, + "y": 5.463618987693384, + "heading": -0.012133903650681083, + "angularVelocity": -0.07311289048562652, + "velocityX": 0.8372262287970834, + "velocityY": -0.008339593435811936, + "timestamp": 5.22420376803609 + }, + { + "x": 2.592916727068705, + "y": 5.461223602405139, + "heading": -0.020267736871702392, + "angularVelocity": -0.07765316033563659, + "velocityX": 0.4945844156915881, + "velocityY": -0.022868582319897673, + "timestamp": 5.328949449808696 + }, + { + "x": 2.573983182200651, + "y": 5.456183859404588, + "heading": -0.02941680850548689, + "angularVelocity": -0.07029420270393813, + "velocityX": -0.14547032678251723, + "velocityY": -0.03872138453206129, + "timestamp": 5.459103449457666 + }, + { + "x": 2.4879155076147748, + "y": 5.449739339031148, + "heading": -0.03668510065770142, + "angularVelocity": -0.05584378637679435, + "velocityX": -0.6612756796636089, + "velocityY": -0.04951457856894505, + "timestamp": 5.589257449106635 + }, + { + "x": 2.35046961428428, + "y": 5.4436438239165845, + "heading": -0.03977633600135786, + "angularVelocity": -0.02375059815495546, + "velocityX": -1.056025121733672, + "velocityY": -0.046833099245860266, + "timestamp": 5.719411448755605 + }, + { + "x": 2.1764781877651598, + "y": 5.443212327064482, + "heading": -0.032264347817817784, + "angularVelocity": 0.05771615319957162, + "velocityX": -1.336811984121226, + "velocityY": -0.0033152797571240995, + "timestamp": 5.849565448404575 + }, + { + "x": 1.9752868983122995, + "y": 5.4534701667830054, + "heading": -0.009494098721117665, + "angularVelocity": 0.174948516049147, + "velocityX": -1.5457941360881122, + "velocityY": 0.07881309585526655, + "timestamp": 5.979719448053545 + }, + { + "x": 1.7725080662079153, + "y": 5.4634789711909475, + "heading": 0.004869525328448035, + "angularVelocity": 0.11035868346800264, + "velocityX": -1.5579915534229911, + "velocityY": 0.07689970624561784, + "timestamp": 6.1098734477025145 + }, + { + "x": 1.5970859275969194, + "y": 5.473760403676058, + "heading": 0.019510408429697427, + "angularVelocity": 0.11248892189634652, + "velocityX": -1.347804439948399, + "velocityY": 0.07899436394267238, + "timestamp": 6.240027447351484 + }, + { + "x": 1.463998994938607, + "y": 5.481958295434183, + "heading": 0.031093440957313423, + "angularVelocity": 0.0889948258110792, + "velocityX": -1.0225343286428261, + "velocityY": 0.06298609118419185, + "timestamp": 6.370181447000454 + }, + { + "x": 1.3892531402609951, + "y": 5.486522674152253, + "heading": 0.03752685288213878, + "angularVelocity": 0.049429229542960515, + "velocityX": -0.5742878042328909, + "velocityY": 0.03506906174326123, + "timestamp": 6.500335446649424 + }, + { + "x": 1.3892531401370203, + "y": 5.486522674374907, + "heading": 0.03752685288891443, + "angularVelocity": 5.018347137854837e-11, + "velocityX": -8.981333628450582e-10, + "velocityY": 1.1863234183511445e-9, + "timestamp": 6.630489446298394 + }, + { + "x": 1.410785208000178, + "y": 5.466798705442517, + "heading": 0.02538794622123859, + "angularVelocity": -0.12074981514388294, + "velocityX": 0.21418676347263918, + "velocityY": -0.1962009963268651, + "timestamp": 6.731018848208977 + }, + { + "x": 1.4512665581597533, + "y": 5.429718184658836, + "heading": 0.002445244265967334, + "angularVelocity": -0.2282188250282345, + "velocityX": 0.4026816906907998, + "velocityY": -0.36885249450974994, + "timestamp": 6.8315482501195595 + }, + { + "x": 1.5081514675148215, + "y": 5.3776444456377, + "heading": -0.029986935931297812, + "angularVelocity": -0.3226138782330449, + "velocityX": 0.5658534482437384, + "velocityY": -0.5179951130124598, + "timestamp": 6.932077652030142 + }, + { + "x": 1.578941487701644, + "y": 5.312948064530918, + "heading": -0.07059739560458454, + "angularVelocity": -0.4039659933419245, + "velocityX": 0.7041722939494359, + "velocityY": -0.6435568078574709, + "timestamp": 7.032607053940725 + }, + { + "x": 1.6611946188865518, + "y": 5.238029568056684, + "heading": -0.11800108662062794, + "angularVelocity": -0.4715405656932785, + "velocityX": 0.8181997404499256, + "velocityY": -0.7452396506196601, + "timestamp": 7.133136455851308 + }, + { + "x": 1.7524893345353523, + "y": 5.155358397604862, + "heading": -0.17044488274189246, + "angularVelocity": -0.521676197753198, + "velocityX": 0.9081394425873396, + "velocityY": -0.8223581237255294, + "timestamp": 7.233665857761891 + }, + { + "x": 1.8499500532222615, + "y": 5.067431436588914, + "heading": -0.22499900927916092, + "angularVelocity": -0.5426683689609221, + "velocityX": 0.9694747626122975, + "velocityY": -0.8746392520744162, + "timestamp": 7.334195259672474 + }, + { + "x": 1.9473638091809122, + "y": 4.979075974351683, + "heading": -0.27982409115325835, + "angularVelocity": -0.5453636534436577, + "velocityX": 0.9690076084526664, + "velocityY": -0.8789016988127862, + "timestamp": 7.434724661583057 + }, + { + "x": 2.05818821772957, + "y": 4.854821291737014, + "heading": -0.34544281347225186, + "angularVelocity": -0.6527316496725523, + "velocityX": 1.1024079106043232, + "velocityY": -1.2360033993617228, + "timestamp": 7.53525406349364 + }, + { + "x": 2.1664425710584037, + "y": 4.732231232720307, + "heading": -0.4041393246178968, + "angularVelocity": -0.5838740711290391, + "velocityX": 1.076842701113489, + "velocityY": -1.2194448247922516, + "timestamp": 7.635783465404223 + }, + { + "x": 2.2666201121554423, + "y": 4.61627032113389, + "heading": -0.4602072656334145, + "angularVelocity": -0.557726794034664, + "velocityX": 0.996499915039965, + "velocityY": -1.153502450677793, + "timestamp": 7.736312867314806 + }, + { + "x": 2.356480498307365, + "y": 4.5087945921361055, + "heading": -0.5124957847583047, + "angularVelocity": -0.5201316047807618, + "velocityX": 0.8938716814183492, + "velocityY": -1.0690974663909212, + "timestamp": 7.8368422692253885 + }, + { + "x": 2.433798199341425, + "y": 4.411691494217003, + "heading": -0.5602485683833155, + "angularVelocity": -0.47501310796660834, + "velocityX": 0.7691053456052521, + "velocityY": -0.9659173928845837, + "timestamp": 7.937371671135971 + }, + { + "x": 2.4961951429135376, + "y": 4.327016530758609, + "heading": -0.6027268742617791, + "angularVelocity": -0.4225460918867537, + "velocityX": 0.620683518971687, + "velocityY": -0.8422905312450144, + "timestamp": 8.037901073046553 + }, + { + "x": 2.541198492257727, + "y": 4.256927967764492, + "heading": -0.6391760227839379, + "angularVelocity": -0.3625720221944257, + "velocityX": 0.4476635477259144, + "velocityY": -0.6971946668803632, + "timestamp": 8.138430474957136 + }, + { + "x": 2.5331758624705114, + "y": 4.192320334785442, + "heading": -0.6763224470307224, + "angularVelocity": -0.2449990301925222, + "velocityX": -0.052913214713167914, + "velocityY": -0.426119279886695, + "timestamp": 8.290049133403663 + }, + { + "x": 2.46411262676954, + "y": 4.160473976285577, + "heading": -0.7018758155970488, + "angularVelocity": -0.16853709734646205, + "velocityX": -0.45550618406914944, + "velocityY": -0.21004248027074415, + "timestamp": 8.44166779185019 + }, + { + "x": 2.348779581051416, + "y": 4.153307846262027, + "heading": -0.7202693675207003, + "angularVelocity": -0.121314567375086, + "velocityX": -0.7606784530447123, + "velocityY": -0.04726417363495295, + "timestamp": 8.593286450296716 + }, + { + "x": 2.201596779448434, + "y": 4.163036990055964, + "heading": -0.7333684069992377, + "angularVelocity": -0.0863946406365267, + "velocityX": -0.9707433369527995, + "velocityY": 0.06416850879734587, + "timestamp": 8.744905108743243 + }, + { + "x": 2.03723470076713, + "y": 4.1831659239379135, + "heading": -0.7500897294060416, + "angularVelocity": -0.11028538710126731, + "velocityX": -1.0840491612778367, + "velocityY": 0.13276026521150996, + "timestamp": 8.89652376718977 + }, + { + "x": 1.8800877783569956, + "y": 4.195672966501764, + "heading": -0.7289968148061016, + "angularVelocity": 0.13911819814698503, + "velocityX": -1.0364616391016628, + "velocityY": 0.08249012434493122, + "timestamp": 9.048142425636296 + }, + { + "x": 1.7046442157621393, + "y": 4.233321160679635, + "heading": -0.7238359462014516, + "angularVelocity": 0.03403847932720532, + "velocityX": -1.1571370254440554, + "velocityY": 0.24830844691554524, + "timestamp": 9.199761084082823 + }, + { + "x": 1.572815192920781, + "y": 4.2658136358994225, + "heading": -0.7241682787394389, + "angularVelocity": -0.002191897597681109, + "velocityX": -0.8694775764003041, + "velocityY": 0.21430393183203963, + "timestamp": 9.35137974252935 + }, + { + "x": 1.4994455554717883, + "y": 4.286480902556393, + "heading": -0.7254698893818832, + "angularVelocity": -0.00858476582372283, + "velocityX": -0.4839090323218549, + "velocityY": 0.13631083596916982, + "timestamp": 9.502998400975876 + }, + { + "x": 1.4994455567524874, + "y": 4.286480903049883, + "heading": -0.7254698893413497, + "angularVelocity": 3.8000232610425283e-11, + "velocityX": 2.8946950656209713e-9, + "velocityY": -5.415769760526697e-10, + "timestamp": 9.654617059422403 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/test.traj b/src/main/deploy/choreo/test.traj new file mode 100644 index 00000000..9ee66c72 --- /dev/null +++ b/src/main/deploy/choreo/test.traj @@ -0,0 +1,1084 @@ +{ + "samples": [ + { + "x": 2.1199185848236084, + "y": 5.957952976226807, + "heading": -6.25322258455275e-30, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 2.157285981558088, + "y": 5.958401136866338, + "heading": 0.034313098562848664, + "angularVelocity": 0.4715592535645141, + "velocityX": 0.5135339695272734, + "velocityY": 0.006158997744474669, + "timestamp": 0.07276518974757916 + }, + { + "x": 2.2137097634050407, + "y": 5.959057625247641, + "heading": 0.03431310711765102, + "angularVelocity": 1.1756723768181273e-7, + "velocityX": 0.7754227267555416, + "velocityY": 0.009022011535733282, + "timestamp": 0.14553037949515832 + }, + { + "x": 2.270133545252147, + "y": 5.959714113615942, + "heading": 0.03431311567244465, + "angularVelocity": 1.1756711762076828e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058456, + "timestamp": 0.21829556924273746 + }, + { + "x": 2.3265573270992537, + "y": 5.960370601984243, + "heading": 0.03431312422723829, + "angularVelocity": 1.1756711788910916e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058367, + "timestamp": 0.29106075899031664 + }, + { + "x": 2.38298110894636, + "y": 5.961027090352544, + "heading": 0.0343131327820319, + "angularVelocity": 1.175671175035012e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058381, + "timestamp": 0.3638259487378958 + }, + { + "x": 2.4394048907934667, + "y": 5.961683578720844, + "heading": 0.034313141336825545, + "angularVelocity": 1.1756711790299388e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058397, + "timestamp": 0.436591138485475 + }, + { + "x": 2.495828672640573, + "y": 5.962340067089145, + "heading": 0.03431314989161917, + "angularVelocity": 1.1756711758579864e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.0090220113570584, + "timestamp": 0.5093563282330541 + }, + { + "x": 2.5522524544876797, + "y": 5.962996555457446, + "heading": 0.0343131584464128, + "angularVelocity": 1.175671177574276e-7, + "velocityX": 0.7754227267576639, + "velocityY": 0.009022011357058414, + "timestamp": 0.5821215179806333 + }, + { + "x": 2.6086762363347864, + "y": 5.963653043825747, + "heading": 0.034313167001206415, + "angularVelocity": 1.1756711751242314e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058426, + "timestamp": 0.6548867077282124 + }, + { + "x": 2.6651000181818927, + "y": 5.964309532194048, + "heading": 0.03431317555600002, + "angularVelocity": 1.1756711746464553e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058426, + "timestamp": 0.7276518974757916 + }, + { + "x": 2.7215238000289994, + "y": 5.964966020562349, + "heading": 0.03431318411079364, + "angularVelocity": 1.1756711761494635e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058443, + "timestamp": 0.8004170872233708 + }, + { + "x": 2.7779475818761057, + "y": 5.965622508930649, + "heading": 0.034313192665587286, + "angularVelocity": 1.1756711789914241e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058452, + "timestamp": 0.87318227697095 + }, + { + "x": 2.8343713637232124, + "y": 5.96627899729895, + "heading": 0.03431320122038093, + "angularVelocity": 1.1756711786542428e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058466, + "timestamp": 0.9459474667185291 + }, + { + "x": 2.8907951455703187, + "y": 5.966935485667251, + "heading": 0.03431320977517455, + "angularVelocity": 1.1756711758478168e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058475, + "timestamp": 1.0187126564661082 + }, + { + "x": 2.9472189274174254, + "y": 5.967591974035552, + "heading": 0.03431321832996816, + "angularVelocity": 1.1756711747039204e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058483, + "timestamp": 1.0914778462136874 + }, + { + "x": 3.0036427092645317, + "y": 5.968248462403853, + "heading": 0.034313226884761766, + "angularVelocity": 1.1756711737664598e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058489, + "timestamp": 1.1642430359612665 + }, + { + "x": 3.0600664911116384, + "y": 5.968904950772154, + "heading": 0.03431323543955538, + "angularVelocity": 1.1756711759331021e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058496, + "timestamp": 1.2370082257088457 + }, + { + "x": 3.116490272958745, + "y": 5.969561439140454, + "heading": 0.034313243994349, + "angularVelocity": 1.1756711756317827e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058518, + "timestamp": 1.309773415456425 + }, + { + "x": 3.1729140548058514, + "y": 5.970217927508755, + "heading": 0.03431325254914262, + "angularVelocity": 1.1756711755305748e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058523, + "timestamp": 1.382538605204004 + }, + { + "x": 3.229337836652958, + "y": 5.970874415877057, + "heading": 0.03431326110393624, + "angularVelocity": 1.175671175226974e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.00902201135705854, + "timestamp": 1.4553037949515832 + }, + { + "x": 3.2857616185000644, + "y": 5.971530904245357, + "heading": 0.03431326965872984, + "angularVelocity": 1.1756711738766866e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058546, + "timestamp": 1.5280689846991624 + }, + { + "x": 3.342185400347171, + "y": 5.972187392613658, + "heading": 0.03431327821352346, + "angularVelocity": 1.1756711754575316e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058565, + "timestamp": 1.6008341744467416 + }, + { + "x": 3.3986091821942774, + "y": 5.972843880981959, + "heading": 0.03431328676831708, + "angularVelocity": 1.1756711769845501e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058572, + "timestamp": 1.6735993641943208 + }, + { + "x": 3.455032964041384, + "y": 5.973500369350259, + "heading": 0.034313295323110705, + "angularVelocity": 1.175671175699485e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058579, + "timestamp": 1.7463645539419 + }, + { + "x": 3.5114567458884904, + "y": 5.97415685771856, + "heading": 0.03431330387790433, + "angularVelocity": 1.1756711766849268e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058603, + "timestamp": 1.819129743689479 + }, + { + "x": 3.567880527735597, + "y": 5.9748133460868615, + "heading": 0.03431331243269794, + "angularVelocity": 1.1756711756878859e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058601, + "timestamp": 1.8918949334370583 + }, + { + "x": 3.624304309582704, + "y": 5.975469834455162, + "heading": 0.03431332098749156, + "angularVelocity": 1.1756711758891969e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058617, + "timestamp": 1.9646601231846375 + }, + { + "x": 3.68072809142981, + "y": 5.976126322823463, + "heading": 0.03431332954228517, + "angularVelocity": 1.1756711749627294e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058617, + "timestamp": 2.0374253129322164 + }, + { + "x": 3.737151873276917, + "y": 5.976782811191764, + "heading": 0.03431333809707879, + "angularVelocity": 1.1756711754302844e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058624, + "timestamp": 2.1101905026797954 + }, + { + "x": 3.793575655124023, + "y": 5.977439299560064, + "heading": 0.03431334665187242, + "angularVelocity": 1.175671176839478e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058646, + "timestamp": 2.1829556924273743 + }, + { + "x": 3.84999943697113, + "y": 5.978095787928365, + "heading": 0.034313355206666014, + "angularVelocity": 1.1756711730438892e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.00902201135705865, + "timestamp": 2.2557208821749533 + }, + { + "x": 3.906423218818236, + "y": 5.9787522762966665, + "heading": 0.03431336376145962, + "angularVelocity": 1.1756711741363398e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058672, + "timestamp": 2.328486071922532 + }, + { + "x": 3.962847000665343, + "y": 5.979408764664967, + "heading": 0.03431337231625322, + "angularVelocity": 1.1756711736937194e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058674, + "timestamp": 2.401251261670111 + }, + { + "x": 4.019270782512449, + "y": 5.980065253033268, + "heading": 0.034313380871046816, + "angularVelocity": 1.17567117272878e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.00902201135705869, + "timestamp": 2.47401645141769 + }, + { + "x": 4.075694564359556, + "y": 5.9807217414015685, + "heading": 0.034313389425840425, + "angularVelocity": 1.1756711748988721e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058704, + "timestamp": 2.546781641165269 + }, + { + "x": 4.132118346206663, + "y": 5.981378229769869, + "heading": 0.034313397980634035, + "angularVelocity": 1.1756711734690936e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.00902201135705871, + "timestamp": 2.619546830912848 + }, + { + "x": 4.188542128053769, + "y": 5.98203471813817, + "heading": 0.03431340653542763, + "angularVelocity": 1.1756711733140912e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058714, + "timestamp": 2.692312020660427 + }, + { + "x": 4.244965909900876, + "y": 5.982691206506428, + "heading": 0.03431341509022124, + "angularVelocity": 1.1756711743485013e-7, + "velocityX": 0.7754227267576708, + "velocityY": 0.00902201135645878, + "timestamp": 2.765077210408006 + }, + { + "x": 4.30138969265204, + "y": 5.983347617167627, + "heading": 0.03431342364501558, + "angularVelocity": 1.1756712748463029e-7, + "velocityX": 0.7754227391819724, + "velocityY": 0.009020943441183957, + "timestamp": 2.837842400155585 + }, + { + "x": 4.337240695953369, + "y": 5.957952976226807, + "heading": -3.4992233566514893e-26, + "angularVelocity": -0.47156372111511136, + "velocityX": 0.4926944247063125, + "velocityY": -0.3489943615747365, + "timestamp": 2.910607589903164 + }, + { + "x": 4.337075774441293, + "y": 5.930386954424772, + "heading": -0.09015124251095946, + "angularVelocity": -1.224439774519884, + "velocityX": -0.002239974219267059, + "velocityY": -0.37440342007034494, + "timestamp": 2.9842341123234344 + }, + { + "x": 4.333264434450346, + "y": 5.912061346555676, + "heading": -0.20505051130417343, + "angularVelocity": -1.5605690044322955, + "velocityX": -0.05176585645578975, + "velocityY": -0.24889954416819945, + "timestamp": 3.057860634743705 + }, + { + "x": 4.325553986362593, + "y": 5.887325303672255, + "heading": -0.3021610086195407, + "angularVelocity": -1.31896080546962, + "velocityX": -0.10472378477610225, + "velocityY": -0.33596647064523955, + "timestamp": 3.1314871571639755 + }, + { + "x": 4.313628651056446, + "y": 5.857761645119009, + "heading": -0.3834128542416627, + "angularVelocity": -1.1035676132892005, + "velocityX": -0.16197064473690057, + "velocityY": -0.40153544648615047, + "timestamp": 3.205113679584246 + }, + { + "x": 4.297606686426737, + "y": 5.824666184248557, + "heading": -0.4508559142750475, + "angularVelocity": -0.9160158298447155, + "velocityX": -0.2176113186258289, + "velocityY": -0.4495046048968324, + "timestamp": 3.2787402020045167 + }, + { + "x": 4.277844157832715, + "y": 5.789044520818464, + "heading": -0.5065111263049437, + "angularVelocity": -0.7559125461910066, + "velocityX": -0.26841589069241406, + "velocityY": -0.4838156449486921, + "timestamp": 3.3523667244247872 + }, + { + "x": 4.254799811398479, + "y": 5.751649129927804, + "heading": -0.5522697074374715, + "angularVelocity": -0.6214958907244229, + "velocityX": -0.3129897444116032, + "velocityY": -0.5079065214733656, + "timestamp": 3.425993246845058 + }, + { + "x": 4.228951375108858, + "y": 5.713030269053256, + "heading": -0.5898366283888508, + "angularVelocity": -0.5102362534107197, + "velocityX": -0.35107506697211693, + "velocityY": -0.524523766776681, + "timestamp": 3.4996197692653284 + }, + { + "x": 4.2007493706625425, + "y": 5.673585962715614, + "heading": -0.6207093446974378, + "angularVelocity": -0.419315150216672, + "velocityX": -0.3830413758419039, + "velocityY": -0.5357350183197291, + "timestamp": 3.573246291685599 + }, + { + "x": 4.1705955025497214, + "y": 5.633604594294146, + "heading": -0.6461809126237779, + "angularVelocity": -0.34595641745707684, + "velocityX": -0.4095517093785753, + "velocityY": -0.5430294288958701, + "timestamp": 3.6468728141058695 + }, + { + "x": 4.138835739465679, + "y": 5.593298519659669, + "heading": -0.6673577962188777, + "angularVelocity": -0.28762574815389413, + "velocityX": -0.43136307460989803, + "velocityY": -0.5474396088464458, + "timestamp": 3.72049933652614 + }, + { + "x": 4.1057614035434415, + "y": 5.552829496334061, + "heading": -0.6851851845597893, + "angularVelocity": -0.2421326955950787, + "velocityX": -0.44921768453825217, + "velocityY": -0.549652788089118, + "timestamp": 3.7941258589464106 + }, + { + "x": 4.071614137002915, + "y": 5.512327581345269, + "heading": -0.700475200245641, + "angularVelocity": -0.20766994261353336, + "velocityX": -0.4637902948289247, + "velocityY": -0.5500995247011782, + "timestamp": 3.867752381366681 + }, + { + "x": 4.036592388923184, + "y": 5.471905240898815, + "heading": -0.7139354213530316, + "angularVelocity": -0.18281755901165134, + "velocityX": -0.47566755740304917, + "velocityY": -0.5490187383252648, + "timestamp": 3.9413789037869518 + }, + { + "x": 4.0008581978077, + "y": 5.431668194433558, + "heading": -0.7261965659955223, + "angularVelocity": -0.16653162799816065, + "velocityX": -0.4853440029600765, + "velocityY": -0.5465020639651851, + "timestamp": 4.015005426207223 + }, + { + "x": 3.964543737766948, + "y": 5.391724222217225, + "heading": -0.7378391147106159, + "angularVelocity": -0.15812981969508613, + "velocityX": -0.49322525153997593, + "velocityY": -0.5425215113152707, + "timestamp": 4.088631948627493 + }, + { + "x": 3.9277575195733885, + "y": 5.352190902296246, + "heading": -0.7494192294944804, + "angularVelocity": -0.15728183816374608, + "velocityX": -0.4996327000693894, + "velocityY": -0.536944006336704, + "timestamp": 4.162258471047764 + }, + { + "x": 3.89059042061129, + "y": 5.3132030416510085, + "heading": -0.761494708226674, + "angularVelocity": -0.16400990207394403, + "velocityX": -0.5048058463218383, + "velocityY": -0.5295355445784652, + "timestamp": 4.2358849934680345 + }, + { + "x": 3.8531219539867014, + "y": 5.274920426877835, + "heading": -0.7746519957947015, + "angularVelocity": -0.1787030968666938, + "velocityX": -0.5088990406298591, + "velocityY": -0.5199568513456381, + "timestamp": 4.309511515888305 + }, + { + "x": 3.8154274539330624, + "y": 5.23753642375436, + "heading": -0.7895355361070671, + "angularVelocity": -0.2021491688471734, + "velocityX": -0.5119690407007602, + "velocityY": -0.507751852112243, + "timestamp": 4.383138038308576 + }, + { + "x": 3.7775872299458775, + "y": 5.201287866945812, + "heading": -0.8068810412551409, + "angularVelocity": -0.23558772814317216, + "velocityX": -0.5139482722161929, + "velocityY": -0.49233014974734224, + "timestamp": 4.456764560728846 + }, + { + "x": 3.7396993230404707, + "y": 5.166466510716887, + "heading": -0.8275545372921488, + "angularVelocity": -0.2807887070776005, + "velocityX": -0.5145959045727775, + "velocityY": -0.4729458228402956, + "timestamp": 4.530391083149117 + }, + { + "x": 3.701898417706247, + "y": 5.133430805056498, + "heading": -0.8525964909440766, + "angularVelocity": -0.3401213697013267, + "velocityX": -0.5134142438298316, + "velocityY": -0.44869300592275085, + "timestamp": 4.604017605569387 + }, + { + "x": 3.664343129524347, + "y": 5.101026571041832, + "heading": -0.8792757408403825, + "angularVelocity": -0.36235922897480916, + "velocityX": -0.5100782564132129, + "velocityY": -0.44011631881374225, + "timestamp": 4.677644127989658 + }, + { + "x": 3.6270751479601078, + "y": 5.068774406382283, + "heading": -0.9064492634455257, + "angularVelocity": -0.3690724716024585, + "velocityX": -0.5061760400893075, + "velocityY": -0.4380509033884428, + "timestamp": 4.7512706504099285 + }, + { + "x": 3.590111595107405, + "y": 5.036682857394856, + "heading": -0.9341662862028731, + "angularVelocity": -0.37645432442313, + "velocityX": -0.5020412704230309, + "velocityY": -0.43586941135483154, + "timestamp": 4.824897172830199 + }, + { + "x": 3.55347222485786, + "y": 5.004762522170154, + "heading": -0.9624850330743058, + "angularVelocity": -0.38462697870999835, + "velocityX": -0.4976382021739686, + "velocityY": -0.4335439753964831, + "timestamp": 4.89852369525047 + }, + { + "x": 3.517180102788644, + "y": 4.973026350131823, + "heading": -0.9914744586380927, + "angularVelocity": -0.3937361783612594, + "velocityX": -0.49292185582330617, + "velocityY": -0.4310426595619462, + "timestamp": 4.97215021767074 + }, + { + "x": 3.4812625207112933, + "y": 4.941490315870616, + "heading": -1.0212172545129166, + "angularVelocity": -0.40396850071293, + "velocityX": -0.48783483039342795, + "velocityY": -0.42832437584372535, + "timestamp": 5.045776740091011 + }, + { + "x": 3.445752274995712, + "y": 4.9101744053392995, + "heading": -1.051814154831974, + "angularVelocity": -0.41556900031765764, + "velocityX": -0.4823023626307331, + "velocityY": -0.4253346416738305, + "timestamp": 5.119403262511281 + }, + { + "x": 3.4106895099559638, + "y": 4.879104067704057, + "heading": -1.0833902078498774, + "angularVelocity": -0.4288679130824993, + "velocityX": -0.4762246523013149, + "velocityY": -0.4219992553483426, + "timestamp": 5.193029784931552 + }, + { + "x": 3.376124474151927, + "y": 4.848312423976759, + "heading": -1.1161042305606605, + "angularVelocity": -0.44432388812345175, + "velocityX": -0.4694644629109969, + "velocityY": -0.4182140173826824, + "timestamp": 5.266656307351822 + }, + { + "x": 3.3421218274128583, + "y": 4.817843776695172, + "heading": -1.1501637008410697, + "angularVelocity": -0.4625978405715368, + "velocityX": -0.4618260597040901, + "velocityY": -0.4138270595977395, + "timestamp": 5.340282829772093 + }, + { + "x": 3.3087677521953345, + "y": 4.787759503770135, + "heading": -1.1858495459870186, + "angularVelocity": -0.48468736499938486, + "velocityX": -0.45301712101970637, + "velocityY": -0.40860646321595784, + "timestamp": 5.413909352192364 + }, + { + "x": 3.2761825599174124, + "y": 4.758148688152823, + "heading": -1.2235604524642407, + "angularVelocity": -0.5121918737647652, + "velocityX": -0.44257410518347007, + "velocityY": -0.4021759366589302, + "timestamp": 5.487535874612634 + }, + { + "x": 3.244545279648596, + "y": 4.729149218235016, + "heading": -1.2639000345369864, + "angularVelocity": -0.547894708953951, + "velocityX": -0.4296995054068451, + "velocityY": -0.3938726014013467, + "timestamp": 5.561162397032905 + }, + { + "x": 3.2141486367937797, + "y": 4.700995814255907, + "heading": -1.3078734515357762, + "angularVelocity": -0.5972496805944914, + "velocityX": -0.4128490910015826, + "velocityY": -0.382381281278044, + "timestamp": 5.634788919453175 + }, + { + "x": 3.185551521789466, + "y": 4.674155611334741, + "heading": -1.357437524753386, + "angularVelocity": -0.6731823205595795, + "velocityX": -0.38840779197850694, + "velocityY": -0.36454530295424753, + "timestamp": 5.708415441873446 + }, + { + "x": 3.161389111124608, + "y": 4.65130116728667, + "heading": -1.4227635315487523, + "angularVelocity": -0.8872618812888667, + "velocityX": -0.3281753622279693, + "velocityY": -0.3104104784090629, + "timestamp": 5.782041964293716 + }, + { + "x": 3.1613881587982178, + "y": 4.647716999053955, + "heading": -1.5707963267948966, + "angularVelocity": -2.010590618434374, + "velocityX": -0.00001293455617664302, + "velocityY": -0.048680395527252375, + "timestamp": 5.855668486713987 + }, + { + "x": 3.1590242751538056, + "y": 4.648068652143942, + "heading": -1.71822885135636, + "angularVelocity": -2.0416470093627317, + "velocityX": -0.032735083303029204, + "velocityY": 0.0048696953514198, + "timestamp": 5.927881030746015 + }, + { + "x": 3.147722843506569, + "y": 4.651261007949863, + "heading": -1.8460861775851984, + "angularVelocity": -1.7705694757427723, + "velocityX": -0.15650233347580403, + "velocityY": 0.04420777371456313, + "timestamp": 6.000093574778043 + }, + { + "x": 3.1294383579479246, + "y": 4.658664086881976, + "heading": -1.9555006668792347, + "angularVelocity": -1.5151728935835482, + "velocityX": -0.25320373078858216, + "velocityY": 0.10251790781432463, + "timestamp": 6.07230611881007 + }, + { + "x": 3.105950392035797, + "y": 4.670811298853881, + "heading": -2.048080530965875, + "angularVelocity": -1.2820468427975569, + "velocityX": -0.325261576461146, + "velocityY": 0.16821470749621126, + "timestamp": 6.144518662842098 + }, + { + "x": 3.078734489629141, + "y": 4.687681461713096, + "heading": -2.125691691683944, + "angularVelocity": -1.0747600954710448, + "velocityX": -0.37688607667090807, + "velocityY": 0.23361817652806605, + "timestamp": 6.216731206874126 + }, + { + "x": 3.048937913872785, + "y": 4.708930614799334, + "heading": -2.190285310241282, + "angularVelocity": -0.8944930472009076, + "velocityX": -0.41262326588495246, + "velocityY": 0.2942584750485111, + "timestamp": 6.288943750906154 + }, + { + "x": 3.0174119547024816, + "y": 4.734060139937329, + "heading": -2.243768219004051, + "angularVelocity": -0.7406318317638603, + "velocityX": -0.4365717839316291, + "velocityY": 0.347993904311833, + "timestamp": 6.361156294938182 + }, + { + "x": 2.9847672349131935, + "y": 4.762525988198673, + "heading": -2.2879169282753247, + "angularVelocity": -0.6113717479845765, + "velocityX": -0.4520643916770092, + "velocityY": 0.3941953388142518, + "timestamp": 6.4333688389702095 + }, + { + "x": 2.9514318737909195, + "y": 4.793802004717416, + "heading": -2.3243308683279778, + "angularVelocity": -0.5042605899122328, + "velocityX": -0.4616283994577112, + "velocityY": 0.4331105757037482, + "timestamp": 6.505581383002237 + }, + { + "x": 2.9177026495619596, + "y": 4.827411588267587, + "heading": -2.3544162803840454, + "angularVelocity": -0.4166230737242065, + "velocityX": -0.46708261952383673, + "velocityY": 0.46542583426037293, + "timestamp": 6.577793927034265 + }, + { + "x": 2.88378589769779, + "y": 4.862939604927141, + "heading": -2.379391061920221, + "angularVelocity": -0.3458510134347111, + "velocityX": -0.4696795040087044, + "velocityY": 0.4919923142964852, + "timestamp": 6.650006471066293 + }, + { + "x": 2.8498283989976017, + "y": 4.900033117168976, + "heading": -2.4003021659557313, + "angularVelocity": -0.289577168562792, + "velocityX": -0.47024376658337114, + "velocityY": 0.5136713120837215, + "timestamp": 6.722219015098321 + }, + { + "x": 2.8159399738054236, + "y": 4.938396448572123, + "heading": -2.4180494138871094, + "angularVelocity": -0.2457640589910084, + "velocityX": -0.46928723598420935, + "velocityY": 0.5312557799671519, + "timestamp": 6.794431559130349 + }, + { + "x": 2.7822098245267197, + "y": 4.977783840614619, + "heading": -2.4334117988726813, + "angularVelocity": -0.2127384541217516, + "velocityX": -0.4670954296215342, + "velocityY": 0.5454369814893605, + "timestamp": 6.866644103162376 + }, + { + "x": 2.7487184813842567, + "y": 5.0179914540575155, + "heading": -2.4470741047795617, + "angularVelocity": -0.18919574278978962, + "velocityX": -0.46378843996424124, + "velocityY": 0.5567954152821849, + "timestamp": 6.938856647194404 + }, + { + "x": 2.7155468563385656, + "y": 5.0588495367779895, + "heading": -2.459652889491748, + "angularVelocity": -0.17419113092882704, + "velocityX": -0.4593609807041143, + "velocityY": 0.5658031200556053, + "timestamp": 7.011069191226432 + }, + { + "x": 2.682783562068564, + "y": 5.100215023933873, + "heading": -2.471721683170927, + "angularVelocity": -0.16712877022898326, + "velocityX": -0.45370641221933317, + "velocityY": 0.5728296615271857, + "timestamp": 7.08328173525846 + }, + { + "x": 2.650531366839192, + "y": 5.141964497495229, + "heading": -2.4838357664761026, + "angularVelocity": -0.16775594140268155, + "velocityX": -0.44662870781934594, + "velocityY": 0.5781471089405039, + "timestamp": 7.155494279290488 + }, + { + "x": 2.618913440143494, + "y": 5.183987197473736, + "heading": -2.496557238457018, + "angularVelocity": -0.17616706559006176, + "velocityX": -0.4378453511023647, + "velocityY": 0.5819307509768665, + "timestamp": 7.2277068233225155 + }, + { + "x": 2.5880798824866296, + "y": 5.226177564560792, + "heading": -2.5104813496102354, + "angularVelocity": -0.1928212243435465, + "velocityX": -0.4269834011551906, + "velocityY": 0.5842526066986774, + "timestamp": 7.299919367354543 + }, + { + "x": 2.5582148956282778, + "y": 5.268426535906725, + "heading": -2.5262653286045555, + "angularVelocity": -0.21857669198470553, + "velocityX": -0.41357062347929907, + "velocityY": 0.585064159035808, + "timestamp": 7.372131911386571 + }, + { + "x": 2.5295447873325214, + "y": 5.31061044475372, + "heading": -2.5446612115646436, + "angularVelocity": -0.2547463630685689, + "velocityX": -0.39702393372321826, + "velocityY": 0.5841631729285931, + "timestamp": 7.444344455418599 + }, + { + "x": 2.5023467091367184, + "y": 5.352575812308809, + "heading": -2.566554449178874, + "angularVelocity": -0.3031777637486395, + "velocityX": -0.3766392468286359, + "velocityY": 0.5811368110293486, + "timestamp": 7.516556999450627 + }, + { + "x": 2.476956466389222, + "y": 5.394117632986223, + "heading": -2.593007817119188, + "angularVelocity": -0.36632649209230705, + "velocityX": -0.3516043242602833, + "velocityY": 0.5752715298188288, + "timestamp": 7.5887695434826545 + }, + { + "x": 2.4521989817386887, + "y": 5.435268589005595, + "heading": -2.621259955334882, + "angularVelocity": -0.39123588005934157, + "velocityX": -0.3428418840852991, + "velocityY": 0.5698588322981821, + "timestamp": 7.660982087514682 + }, + { + "x": 2.427559065081996, + "y": 5.476070836186979, + "heading": -2.6500802362174434, + "angularVelocity": -0.39910352514071706, + "velocityX": -0.34121380138282903, + "velocityY": 0.5650299089765801, + "timestamp": 7.73319463154671 + }, + { + "x": 2.403044029416898, + "y": 5.516501717159452, + "heading": -2.6795300674706377, + "angularVelocity": -0.407821544690809, + "velocityX": -0.3394844482175417, + "velocityY": 0.559887226165873, + "timestamp": 7.805407175578738 + }, + { + "x": 2.378663293884139, + "y": 5.556534603961008, + "heading": -2.7096829003029756, + "angularVelocity": -0.41755671727843513, + "velocityX": -0.33762465870120245, + "velocityY": 0.5543757990828, + "timestamp": 7.877619719610766 + }, + { + "x": 2.354428772799968, + "y": 5.59613781059984, + "heading": -2.74062708945702, + "angularVelocity": -0.42851542718561086, + "velocityX": -0.33559987961957327, + "velocityY": 0.5484255840822709, + "timestamp": 7.949832263642794 + }, + { + "x": 2.330355697409695, + "y": 5.635273029678329, + "heading": -2.77247058331099, + "angularVelocity": -0.4409690072662031, + "velocityX": -0.3333641781072918, + "velocityY": 0.5419448878733852, + "timestamp": 8.02204480767482 + }, + { + "x": 2.306463861338364, + "y": 5.673893057942382, + "heading": -2.8053478457859313, + "angularVelocity": -0.45528464501069815, + "velocityX": -0.33085437428619124, + "velocityY": 0.534810520550616, + "timestamp": 8.094257351706847 + }, + { + "x": 2.282779542094817, + "y": 5.711938365696433, + "heading": -2.839430372846894, + "angularVelocity": -0.47197516051846805, + "velocityX": -0.3279806792714956, + "velocityY": 0.5268517854346452, + "timestamp": 8.166469895738874 + }, + { + "x": 2.259338596376033, + "y": 5.749331681355029, + "heading": -2.8749434001407064, + "angularVelocity": -0.4917847414164131, + "velocityX": -0.3246104403742845, + "velocityY": 0.5178229926647014, + "timestamp": 8.238682439770901 + }, + { + "x": 2.2361917393720554, + "y": 5.785968937848207, + "heading": -2.9121940133772717, + "angularVelocity": -0.5158468481603996, + "velocityX": -0.3205378970405998, + "velocityY": 0.5073530781151929, + "timestamp": 8.310894983802928 + }, + { + "x": 2.2134142469672544, + "y": 5.821702966064213, + "heading": -2.9516221084722227, + "angularVelocity": -0.5460006377488126, + "velocityX": -0.3154229325406202, + "velocityY": 0.4948451642993865, + "timestamp": 8.383107527834955 + }, + { + "x": 2.191125686501144, + "y": 5.856311001786043, + "heading": -2.9939026154296866, + "angularVelocity": -0.5855008644856994, + "velocityX": -0.3086521983801695, + "velocityY": 0.4792524094772386, + "timestamp": 8.455320071866982 + }, + { + "x": 2.169536365545648, + "y": 5.8894197426408175, + "heading": -3.0401828782490323, + "angularVelocity": -0.6408895218927577, + "velocityX": -0.29896912295350214, + "velocityY": 0.4584901598272232, + "timestamp": 8.527532615899009 + }, + { + "x": 2.1490856813219974, + "y": 5.9202865649088325, + "heading": -3.092780352693255, + "angularVelocity": -0.7283703288571992, + "velocityX": -0.28320127060722483, + "velocityY": 0.42744404980837086, + "timestamp": 8.599745159931036 + }, + { + "x": 2.131117343902588, + "y": 5.946754455566406, + "heading": -3.158540067224243, + "angularVelocity": -0.9106411553901581, + "velocityX": -0.24882570833455084, + "velocityY": 0.3665276028197312, + "timestamp": 8.671957703963063 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/commandless.auto b/src/main/deploy/pathplanner/autos/commandless.auto new file mode 100644 index 00000000..1b28b59b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/commandless.auto @@ -0,0 +1,37 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.7470832465334709, + "y": 6.625458718233556 + }, + "rotation": 60.53273310463171 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "amp 4 local sgmt 1" + } + }, + { + "type": "path", + "data": { + "pathName": "amp 4 local sgmt 2" + } + }, + { + "type": "path", + "data": { + "pathName": "amp 4 local sgmt 3" + } + } + ] + } + }, + "folder": null, + "choreoAuto": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/local 4.auto b/src/main/deploy/pathplanner/autos/local 4.auto index 67fcef43..347fd4ae 100644 --- a/src/main/deploy/pathplanner/autos/local 4.auto +++ b/src/main/deploy/pathplanner/autos/local 4.auto @@ -18,7 +18,7 @@ } }, { - "type": "parallel", + "type": "deadline", "data": { "commands": [ { @@ -28,9 +28,22 @@ } }, { - "type": "named", + "type": "race", "data": { - "name": "intake" + "commands": [ + { + "type": "named", + "data": { + "name": "intake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + } + ] } } ] @@ -43,7 +56,7 @@ } }, { - "type": "sequential", + "type": "deadline", "data": { "commands": [ { @@ -53,9 +66,22 @@ } }, { - "type": "named", + "type": "race", "data": { - "name": "intake" + "commands": [ + { + "type": "named", + "data": { + "name": "intake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.5 + } + } + ] } } ] @@ -68,7 +94,7 @@ } }, { - "type": "sequential", + "type": "deadline", "data": { "commands": [ { @@ -78,9 +104,22 @@ } }, { - "type": "named", + "type": "race", "data": { - "name": "intake" + "commands": [ + { + "type": "named", + "data": { + "name": "intake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.5 + } + } + ] } } ] diff --git a/src/main/deploy/pathplanner/autos/single.auto b/src/main/deploy/pathplanner/autos/single.auto new file mode 100644 index 00000000..e800b224 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/single.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.7470832467079163, + "y": 6.625458717346191 + }, + "rotation": 60.53273310724105 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "amp 4 local" + } + } + ] + } + }, + "folder": null, + "choreoAuto": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/triangle test.auto b/src/main/deploy/pathplanner/autos/triangle test.auto new file mode 100644 index 00000000..afbf7a14 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/triangle test.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 2.1199185848236084, + "y": 5.957952976226807 + }, + "rotation": 2.7794689858351906e-30 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "test" + } + } + ] + } + }, + "folder": null, + "choreoAuto": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java new file mode 100644 index 00000000..0be41050 --- /dev/null +++ b/src/main/java/frc/robot/Constants.java @@ -0,0 +1,3 @@ +package frc.robot; + +public class Constants {} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 36b5abff..9a6886b9 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -6,6 +6,8 @@ import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.PathPlannerAuto; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; @@ -13,6 +15,12 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.subsystems.pivot.PivotIOReal; +import frc.robot.subsystems.pivot.PivotSubsystem; +import frc.robot.subsystems.routing.RoutingIOReal; +import frc.robot.subsystems.routing.RoutingSubsystem; +import frc.robot.subsystems.shooter.ShooterIOReal; +import frc.robot.subsystems.shooter.ShooterSubsystem; import frc.robot.subsystems.swerve.GyroIO; import frc.robot.subsystems.swerve.GyroIOPigeon2; import frc.robot.subsystems.swerve.SwerveSubsystem; @@ -41,6 +49,9 @@ public static enum RobotMode { mode == RobotMode.REAL ? SwerveSubsystem.createTalonFXModules() : SwerveSubsystem.createSimModules()); + private final ShooterSubsystem shooter = new ShooterSubsystem(new ShooterIOReal()); + private final PivotSubsystem pivot = new PivotSubsystem(new PivotIOReal()); + private final RoutingSubsystem routing = new RoutingSubsystem(new RoutingIOReal()); @Override public void robotInit() { @@ -64,7 +75,7 @@ public void robotInit() { switch (mode) { case REAL: - Logger.addDataReceiver(new WPILOGWriter("/U")); // Log to a USB stick + // Logger.addDataReceiver(new WPILOGWriter("/U")); // Log to a USB stick Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables new PowerDistribution(1, ModuleType.kRev); // Enables power distribution logging break; @@ -93,16 +104,32 @@ public void robotInit() { new ChassisSpeeds( -controller.getLeftY() * SwerveSubsystem.MAX_LINEAR_SPEED, -controller.getLeftX() * SwerveSubsystem.MAX_LINEAR_SPEED, - controller.getRightX() * SwerveSubsystem.MAX_ANGULAR_SPEED))); - + -controller.getRightX() * SwerveSubsystem.MAX_ANGULAR_SPEED))); + + shooter.setDefaultCommand(shooter.run(0.0)); + pivot.setDefaultCommand(pivot.run(-100.0)); + routing.setDefaultCommand(routing.stop()); + + // Controller bindings here + controller.start().onTrue(Commands.runOnce(() -> swerve.setYaw(Rotation2d.fromDegrees(0)))); + + controller.leftTrigger().whileTrue(intake()); + controller + .rightTrigger() + .whileTrue(Commands.parallel(shooter.runVelocity(80.0), routing.run(80.0))); + controller + .leftBumper() + .whileTrue( + Commands.parallel( + pivot.run(10.0), + shooter.run(-2.0), + Commands.waitSeconds(0.5).andThen(routing.run(-6.0 * 360)))); + controller + .a() + .onTrue(swerve.runOnce(() -> swerve.setPose(new Pose2d(2.0, 2.0, new Rotation2d())))); // Auto Bindings here - NamedCommands.registerCommand( - "fender", - Commands.deadline( - Commands.sequence( - Commands.print("fender shot"), Commands.waitSeconds(1.0), Commands.print("pew!")), - swerve.stopCmd())); - NamedCommands.registerCommand("intake", Commands.print("intake")); + NamedCommands.registerCommand("fender", shootFender()); + NamedCommands.registerCommand("intake", intake()); NamedCommands.registerCommand("stop", swerve.stopWithXCmd().asProxy()); } @@ -111,15 +138,9 @@ public void robotPeriodic() { CommandScheduler.getInstance().run(); } - @Override - public void disabledInit() {} - @Override public void disabledPeriodic() {} - @Override - public void disabledExit() {} - @Override public void autonomousInit() { autonomousCommand = new PathPlannerAuto("local 4"); @@ -129,12 +150,6 @@ public void autonomousInit() { } } - @Override - public void autonomousPeriodic() {} - - @Override - public void autonomousExit() {} - @Override public void teleopInit() { if (autonomousCommand != null) { @@ -145,17 +160,22 @@ public void teleopInit() { @Override public void teleopPeriodic() {} - @Override - public void teleopExit() {} - @Override public void testInit() { CommandScheduler.getInstance().cancelAll(); } - @Override - public void testPeriodic() {} + private Command intake() { + return Commands.parallel(shooter.run(5.0), pivot.run(0.0)); + } - @Override - public void testExit() {} + private Command shootFender() { + return Commands.parallel( + swerve.stopCmd(), + pivot.run(-160.0), + Commands.waitSeconds(0.5).andThen(shooter.run(-40.0)), + Commands.waitSeconds(1.0).andThen(routing.run(-40.0).asProxy())) + .withTimeout(1.5) + .andThen(Commands.print("done shooting")); + } } diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotIO.java b/src/main/java/frc/robot/subsystems/pivot/PivotIO.java new file mode 100644 index 00000000..f50514e2 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/pivot/PivotIO.java @@ -0,0 +1,27 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.pivot; + +import org.littletonrobotics.junction.AutoLog; + +/** Add your docs here. */ +public interface PivotIO { + @AutoLog + public class PivotIOInputs { + + // Motor values + public double velocityRPS; + public double currentDrawAmps; + public double temperatureCelsius; + public double motorOutputVolts; + public double positionRotations; + } + + public abstract void setPosition(double degrees); + + public abstract void reset(double degrees); + + public abstract PivotIOInputsAutoLogged updateInputs(); +} diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java b/src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java new file mode 100644 index 00000000..f5da4caf --- /dev/null +++ b/src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java @@ -0,0 +1,67 @@ +package frc.robot.subsystems.pivot; + +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.GravityTypeValue; +import edu.wpi.first.math.util.Units; + +public class PivotIOReal implements PivotIO { + + public static final int PIVOT_MOTOR_ID = 10; + public static final double PIVOT_GEAR_RATIO = + (27.0 / 1) * (48.0 / 22); // check if this is the correct gear ratio + + private TalonFX pivotMotor = new TalonFX(10); + private MotionMagicVoltage motorRequest = new MotionMagicVoltage(0.0); + + private StatusSignal supplyVoltageSignal = pivotMotor.getMotorVoltage(); + private StatusSignal position = pivotMotor.getRotorPosition(); + private StatusSignal velocity = pivotMotor.getRotorVelocity(); + private StatusSignal currentDraw = pivotMotor.getStatorCurrent(); + + public PivotIOReal() { + TalonFXConfiguration pivotConfig = new TalonFXConfiguration(); + pivotConfig.Slot0.kP = 120; + pivotConfig.Slot0.kD = 0.0; + pivotConfig.Slot0.kI = 0; + pivotConfig.Slot0.GravityType = GravityTypeValue.Arm_Cosine; + pivotConfig.Slot0.kG = 0.4; + pivotConfig.MotionMagic.MotionMagicCruiseVelocity = 11; + pivotConfig.MotionMagic.MotionMagicAcceleration = 6; + + pivotConfig.Feedback.SensorToMechanismRatio = 58.9; + + pivotConfig.CurrentLimits.StatorCurrentLimit = 60.0; + pivotConfig.CurrentLimits.StatorCurrentLimitEnable = true; + + pivotMotor.getConfigurator().apply(pivotConfig); + } + + @Override + public void setPosition(double degrees) { + pivotMotor.setControl(motorRequest.withPosition(Units.degreesToRotations(degrees))); + } + + @Override + public void reset(double degrees) { + pivotMotor.setPosition((Units.degreesToRotations(degrees))); + } + + @Override + public PivotIOInputsAutoLogged updateInputs() { + PivotIOInputsAutoLogged updated = new PivotIOInputsAutoLogged(); + + updated.currentDrawAmps = currentDraw.getValue(); + updated.positionRotations = position.getValue(); + updated.velocityRPS = velocity.getValue(); + updated.motorOutputVolts = 12 * supplyVoltageSignal.getValue(); + + return (updated); + } +} diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotIOSim.java b/src/main/java/frc/robot/subsystems/pivot/PivotIOSim.java new file mode 100644 index 00000000..4e17ac51 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/pivot/PivotIOSim.java @@ -0,0 +1,55 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.pivot; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; + +/** Add your docs here. */ +public class PivotIOSim implements PivotIO { + + SingleJointedArmSim arm = + new SingleJointedArmSim( + DCMotor.getFalcon500(1), + 27 * (24 / 18), + SingleJointedArmSim.estimateMOI(Units.inchesToMeters(30), 3), + Units.inchesToMeters(30), + 0, + 3.5, + true, + 0); + PIDController pid = new PIDController(1, 0, 0); + + public PivotIOInputsAutoLogged updateInputs() { + + PivotIOInputsAutoLogged input = new PivotIOInputsAutoLogged(); + + // var motorSimState = motor.getSimState(); + + input.velocityRPS = arm.getVelocityRadPerSec() * 60; + input.currentDrawAmps = arm.getCurrentDrawAmps(); + input.temperatureCelsius = 0; + input.motorOutputVolts = arm.getOutput(0); + input.positionRotations = Units.radiansToDegrees(arm.getAngleRads()); + + arm.update(0.020); + + return input; + } + + @Override + public void setPosition(double degrees) { + double outputVolts = pid.calculate(Units.radiansToDegrees(arm.getAngleRads()), degrees); + arm.setInput(outputVolts); + } + + @Override + public void reset(double degrees) { + // TODO Auto-generated method stub + + } +} diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotSubsystem.java b/src/main/java/frc/robot/subsystems/pivot/PivotSubsystem.java new file mode 100644 index 00000000..54c73dfa --- /dev/null +++ b/src/main/java/frc/robot/subsystems/pivot/PivotSubsystem.java @@ -0,0 +1,51 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.pivot; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger; + +/** Add your docs here. */ +public class PivotSubsystem extends SubsystemBase { + PivotIO io; + PivotIOInputsAutoLogged inputs; + + public PivotSubsystem(PivotIO io) { + this.io = io; + inputs = new PivotIOInputsAutoLogged(); + + SmartDashboard.putData( + "reset Pivot value", + new InstantCommand( + () -> { + io.reset(0); + })); + } + + public Command reset() { + return this.runOnce( + () -> { + io.reset(0); + }); + } + + public Command run(double degrees) { + return new RunCommand( + () -> { + io.setPosition(degrees); + }, + this); + } + + @Override + public void periodic() { + inputs = io.updateInputs(); + Logger.processInputs("Pivot", inputs); + } +} diff --git a/src/main/java/frc/robot/subsystems/routing/RoutingIO.java b/src/main/java/frc/robot/subsystems/routing/RoutingIO.java new file mode 100644 index 00000000..9a051933 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/routing/RoutingIO.java @@ -0,0 +1,25 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.routing; + +import org.littletonrobotics.junction.AutoLog; + +/** Add your docs here. */ +public interface RoutingIO { + @AutoLog + public static class RoutingIOInputs { + // Motor values + public double velocityRPS = 0.0; + public double currentDrawAmps = 0.0; + public double temperatureCelsius = 0.0; + public double motorOutputVolts = 0.0; + } + + public abstract void updateInputs(RoutingIOInputsAutoLogged inputs); + + public abstract void setVelocity(double rps); + + public abstract void setVoltage(double voltage); +} diff --git a/src/main/java/frc/robot/subsystems/routing/RoutingIOReal.java b/src/main/java/frc/robot/subsystems/routing/RoutingIOReal.java new file mode 100644 index 00000000..92f761e6 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/routing/RoutingIOReal.java @@ -0,0 +1,56 @@ +package frc.robot.subsystems.routing; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; + +public class RoutingIOReal implements RoutingIO { + public static final int KICKER_MOTOR_ID = 12; + + private TalonFX motor = new TalonFX(12); + private VelocityVoltage velocityOut = new VelocityVoltage(0.0).withEnableFOC(true); + private VoltageOut voltageOut = new VoltageOut(0.0); + + private StatusSignal voltage = motor.getMotorVoltage(); + private StatusSignal velocity = motor.getVelocity(); + private StatusSignal currentDraw = motor.getStatorCurrent(); + + public RoutingIOReal() { + TalonFXConfiguration config = new TalonFXConfiguration(); + config.Slot0.kP = 0.1; + config.Slot0.kD = 0.0; + config.Slot0.kI = 0.0; + config.Slot0.kV = 0.12; + + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + + config.CurrentLimits.StatorCurrentLimit = 60.0; + config.CurrentLimits.StatorCurrentLimitEnable = true; + config.Feedback.SensorToMechanismRatio = 1.0; + motor.getConfigurator().apply(config); + + BaseStatusSignal.setUpdateFrequencyForAll(50, voltage, velocity, currentDraw); + motor.optimizeBusUtilization(); + } + + public void updateInputs(RoutingIOInputsAutoLogged inputs) { + BaseStatusSignal.refreshAll(currentDraw, velocity, voltage); + inputs.currentDrawAmps = currentDraw.getValueAsDouble(); + inputs.velocityRPS = velocity.getValueAsDouble(); + inputs.motorOutputVolts = voltage.getValueAsDouble(); + } + + @Override + public void setVelocity(double rps) { + motor.setControl(velocityOut.withVelocity(rps)); + } + + @Override + public void setVoltage(double voltage) { + motor.setControl(voltageOut.withOutput(voltage)); + } +} diff --git a/src/main/java/frc/robot/subsystems/routing/RoutingSubsystem.java b/src/main/java/frc/robot/subsystems/routing/RoutingSubsystem.java new file mode 100644 index 00000000..91ffc7ba --- /dev/null +++ b/src/main/java/frc/robot/subsystems/routing/RoutingSubsystem.java @@ -0,0 +1,32 @@ +package frc.robot.subsystems.routing; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger; + +public class RoutingSubsystem extends SubsystemBase { + RoutingIO io; + RoutingIOInputsAutoLogged inputs; + + public RoutingSubsystem(RoutingIO io) { + this.io = io; + inputs = new RoutingIOInputsAutoLogged(); + } + + public Command run(double velocity) { + return this.run( + () -> { + io.setVelocity(velocity); + }); + } + + public Command stop() { + return this.run(() -> io.setVoltage(0.0)); + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Routing", inputs); + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java new file mode 100644 index 00000000..735ce4f7 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -0,0 +1,26 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.shooter; + +import org.littletonrobotics.junction.AutoLog; + +/** Add your docs here. */ +public interface ShooterIO { + @AutoLog + public static class ShooterIOInputs { + + // Motor values + public double velocityRPS = 0.0; + public double currentDrawAmps = 0.0; + public double temperatureCelsius = 0.0; + public double motorOutputVolts = 0.0; + } + + public abstract void setVoltage(double voltage); + + public abstract void setVelocity(double rps); + + public abstract void updateInputs(ShooterIOInputsAutoLogged inputs); +} diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java new file mode 100644 index 00000000..af959bc6 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java @@ -0,0 +1,64 @@ +package frc.robot.subsystems.shooter; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; + +public class ShooterIOReal implements ShooterIO { + + public static final int SHOOTER_MOTOR_ID = 27; + public static final double SHOOTER_GEAR_RATIO = 1; + + private TalonFX motor = new TalonFX(27); + private VelocityVoltage velocityVoltage = new VelocityVoltage(0).withEnableFOC(true); + private VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true); + + private StatusSignal voltage = motor.getMotorVoltage(); + private StatusSignal velocity = motor.getVelocity(); + private StatusSignal currentDraw = motor.getStatorCurrent(); + + public ShooterIOReal() { + TalonFXConfiguration config = new TalonFXConfiguration(); + config.Slot0.kP = 0.1; + config.Slot0.kD = 0; + config.Slot0.kI = 0; + config.Slot0.kV = 0.12; + + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + + config.CurrentLimits.StatorCurrentLimit = 60.0; + config.CurrentLimits.StatorCurrentLimitEnable = true; + motor.getConfigurator().apply(config); + + voltage = motor.getMotorVoltage(); + velocity = motor.getVelocity(); + currentDraw = motor.getStatorCurrent(); + + BaseStatusSignal.setUpdateFrequencyForAll(50, voltage, velocity, currentDraw); + motor.optimizeBusUtilization(); + } + + public void setVoltage(double voltage) { + motor.setControl(voltageOut.withOutput(voltage)); + } + + public void setVelocity(double rps) { + motor.setControl(velocityVoltage.withVelocity(rps)); + } + + @Override + public void updateInputs(ShooterIOInputsAutoLogged inputs) { + BaseStatusSignal.refreshAll(currentDraw, velocity, voltage); + inputs.currentDrawAmps = currentDraw.getValueAsDouble(); + inputs.velocityRPS = velocity.getValueAsDouble(); + inputs.motorOutputVolts = voltage.getValueAsDouble(); + inputs.temperatureCelsius = 20.0; + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java new file mode 100644 index 00000000..1c6693ea --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -0,0 +1,34 @@ +package frc.robot.subsystems.shooter; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger; + +public class ShooterSubsystem extends SubsystemBase { + + ShooterIO io; + ShooterIOInputsAutoLogged inputs = new ShooterIOInputsAutoLogged(); + + public ShooterSubsystem(ShooterIO io) { + this.io = io; + } + + public Command run(double voltage) { + return new RunCommand( + () -> { + io.setVoltage(voltage); + }, + this); + } + + public Command runVelocity(double rps) { + return this.run(() -> io.setVelocity(rps)); + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Shooter", inputs); + } +} diff --git a/src/main/java/frc/robot/subsystems/swerve/GyroIO.java b/src/main/java/frc/robot/subsystems/swerve/GyroIO.java index f114108d..42198db4 100644 --- a/src/main/java/frc/robot/subsystems/swerve/GyroIO.java +++ b/src/main/java/frc/robot/subsystems/swerve/GyroIO.java @@ -26,4 +26,6 @@ public static class GyroIOInputs { } public default void updateInputs(GyroIOInputs inputs) {} + + public default void setYaw(Rotation2d yaw) {} } diff --git a/src/main/java/frc/robot/subsystems/swerve/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/swerve/GyroIOPigeon2.java index 084a61ac..1d91209e 100644 --- a/src/main/java/frc/robot/subsystems/swerve/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/swerve/GyroIOPigeon2.java @@ -24,13 +24,14 @@ /** IO implementation for Pigeon2 */ public class GyroIOPigeon2 implements GyroIO { - private final Pigeon2 pigeon = new Pigeon2(20); + private final Pigeon2 pigeon = new Pigeon2(SwerveSubsystem.PIGEON_ID); private final StatusSignal yaw = pigeon.getYaw(); private final Queue yawPositionQueue; private final StatusSignal yawVelocity = pigeon.getAngularVelocityZWorld(); public GyroIOPigeon2() { - pigeon.getConfigurator().apply(new Pigeon2Configuration()); + var config = new Pigeon2Configuration(); + pigeon.getConfigurator().apply(config); pigeon.getConfigurator().setYaw(0.0); yaw.setUpdateFrequency(Module.ODOMETRY_FREQUENCY_HZ); yawVelocity.setUpdateFrequency(100.0); @@ -50,4 +51,9 @@ public void updateInputs(GyroIOInputs inputs) { .toArray(Rotation2d[]::new); yawPositionQueue.clear(); } + + @Override + public void setYaw(Rotation2d yaw) { + pigeon.setYaw(yaw.getDegrees()); + } } diff --git a/src/main/java/frc/robot/subsystems/swerve/Module.java b/src/main/java/frc/robot/subsystems/swerve/Module.java index 98219268..cde771ca 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Module.java +++ b/src/main/java/frc/robot/subsystems/swerve/Module.java @@ -28,13 +28,14 @@ public record ModuleConstants( public static final double WHEEL_RADIUS = Units.inchesToMeters(2.0); public static final double ODOMETRY_FREQUENCY_HZ = 250.0; - // Gear ratios for SDS MK4i L2, adjust as necessary + // Gear ratios for SDS MK3 Fast, adjust as necessary // These numbers are taken from SDS's website // They are the gear tooth counts for each stage of the modules' gearboxes - public static final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); - public static final double TURN_GEAR_RATIO = 150.0 / 7.0; + public static final double DRIVE_GEAR_RATIO = + 8.16; // (48.0 / 16.0) * (16.0 / 28.0) * (60.0 / 15.0); + public static final double TURN_GEAR_RATIO = 12.8 / 1.0; - public static final double DRIVE_STATOR_CURRENT_LIMIT = 80.0; // TODO bump as needed + public static final double DRIVE_STATOR_CURRENT_LIMIT = 50.0; // TODO bump as needed public static final double TURN_STATOR_CURRENT_LIMIT = 40.0; private final ModuleIO io; diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java index 4af58c16..3c178e0c 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java @@ -17,7 +17,7 @@ import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.MotionMagicExpoVoltage; +import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.CANcoder; @@ -44,7 +44,7 @@ */ public class ModuleIOTalonFX implements ModuleIO { // Constants - private static final boolean IS_TURN_MOTOR_INVERTED = true; + private static final boolean IS_TURN_MOTOR_INVERTED = false; private final String name; @@ -71,8 +71,7 @@ public class ModuleIOTalonFX implements ModuleIO { private final VoltageOut driveVoltage = new VoltageOut(0.0).withEnableFOC(true); private final VoltageOut turnVoltage = new VoltageOut(0.0).withEnableFOC(true); private final VelocityVoltage drivePIDF = new VelocityVoltage(0.0).withEnableFOC(true); - private final MotionMagicExpoVoltage turnMotionMagic = - new MotionMagicExpoVoltage(0.0).withEnableFOC(true); + private final PositionVoltage turnPID = new PositionVoltage(0.0).withEnableFOC(true); public ModuleIOTalonFX(ModuleConstants constants) { name = constants.prefix(); @@ -87,19 +86,19 @@ public ModuleIOTalonFX(ModuleConstants constants) { driveConfig.CurrentLimits.StatorCurrentLimit = Module.DRIVE_STATOR_CURRENT_LIMIT; driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; // Inverts - driveConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + driveConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; driveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; // Sensor // Meters per second driveConfig.Feedback.SensorToMechanismRatio = - (1.0 / Module.DRIVE_GEAR_RATIO) * Module.WHEEL_RADIUS * 2 * Math.PI; + (Module.DRIVE_GEAR_RATIO) * (1.0 / (Module.WHEEL_RADIUS * 2 * Math.PI)); // Controls Gains - driveConfig.Slot0.kV = - (5800.0 * (1.0 / Module.DRIVE_GEAR_RATIO) * Module.WHEEL_RADIUS * 2 * Math.PI) - / 12.0; // Hypothetical based on free speed + driveConfig.Slot0.kV = 2.5; + // (5800.0 * (Module.DRIVE_GEAR_RATIO) * (1.0 / (Module.WHEEL_RADIUS * 2 * Math.PI))) + // / 12.0; // Hypothetical based on free speed driveConfig.Slot0.kA = 0.0; // TODO: Find using sysid or hand tuning driveConfig.Slot0.kS = 0.0; - driveConfig.Slot0.kP = 0.0; + driveConfig.Slot0.kP = 0.25; // Guess driveConfig.Slot0.kD = 0.0; driveTalon.getConfigurator().apply(driveConfig); @@ -122,18 +121,18 @@ public ModuleIOTalonFX(ModuleConstants constants) { turnConfig.Feedback.FeedbackRotorOffset = 0.0; // Is this correct? Cancoder config should handle it // Controls Gains - turnConfig.Slot0.kV = - (5800.0 / Module.TURN_GEAR_RATIO) - / 12.0; // Free speed over voltage, should find empirically - turnConfig.Slot0.kA = - Module.TURN_GEAR_RATIO - * (9.37 / 483.0) - / (0.004 * (12.0 / 483.0)); // Based on motor dynamics math, should find in real life + turnConfig.Slot0.kV = 0.0; + // (5800.0 / Module.TURN_GEAR_RATIO) + // / 12.0; // Free speed over voltage, should find empirically + turnConfig.Slot0.kA = 0.0; + // Module.TURN_GEAR_RATIO + // * (9.37 / 483.0) + // / (0.004 * (12.0 / 483.0)); // Based on motor dynamics math, should find in real life // gearing * Kt (torque per amp) / (intertia * resistance (nominal voltage / stall current)) // Yeah its messy and should be found using sysid later but its worth trying as a first guess // If this works we can use a similar technique on future mechanisms turnConfig.Slot0.kS = 0.0; // TODO: Find empirically - turnConfig.Slot0.kP = 0.0; + turnConfig.Slot0.kP = 50.0; turnConfig.Slot0.kD = 0.0; turnConfig.ClosedLoopGeneral.ContinuousWrap = true; @@ -187,8 +186,8 @@ public void updateInputs(ModuleIOInputs inputs) { turnAppliedVolts, turnCurrent); - inputs.drivePositionMeters = Units.rotationsToRadians(drivePosition.getValueAsDouble()); - inputs.driveVelocityMetersPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()); + inputs.drivePositionMeters = drivePosition.getValueAsDouble(); + inputs.driveVelocityMetersPerSec = driveVelocity.getValueAsDouble(); inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); inputs.driveCurrentAmps = new double[] {driveCurrent.getValueAsDouble()}; @@ -225,7 +224,7 @@ public void setDriveSetpoint(final double metersPerSecond) { @Override public void setTurnSetpoint(final Rotation2d rotation) { - turnTalon.setControl(turnMotionMagic.withPosition(rotation.getRotations())); + turnTalon.setControl(turnPID.withPosition(rotation.getRotations())); } @Override diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index cdfce767..fd767bff 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -22,9 +22,9 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; @@ -42,22 +42,22 @@ public class SwerveSubsystem extends SubsystemBase { // Drivebase constants public static final double MAX_LINEAR_SPEED = Units.feetToMeters(14.5); - public static final double TRACK_WIDTH_X = Units.inchesToMeters(25.0); - public static final double TRACK_WIDTH_Y = Units.inchesToMeters(25.0); + public static final double TRACK_WIDTH_X = Units.inchesToMeters(20.5); + public static final double TRACK_WIDTH_Y = Units.inchesToMeters(20.5); public static final double DRIVE_BASE_RADIUS = Math.hypot(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0); public static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS; // Hardware constants - public static final int PigeonID = 0; + public static final int PIGEON_ID = 0; public static final ModuleConstants frontLeft = - new ModuleConstants("Front Left", 0, 1, 0, Rotation2d.fromRotations(0.0)); + new ModuleConstants("Front Left", 6, 5, 21, Rotation2d.fromRotations(0.125732)); public static final ModuleConstants frontRight = - new ModuleConstants("Front Right", 2, 3, 1, Rotation2d.fromRotations(0.0)); + new ModuleConstants("Front Right", 8, 7, 23, Rotation2d.fromRotations(0.461426)); public static final ModuleConstants backLeft = - new ModuleConstants("Back Left", 4, 5, 2, Rotation2d.fromRotations(0.0)); + new ModuleConstants("Back Left", 4, 3, 20, Rotation2d.fromRotations(0.152344)); public static final ModuleConstants backRight = - new ModuleConstants("Back Right", 6, 7, 3, Rotation2d.fromRotations(0.0)); + new ModuleConstants("Back Right", 2, 1, 22, Rotation2d.fromRotations(-0.238281)); public static final Lock odometryLock = new ReentrantLock(); private final GyroIO gyroIO; @@ -67,6 +67,7 @@ public class SwerveSubsystem extends SubsystemBase { private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); private Pose2d pose = new Pose2d(); private Rotation2d lastGyroRotation = new Rotation2d(); + private SwerveDriveOdometry odometry; public SwerveSubsystem(GyroIO gyroIO, ModuleIO... moduleIOs) { this.gyroIO = gyroIO; @@ -86,16 +87,30 @@ public SwerveSubsystem(GyroIO gyroIO, ModuleIO... moduleIOs) { // your Constants class new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants new PIDConstants(5.0, 0.0, 0.0), // Rotation PID constants - 4.5, // Max module speed, in m/s - 0.4, // Drive base radius in meters. Distance from robot center to furthest module. - new ReplanningConfig() // Default path replanning config. See the API for the options + MAX_LINEAR_SPEED, // Max module speed, in m/s + DRIVE_BASE_RADIUS, // Drive base radius in meters. Distance from robot center to + // furthest module. + new ReplanningConfig( + false, false) // Default path replanning config. See the API for the options // here ), - () -> true, + () -> false, this // Reference to this subsystem to set requirements ); + PathPlannerLogging.setLogTargetPoseCallback( - (pose) -> Logger.recordOutput("PathPlanner/Target", pose)); + (pose) -> { + Logger.recordOutput("PathPlanner/Target", pose); + Logger.recordOutput( + "PathPlanner/Absolute Translation Error", + pose.minus(getPose()).getTranslation().getNorm()); + }); + PathPlannerLogging.setLogActivePathCallback( + (path) -> Logger.recordOutput("PathPlanner/Active Path", path.toArray(Pose2d[]::new))); + Logger.recordOutput("PathPlanner/Target", new Pose2d()); + Logger.recordOutput("PathPlanner/Absolute Translation Error", 0.0); + + odometry = new SwerveDriveOdometry(kinematics, getRotation(), getModulePositions()); } /** @@ -151,42 +166,47 @@ public void periodic() { } // Update odometry - int deltaCount = - Math.min( - gyroInputs.connected ? gyroInputs.odometryYawPositions.length : Integer.MAX_VALUE, - Arrays.stream(modules) - .map((m) -> m.getPositionDeltas().length) - .min(Integer::compare) - .get()); - for (int deltaIndex = 0; deltaIndex < deltaCount; deltaIndex++) { - // Read wheel deltas from each module - SwerveModulePosition[] wheelDeltas = new SwerveModulePosition[4]; - for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) { - wheelDeltas[moduleIndex] = modules[moduleIndex].getPositionDeltas()[deltaIndex]; - } - - // The twist represents the motion of the robot since the last - // sample in x, y, and theta based only on the modules, without - // the gyro. The gyro is always disconnected in simulation. - var twist = kinematics.toTwist2d(wheelDeltas); - if (gyroInputs.connected) { - // If the gyro is connected, replace the theta component of the twist - // with the change in angle since the last sample. - Rotation2d gyroRotation = gyroInputs.odometryYawPositions[deltaIndex]; - twist = new Twist2d(twist.dx, twist.dy, gyroRotation.minus(lastGyroRotation).getRadians()); - lastGyroRotation = gyroRotation; - } - // Apply the twist (change since last sample) to the current pose - pose = pose.exp(twist); - } + // int deltaCount = + // Math.min( + // gyroInputs.connected ? gyroInputs.odometryYawPositions.length : Integer.MAX_VALUE, + // Arrays.stream(modules) + // .map((m) -> m.getPositionDeltas().length) + // .min(Integer::compare) + // .get()); + // for (int deltaIndex = 0; deltaIndex < deltaCount; deltaIndex++) { + // // Read wheel deltas from each module + // SwerveModulePosition[] wheelDeltas = new SwerveModulePosition[4]; + // for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) { + // wheelDeltas[moduleIndex] = modules[moduleIndex].getPositionDeltas()[deltaIndex]; + // } + + // // The twist represents the motion of the robot since the last + // // sample in x, y, and theta based only on the modules, without + // // the gyro. The gyro is always disconnected in simulation. + // var twist = kinematics.toTwist2d(wheelDeltas); + // if (gyroInputs.connected) { + // // If the gyro is connected, replace the theta component of the twist + // // with the change in angle since the last sample. + // Rotation2d gyroRotation = gyroInputs.odometryYawPositions[deltaIndex]; + // twist = new Twist2d(twist.dx, twist.dy, + // gyroRotation.minus(lastGyroRotation).getRadians()); + // lastGyroRotation = gyroRotation; + // } + // Apply the twist (change since last sample) to the current pose + // pose = pose.exp(twist); + // } + pose = odometry.update(gyroInputs.yawPosition, getModulePositions()); } - private void runVelocity(ChassisSpeeds speeds) { + public void runVelocity(ChassisSpeeds speeds) { // Calculate module setpoints ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02); SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, MAX_LINEAR_SPEED); + Logger.recordOutput("Swerve/Target Speeds", discreteSpeeds); + Logger.recordOutput("Swerve/Speed Error", discreteSpeeds.minus(getVelocity())); + // Send setpoints to modules SwerveModuleState[] optimizedSetpointStates = Streams.zip( @@ -247,6 +267,16 @@ public double getCharacterizationVelocity() { return driveVelocityAverage / 4.0; } + private SwerveModulePosition[] getModulePositions() { + + SwerveModulePosition[] positions = new SwerveModulePosition[4]; + for (int i = 0; i < 4; i++) { + positions[i] = modules[i].getPosition(); + } + + return positions; + } + /** Returns the module states (turn angles and drive velocitoes) for all of the modules. */ @AutoLogOutput(key = "SwerveStates/Measured") private SwerveModuleState[] getModuleStates() { @@ -288,6 +318,12 @@ public Rotation2d getRotation() { /** Resets the current odometry pose. */ public void setPose(Pose2d pose) { this.pose = pose; + odometry.resetPosition(gyroInputs.yawPosition, getModulePositions(), pose); + } + + public void setYaw(Rotation2d yaw) { + gyroIO.setYaw(yaw); + setPose(new Pose2d(getPose().getTranslation(), yaw)); } /** Returns an array of module translations. */