From a46206029de6e4ed6f57e82e57e00e6c7c779c1b Mon Sep 17 00:00:00 2001 From: SirBeans Date: Mon, 15 Jul 2024 18:20:02 -0400 Subject: [PATCH] Fix more nitpicks --- .../auto/choreoStuff/Choreo1stgen.kt | 20 + .../auto/choreoStuff/Trajectory.json | 731 ++++++++++++++++++ 2 files changed, 751 insertions(+) create mode 100644 src/main/kotlin/com/team4099/robot2023/auto/choreoStuff/Choreo1stgen.kt create mode 100644 src/main/kotlin/com/team4099/robot2023/auto/choreoStuff/Trajectory.json diff --git a/src/main/kotlin/com/team4099/robot2023/auto/choreoStuff/Choreo1stgen.kt b/src/main/kotlin/com/team4099/robot2023/auto/choreoStuff/Choreo1stgen.kt new file mode 100644 index 00000000..19199ce7 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/auto/choreoStuff/Choreo1stgen.kt @@ -0,0 +1,20 @@ +package com.team4099.robot2023.auto.choreoStuff + +import com.choreo.lib.Choreo; +import com.choreo.lib.ChoreoTrajectory; +import edu.wpi.first.math.proto.Trajectory +import org.team4099.lib.geometry.Pose2d +import java.util.function.Supplier + +class Choreo1stgen { + var pose: Pose2d = Pose2d(); + + var trajectory: ChoreoTrajectory = Choreo.getTrajectory("Trajectory"); + + init { + trajectory.sample(1. + ). + } + + +} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/auto/choreoStuff/Trajectory.json b/src/main/kotlin/com/team4099/robot2023/auto/choreoStuff/Trajectory.json new file mode 100644 index 00000000..8dfc3959 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/auto/choreoStuff/Trajectory.json @@ -0,0 +1,731 @@ +[ + { + "timestamp": 0, + "x": 5.982723236083984, + "y": 3.8337526321411137, + "heading": -0.02726589531195272, + "velocityX": 0, + "velocityY": 9.683808101189284e-35, + "angularVelocity": 0 + }, + { + "timestamp": 0.03810324695055001, + "x": 5.98684701120518, + "y": 3.836695144747058, + "heading": -0.02635908015583591, + "velocityX": 0.10822634424167109, + "velocityY": 0.07722472076364896, + "angularVelocity": 0.023798894548105808 + }, + { + "timestamp": 0.07620649390110001, + "x": 5.9951102276307955, + "y": 3.842555990890375, + "heading": -0.02450943734161065, + "velocityX": 0.21686383935573297, + "velocityY": 0.15381487438388347, + "angularVelocity": 0.048542918576604074 + }, + { + "timestamp": 0.11430974085165002, + "x": 6.007530540708913, + "y": 3.851307449548468, + "heading": -0.021675911820365212, + "velocityX": 0.3259646899445822, + "velocityY": 0.2296775041100852, + "angularVelocity": 0.07436441112018764 + }, + { + "timestamp": 0.15241298780220003, + "x": 6.024127990876516, + "y": 3.8629174242827835, + "heading": -0.017811264169326626, + "velocityX": 0.43559149143231607, + "velocityY": 0.3046977794145049, + "angularVelocity": 0.10142567787079011 + }, + { + "timestamp": 0.19051623475275004, + "x": 6.044925508435204, + "y": 3.8773483282979404, + "heading": -0.0128605531052204, + "velocityX": 0.5458200868203993, + "velocityY": 0.37873160872313927, + "angularVelocity": 0.12992885017200875 + }, + { + "timestamp": 0.22861948170330004, + "x": 6.0699495650843565, + "y": 3.894555559389049, + "heading": -0.006759072154586934, + "velocityX": 0.6567434182611646, + "velocityY": 0.45159487624347117, + "angularVelocity": 0.16013021038733294 + }, + { + "timestamp": 0.26672272865385005, + "x": 6.0992310251847055, + "y": 3.91448535931083, + "heading": 0.0005705329934009674, + "velocityX": 0.7684767688787957, + "velocityY": 0.5230472864332357, + "angularVelocity": 0.19236169446395399 + }, + { + "timestamp": 0.30482597560440006, + "x": 6.132806269868785, + "y": 3.9370717165814155, + "heading": 0.009222426006961602, + "velocityX": 0.8811649234946475, + "velocityY": 0.5927672594384606, + "angularVelocity": 0.2270644552887354 + }, + { + "timestamp": 0.34292922255495006, + "x": 6.170718689966367, + "y": 3.962231721071589, + "heading": 0.01931383560441445, + "velocityX": 0.9949918479857032, + "velocityY": 0.6603112990035996, + "angularVelocity": 0.2648438231667591 + }, + { + "timestamp": 0.38103246950550007, + "x": 6.213020653091169, + "y": 3.989858289069104, + "heading": 0.030994951202651673, + "velocityX": 1.1101931334015223, + "velocityY": 0.7250449819504136, + "angularVelocity": 0.3065648345768179 + }, + { + "timestamp": 0.4191357164560501, + "x": 6.259775973212591, + "y": 4.019808160211835, + "heading": 0.04446563724227389, + "velocityX": 1.2270691834242344, + "velocityY": 0.7860188708221707, + "angularVelocity": 0.3535311848122014 + }, + { + "timestamp": 0.4572389634066001, + "x": 6.311062435579481, + "y": 4.051880814371457, + "heading": 0.060006160684953305, + "velocityX": 1.3459866670536105, + "velocityY": 0.8417302126835429, + "angularVelocity": 0.40785299643479905 + }, + { + "timestamp": 0.4953422103571501, + "x": 6.366971668412108, + "y": 4.085778739706103, + "heading": 0.0780398678585479, + "velocityX": 1.4673088859116663, + "velocityY": 0.8896335102002109, + "angularVelocity": 0.47328531337489643 + }, + { + "timestamp": 0.5334454573077001, + "x": 6.42759324409593, + "y": 4.1210284288786045, + "heading": 0.09927318993914273, + "velocityX": 1.5909818856775562, + "velocityY": 0.9251098526654346, + "angularVelocity": 0.5572575509942492 + }, + { + "timestamp": 0.5715487042582501, + "x": 6.492929850638083, + "y": 4.156837507973627, + "heading": 0.12496268004834134, + "velocityX": 1.7147254307998017, + "velocityY": 0.9397907517301461, + "angularVelocity": 0.6742073750968268 + }, + { + "timestamp": 0.6096519512088001, + "x": 6.562698684949471, + "y": 4.191947985905663, + "heading": 0.1565497680971661, + "velocityX": 1.8310469551829356, + "velocityY": 0.9214563256930572, + "angularVelocity": 0.8289867813578368 + }, + { + "timestamp": 0.6477551981593501, + "x": 6.635139463079909, + "y": 4.223639351259096, + "heading": 0.19449877991768647, + "velocityX": 1.9011707381381133, + "velocityY": 0.8317234852599791, + "angularVelocity": 0.9959521788203487 + }, + { + "timestamp": 0.6858584451099001, + "x": 6.7081933797465005, + "y": 4.250935758839449, + "heading": 0.23877166971400077, + "velocityX": 1.917262241755836, + "velocityY": 0.7163800926409031, + "angularVelocity": 1.1619190840553386 + }, + { + "timestamp": 0.7239616920604501, + "x": 6.780136139474193, + "y": 4.273495418390116, + "heading": 0.28647328812510897, + "velocityX": 1.888100502853595, + "velocityY": 0.5920665915937995, + "angularVelocity": 1.2519042923826915 + }, + { + "timestamp": 0.7620649390110001, + "x": 6.850045632550813, + "y": 4.291512251071874, + "heading": 0.3365456853758495, + "velocityX": 1.8347384716936121, + "velocityY": 0.47284245106833517, + "angularVelocity": 1.3141241562884853 + }, + { + "timestamp": 0.8001681859615501, + "x": 6.9174385893155375, + "y": 4.305183550968592, + "heading": 0.3884190669725829, + "velocityX": 1.7686932783493219, + "velocityY": 0.35879619168575444, + "angularVelocity": 1.3613900585442669 + }, + { + "timestamp": 0.8382714329121002, + "x": 6.982024412284714, + "y": 4.314663617385671, + "heading": 0.44170912410253604, + "velocityX": 1.6950215044138437, + "velocityY": 0.24879943773254676, + "angularVelocity": 1.398569974866313 + }, + { + "timestamp": 0.8763746798626502, + "x": 7.043611527543253, + "y": 4.320071315528175, + "heading": 0.49614021122658447, + "velocityX": 1.616321972205263, + "velocityY": 0.14192223958037933, + "angularVelocity": 1.4285157166445919 + }, + { + "timestamp": 0.9144779268132002, + "x": 7.102065214635615, + "y": 4.32149953061112, + "heading": 0.5515071646371278, + "velocityX": 1.5340867713510038, + "velocityY": 0.037482765833582746, + "angularVelocity": 1.4530770430772049 + }, + { + "timestamp": 0.9525811737637502, + "x": 7.157286057818237, + "y": 4.319022257662232, + "heading": 0.6076525295540515, + "velocityX": 1.4492424557488948, + "velocityY": -0.06501474669864526, + "angularVelocity": 1.4735060502794106 + }, + { + "timestamp": 0.9906844207143002, + "x": 7.209197910760983, + "y": 4.312699565476428, + "heading": 0.664452255224938, + "velocityX": 1.362399719113554, + "velocityY": -0.1659357847904016, + "angularVelocity": 1.4906794096734857 + }, + { + "timestamp": 1.0287876676648502, + "x": 7.2577407022992615, + "y": 4.302581057423054, + "heading": 0.7218063948940598, + "velocityX": 1.2739804458469486, + "velocityY": -0.2655550081205459, + "angularVelocity": 1.5052297181801246 + }, + { + "timestamp": 1.0668909146154002, + "x": 7.3028658963835795, + "y": 4.288708316667104, + "heading": 0.779632866785423, + "velocityX": 1.184287369075913, + "velocityY": -0.3640829028022947, + "angularVelocity": 1.51762583294836 + }, + { + "timestamp": 1.1049941615659502, + "x": 7.344533497015384, + "y": 4.271116666027759, + "heading": 0.8378631451292572, + "velocityX": 1.0935446180184787, + "velocityY": -0.4616837683721217, + "angularVelocity": 1.5282235243470785 + }, + { + "timestamp": 1.1430974085165002, + "x": 7.3827099980564315, + "y": 4.249836459267662, + "heading": 0.8964392046934532, + "velocityX": 1.0019225157003373, + "velocityY": -0.5584880151477051, + "angularVelocity": 1.537298373553775 + }, + { + "timestamp": 1.1812006554670502, + "x": 7.417366935383198, + "y": 4.22489404631923, + "heading": 0.9553113025588683, + "velocityX": 0.9095533871888511, + "velocityY": -0.6546007215816033, + "angularVelocity": 1.5450677455866486 + }, + { + "timestamp": 1.2193039024176002, + "x": 7.448479836830729, + "y": 4.1963125073290035, + "heading": 1.014436333735295, + "velocityX": 0.816541999371032, + "velocityY": -0.7501077015120999, + "angularVelocity": 1.551705849455849 + }, + { + "timestamp": 1.2574071493681502, + "x": 7.4760274429392135, + "y": 4.164112219770215, + "heading": 1.0737765893932227, + "velocityX": 0.7229726680310206, + "velocityY": -0.8450798852018467, + "angularVelocity": 1.5573543046066753 + }, + { + "timestamp": 1.2955103963187002, + "x": 7.49999111698165, + "y": 4.12831130292246, + "heading": 1.1332988035654967, + "velocityX": 0.628914225434282, + "velocityY": -0.9395765377742389, + "angularVelocity": 1.5621297116627306 + }, + { + "timestamp": 1.3336136432692502, + "x": 7.520354390382808, + "y": 4.08892597080947, + "heading": 1.1929734104345324, + "velocityX": 0.5344235736021341, + "velocityY": -1.0336476616835946, + "angularVelocity": 1.566129179134875 + }, + { + "timestamp": 1.3717168902198003, + "x": 7.537102606975847, + "y": 4.045970815789375, + "heading": 1.252773957923772, + "velocityX": 0.43954827825499565, + "velocityY": -1.1273358167045713, + "angularVelocity": 1.5694344255451789 + }, + { + "timestamp": 1.4098201371703503, + "x": 7.550222640729637, + "y": 3.999459038895289, + "heading": 1.312676639030204, + "velocityX": 0.3443284970127743, + "velocityY": -1.2206775174422342, + "angularVelocity": 1.5721148694801685 + }, + { + "timestamp": 1.4479233841209003, + "x": 7.5597026689810605, + "y": 3.9494026387764034, + "heading": 1.3726599130276154, + "velocityX": 0.24879843609460844, + "velocityY": -1.313704319840455, + "angularVelocity": 1.574229988201789 + }, + { + "timestamp": 1.4860266310714503, + "x": 7.56553198821413, + "y": 3.8958125680834574, + "heading": 1.4327041960899756, + "velocityX": 0.15298746693777887, + "velocityY": -1.4064436755874, + "angularVelocity": 1.5758311395427256 + }, + { + "timestamp": 1.5241298780220003, + "x": 7.5677008628845215, + "y": 3.8386988639831543, + "heading": 1.492791606127603, + "velocityX": 0.05692099345774061, + "velocityY": -1.4989196110878606, + "angularVelocity": 1.576962984692913 + }, + { + "timestamp": 1.564961546846248, + "x": 7.573888102497922, + "y": 3.7818679617757693, + "heading": 1.5572134986128423, + "velocityX": 0.15153041233834258, + "velocityY": -1.3918339329211034, + "angularVelocity": 1.5777433139589185 + }, + { + "timestamp": 1.605793215670496, + "x": 7.583928738356132, + "y": 3.7294180830284507, + "heading": 1.6216492084044356, + "velocityX": 0.24590314692813534, + "velocityY": -1.2845391887625002, + "angularVelocity": 1.5780817107659115 + }, + { + "timestamp": 1.6466248844947438, + "x": 7.597812163368718, + "y": 3.681358537835685, + "heading": 1.6860789847473923, + "velocityX": 0.34001610544853655, + "velocityY": -1.177016432995396, + "angularVelocity": 1.5779363958961614 + }, + { + "timestamp": 1.6874565533189916, + "x": 7.615526682956808, + "y": 3.637699519534437, + "heading": 1.7504811177254895, + "velocityX": 0.4338426544440016, + "velocityY": -1.069244034309975, + "angularVelocity": 1.5772593879350625 + }, + { + "timestamp": 1.7282882221432394, + "x": 7.637059339376509, + "y": 3.5984522360053632, + "heading": 1.8148316323025202, + "velocityX": 0.5273518579998424, + "velocityY": -0.9611971457254432, + "angularVelocity": 1.5759952122950966 + }, + { + "timestamp": 1.7691198909674872, + "x": 7.662395696195494, + "y": 3.5636290681903535, + "heading": 1.8791039149175528, + "velocityX": 0.6205075018618541, + "velocityY": -0.8528470380404654, + "angularVelocity": 1.574079249410144 + }, + { + "timestamp": 1.809951559791735, + "x": 7.691519570992391, + "y": 3.5332437632048217, + "heading": 1.9432682526933631, + "velocityX": 0.7132668253716068, + "velocityY": -0.7441602525804, + "angularVelocity": 1.5714355945625587 + }, + { + "timestamp": 1.8507832286159829, + "x": 7.72441269980617, + "y": 3.507311671935531, + "heading": 2.007291257898031, + "velocityX": 0.805578849969649, + "velocityY": -0.6350975117110675, + "angularVelocity": 1.567974247642089 + }, + { + "timestamp": 1.8916148974402307, + "x": 7.761054310174422, + "y": 3.4858500445720093, + "heading": 2.0711351395192956, + "velocityX": 0.8973821404647174, + "velocityY": -0.5256122999992441, + "angularVelocity": 1.563587368815906 + }, + { + "timestamp": 1.9324465662644785, + "x": 7.8014205695289, + "y": 3.4688784026403385, + "heading": 2.1347567677887986, + "velocityX": 0.9886017524345373, + "velocityY": -0.4156489906087677, + "angularVelocity": 1.5581442077068588 + }, + { + "timestamp": 1.9732782350887264, + "x": 7.845483860168917, + "y": 3.4564190136113133, + "heading": 2.1981064530629975, + "velocityX": 1.0791449849791273, + "velocityY": -0.30514033317260225, + "angularVelocity": 1.5514841077615236 + }, + { + "timestamp": 2.014109903912974, + "x": 7.893211807339716, + "y": 3.448497505357414, + "heading": 2.261126322126411, + "velocityX": 1.168895334066148, + "velocityY": -0.19400402878448852, + "angularVelocity": 1.5434066468033365 + }, + { + "timestamp": 2.054941572737222, + "x": 7.944565946441019, + "y": 3.445143674787076, + "heading": 2.3237481128062893, + "velocityX": 1.2577036545419125, + "velocityY": -0.08213797444264499, + "angularVelocity": 1.533657391017405 + }, + { + "timestamp": 2.09577324156147, + "x": 7.999499846458567, + "y": 3.4463925714516828, + "heading": 2.3858901030926365, + "velocityX": 1.3453748426007262, + "velocityY": 0.03058647125063885, + "angularVelocity": 1.5219066982990461 + }, + { + "timestamp": 2.1366049103857176, + "x": 8.057956384260022, + "y": 3.452285978695619, + "heading": 2.447452706752575, + "velocityX": 1.4316470397785264, + "velocityY": 0.1443342242342635, + "angularVelocity": 1.5077170596413056 + }, + { + "timestamp": 2.1774365792099655, + "x": 8.11986363569219, + "y": 3.462874481468067, + "heading": 2.508311925925005, + "velocityX": 1.5161577573190377, + "velocityY": 0.2593208428003823, + "angularVelocity": 1.4904906148801191 + }, + { + "timestamp": 2.2182682480342133, + "x": 8.185128396146618, + "y": 3.4782204145569087, + "heading": 2.5683091763759016, + "velocityX": 1.598385819970941, + "velocityY": 0.3758340898310503, + "angularVelocity": 1.469380316272348 + }, + { + "timestamp": 2.259099916858461, + "x": 8.253625378823767, + "y": 3.498402135996597, + "heading": 2.627234557026799, + "velocityX": 1.677545509393189, + "velocityY": 0.4942663873612369, + "angularVelocity": 1.4431293735393322 + }, + { + "timestamp": 2.299931585682709, + "x": 8.325177903992097, + "y": 3.5235202051846817, + "heading": 2.6847972126928403, + "velocityX": 1.7523781718624016, + "velocityY": 0.6151614644065581, + "angularVelocity": 1.4097551563178585 + }, + { + "timestamp": 2.3407632545069568, + "x": 8.399520165830122, + "y": 3.5537056177972803, + "heading": 2.7405671841117947, + "velocityX": 1.820701038647485, + "velocityY": 0.7392647296031375, + "angularVelocity": 1.365850895269809 + }, + { + "timestamp": 2.3815949233312046, + "x": 8.476214939455824, + "y": 3.5891254614190196, + "heading": 2.7938439408795164, + "velocityX": 1.878315920806788, + "velocityY": 0.8674601024562811, + "angularVelocity": 1.3047900882289871 + }, + { + "timestamp": 2.4224265921554524, + "x": 8.55445641673558, + "y": 3.629944911690221, + "heading": 2.843312258894989, + "velocityX": 1.9161959217616398, + "velocityY": 0.9997007579314489, + "angularVelocity": 1.2115183983394033 + }, + { + "timestamp": 2.4632582609797002, + "x": 8.63270684819373, + "y": 3.6760250979155664, + "heading": 2.8865347413407387, + "velocityX": 1.91641521670224, + "velocityY": 1.1285403597803692, + "angularVelocity": 1.058552924491086 + }, + { + "timestamp": 2.504089929803948, + "x": 8.70731172182322, + "y": 3.7253996813040025, + "heading": 2.922276108950885, + "velocityX": 1.8271326099998109, + "velocityY": 1.2092227628744916, + "angularVelocity": 0.8753344803012834 + }, + { + "timestamp": 2.544921598628196, + "x": 8.777061536831155, + "y": 3.7753594853432584, + "heading": 2.949709380191114, + "velocityX": 1.7082283682345663, + "velocityY": 1.2235552814236639, + "angularVelocity": 0.6718626015088779 + }, + { + "timestamp": 2.5857532674524437, + "x": 8.841475923005714, + "y": 3.823979349131039, + "heading": 2.9725228149804273, + "velocityX": 1.5775594784484608, + "velocityY": 1.1907390804194327, + "angularVelocity": 0.5587191375275982 + }, + { + "timestamp": 2.6265849362766915, + "x": 8.900725127375773, + "y": 3.870275138965792, + "heading": 2.9920719682931938, + "velocityX": 1.451060073618111, + "velocityY": 1.1338206634173846, + "angularVelocity": 0.4787742915161657 + }, + { + "timestamp": 2.6674166051009394, + "x": 8.954991259763695, + "y": 3.913751195609642, + "heading": 3.009010373474175, + "velocityX": 1.3290206829777829, + "velocityY": 1.0647631580031653, + "angularVelocity": 0.41483499618584174 + }, + { + "timestamp": 2.708248273925187, + "x": 9.004413690363393, + "y": 3.9541157023660385, + "heading": 3.023743409149255, + "velocityX": 1.210394579081024, + "velocityY": 0.9885588299154339, + "angularVelocity": 0.360823745374801 + }, + { + "timestamp": 2.749079942749435, + "x": 9.049097789798745, + "y": 3.9911778957153943, + "heading": 3.0365503891819805, + "velocityX": 1.09434908545349, + "velocityY": 0.9076825517193372, + "angularVelocity": 0.31365311292665315 + }, + { + "timestamp": 2.789911611573683, + "x": 9.089124874577477, + "y": 4.02480391238427, + "heading": 3.0476356317929376, + "velocityX": 0.9802950976856478, + "velocityY": 0.8235278556360364, + "angularVelocity": 0.2714863959801188 + }, + { + "timestamp": 2.8307432803979307, + "x": 9.124559199537389, + "y": 4.054894899911004, + "heading": 3.0571549364034576, + "velocityX": 0.8678147619298414, + "velocityY": 0.7369521842531842, + "angularVelocity": 0.2331353306054306 + }, + { + "timestamp": 2.8715749492221785, + "x": 9.15545264765326, + "y": 4.08137499834509, + "heading": 3.0652309054795936, + "velocityX": 0.7566050814344037, + "velocityY": 0.6485186424307532, + "angularVelocity": 0.19778689700121527 + }, + { + "timestamp": 2.9124066180464263, + "x": 9.181847907790585, + "y": 4.1041842207712955, + "heading": 3.0719624536763006, + "velocityX": 0.6464408851605111, + "velocityY": 0.5586159734097851, + "angularVelocity": 0.1648609618596868 + }, + { + "timestamp": 2.953238286870674, + "x": 9.203780676978399, + "y": 4.123273979904036, + "heading": 3.077431020092421, + "velocityX": 0.537150937479908, + "velocityY": 0.4675233631744787, + "angularVelocity": 0.13392953493168003 + }, + { + "timestamp": 2.994069955694922, + "x": 9.221281224251845, + "y": 4.138604141776491, + "heading": 3.081704794247388, + "velocityX": 0.4286023025112106, + "velocityY": 0.3754478402153836, + "angularVelocity": 0.10466812349403944 + }, + { + "timestamp": 3.03490162451917, + "x": 9.23437552715582, + "y": 4.150141009015649, + "heading": 3.084841687848118, + "velocityX": 0.32068987824956363, + "velocityY": 0.2825470418271738, + "angularVelocity": 0.07682501575503786 + }, + { + "timestamp": 3.0757332933434176, + "x": 9.243086115335979, + "y": 4.157855895393255, + "heading": 3.0868914836292065, + "velocityX": 0.2133292229042485, + "velocityY": 0.18894369492496685, + "angularVelocity": 0.05020112672616272 + }, + { + "timestamp": 3.1165649621676654, + "x": 9.247432708740234, + "y": 4.161724090576172, + "heading": 3.087897426551899, + "velocityX": 0.10645152474578339, + "velocityY": 0.09473517233807482, + "angularVelocity": 0.024636341145439937 + }, + { + "timestamp": 3.1573966309919133, + "x": 9.247432708740234, + "y": 4.161724090576172, + "heading": 3.087897426551899, + "velocityX": 7.74756381521956e-33, + "velocityY": 1.0517742336989922e-32, + "angularVelocity": 0 + } +] \ No newline at end of file