diff --git a/src/main/deploy/choreo/testtest.1.traj b/src/main/deploy/choreo/testtest.1.traj new file mode 100644 index 0000000..1a44408 --- /dev/null +++ b/src/main/deploy/choreo/testtest.1.traj @@ -0,0 +1,283 @@ +{ + "samples": [ + { + "x": 2.5015885829925537, + "y": 6.46366548538208, + "heading": 0, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 2.5156072081078307, + "y": 6.463710143904023, + "heading": 0.07027591296878091, + "angularVelocity": 1.3867283507699126, + "velocityX": 0.27662429508110253, + "velocityY": 0.0008812299387578504, + "timestamp": 0.05067749060568617 + }, + { + "x": 2.544396066654515, + "y": 6.464564468654549, + "heading": 0.20620072889082977, + "angularVelocity": 2.6821536405513675, + "velocityX": 0.5680797964265044, + "velocityY": 0.016858071311649372, + "timestamp": 0.10135498121137233 + }, + { + "x": 2.5897390778177214, + "y": 6.46666732372773, + "heading": 0.39551260118183784, + "angularVelocity": 3.7356204900517853, + "velocityX": 0.8947367089663785, + "velocityY": 0.041494853988394205, + "timestamp": 0.1520324718170585 + }, + { + "x": 2.654398590715119, + "y": 6.46738940563756, + "heading": 0.6167944034832189, + "angularVelocity": 4.3664711819127175, + "velocityX": 1.2759020252305684, + "velocityY": 0.014248572713453313, + "timestamp": 0.20270996242274467 + }, + { + "x": 2.739467602918066, + "y": 6.465281363276188, + "heading": 0.858858871076778, + "angularVelocity": 4.776567756225854, + "velocityX": 1.6786350544634498, + "velocityY": -0.041597212809432765, + "timestamp": 0.25338745302843085 + }, + { + "x": 2.844505231165706, + "y": 6.458645601789904, + "heading": 1.0779317888091438, + "angularVelocity": 4.322884087472656, + "velocityX": 2.0726682989282534, + "velocityY": -0.13094100372719633, + "timestamp": 0.304064943634117 + }, + { + "x": 2.9683658258358037, + "y": 6.4481131339425675, + "heading": 1.2606572080473037, + "angularVelocity": 3.605652471230642, + "velocityX": 2.44409486716376, + "velocityY": -0.207833255385285, + "timestamp": 0.3547424342398032 + }, + { + "x": 3.1110478462155178, + "y": 6.435729773739042, + "heading": 1.4042804378608007, + "angularVelocity": 2.834063567413143, + "velocityX": 2.815491033088066, + "velocityY": -0.24435622315789604, + "timestamp": 0.40541992484548933 + }, + { + "x": 3.2717928299149217, + "y": 6.42478435503329, + "heading": 1.5028090379808443, + "angularVelocity": 1.944228077247935, + "velocityX": 3.171920743869031, + "velocityY": -0.21598186048549792, + "timestamp": 0.4560974154511755 + }, + { + "x": 3.4491659910764505, + "y": 6.417474354407812, + "heading": 1.5494205381132569, + "angularVelocity": 0.9197673281632974, + "velocityX": 3.5000383610475594, + "velocityY": -0.14424551291134025, + "timestamp": 0.5067749060568617 + }, + { + "x": 3.6408166356979788, + "y": 6.41271012244396, + "heading": 1.5494208191511818, + "angularVelocity": 0.0000055456164384325836, + "velocityX": 3.7817706111917184, + "velocityY": -0.09401081045866226, + "timestamp": 0.5574523966625479 + }, + { + "x": 3.8324672911847486, + "y": 6.407946327321931, + "heading": 1.5494211001674396, + "angularVelocity": 0.000005545188889162315, + "velocityX": 3.781770825591478, + "velocityY": -0.0940021904220796, + "timestamp": 0.608129887268234 + }, + { + "x": 4.024117946671782, + "y": 6.403182532210449, + "heading": 1.5494213811836914, + "angularVelocity": 0.000005545188770808219, + "velocityX": 3.781770825596674, + "velocityY": -0.09400219021396121, + "timestamp": 0.6588073778739202 + }, + { + "x": 4.215768602158818, + "y": 6.398418737099029, + "heading": 1.549421662199937, + "angularVelocity": 0.00000554518865069831, + "velocityX": 3.7817708255967277, + "velocityY": -0.09400219021273522, + "timestamp": 0.7094848684796063 + }, + { + "x": 4.407419257645857, + "y": 6.393654941987672, + "heading": 1.5494219432161769, + "angularVelocity": 0.000005545188531584207, + "velocityX": 3.7817708255967815, + "velocityY": -0.09400219021151421, + "timestamp": 0.7601623590852925 + }, + { + "x": 4.599069913132898, + "y": 6.388891146876375, + "heading": 1.5494222242324105, + "angularVelocity": 0.000005545188412800537, + "velocityX": 3.7817708255968348, + "velocityY": -0.09400219021029173, + "timestamp": 0.8108398496909787 + }, + { + "x": 4.790720568620005, + "y": 6.3841273517676544, + "heading": 1.5494225052486381, + "angularVelocity": 0.00000554518829471885, + "velocityX": 3.7817708255981217, + "velocityY": -0.09400219015946695, + "timestamp": 0.8615173402966648 + }, + { + "x": 4.982371226710398, + "y": 6.37936366139224, + "heading": 1.5494227862648886, + "angularVelocity": 0.000005545188744147069, + "velocityX": 3.7817708769677805, + "velocityY": -0.0940001234962541, + "timestamp": 0.912194830902351 + }, + { + "x": 5.174080065051167, + "y": 6.378751705216163, + "heading": 1.549423195262445, + "angularVelocity": 0.000008070596070365484, + "velocityX": 3.7829189261249443, + "velocityY": -0.012075502728391172, + "timestamp": 0.9628723215080371 + }, + { + "x": 5.350199539544593, + "y": 6.384088378759385, + "heading": 1.6010891045354454, + "angularVelocity": 1.0195040965032165, + "velocityX": 3.475299830131391, + "velocityY": 0.10530658640431838, + "timestamp": 1.0135498121137234 + }, + { + "x": 5.508914034064036, + "y": 6.3919447908409595, + "heading": 1.700642086858645, + "angularVelocity": 1.9644418287757384, + "velocityX": 3.1318538590313407, + "velocityY": 0.15502764615367012, + "timestamp": 1.0642273027194096 + }, + { + "x": 5.6491553693293675, + "y": 6.399251365477575, + "heading": 1.8420339185260208, + "angularVelocity": 2.790032220962242, + "velocityX": 2.767329905036698, + "velocityY": 0.1441779091523542, + "timestamp": 1.1149047933250957 + }, + { + "x": 5.7706469991353355, + "y": 6.404320348217774, + "heading": 2.021527738979161, + "angularVelocity": 3.5418845390304368, + "velocityX": 2.3973489680315123, + "velocityY": 0.10002434373950005, + "timestamp": 1.165582283930782 + }, + { + "x": 5.873785275932455, + "y": 6.405912807850552, + "heading": 2.2392890154787506, + "angularVelocity": 4.297001960771041, + "velocityX": 2.0351891059410097, + "velocityY": 0.031423411335980894, + "timestamp": 1.216259774536468 + }, + { + "x": 5.95797180212043, + "y": 6.406163514657056, + "heading": 2.491341752729869, + "angularVelocity": 4.973662551926692, + "velocityX": 1.6612212874354277, + "velocityY": 0.0049471038030451655, + "timestamp": 1.2669372651421542 + }, + { + "x": 6.0211013406129235, + "y": 6.405559581297543, + "heading": 2.729789922190179, + "angularVelocity": 4.705208695427311, + "velocityX": 1.2457116115652769, + "velocityY": -0.011917191484722348, + "timestamp": 1.3176147557478404 + }, + { + "x": 6.065532872021379, + "y": 6.4052262614739615, + "heading": 2.9278417065045454, + "angularVelocity": 3.9080819106726716, + "velocityX": 0.8767508192970696, + "velocityY": -0.006577275622721501, + "timestamp": 1.3682922463535265 + }, + { + "x": 6.093774182775446, + "y": 6.405215566956099, + "heading": 3.067573672201354, + "angularVelocity": 2.75727870553105, + "velocityX": 0.5572752402799367, + "velocityY": -0.00021103092781244465, + "timestamp": 1.4189697369592127 + }, + { + "x": 6.10739278793335, + "y": 6.405193328857422, + "heading": 3.14, + "angularVelocity": 1.429161683678941, + "velocityX": 0.2687308506229643, + "velocityY": -0.00043881609787088405, + "timestamp": 1.4696472275648989 + }, + { + "x": 6.10739278793335, + "y": 6.405193328857422, + "heading": 3.14, + "angularVelocity": 0, + "velocityX": -3.519710920270602e-33, + "velocityY": 0, + "timestamp": 1.520324718170585 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/testtest.traj b/src/main/deploy/choreo/testtest.traj new file mode 100644 index 0000000..1a44408 --- /dev/null +++ b/src/main/deploy/choreo/testtest.traj @@ -0,0 +1,283 @@ +{ + "samples": [ + { + "x": 2.5015885829925537, + "y": 6.46366548538208, + "heading": 0, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 2.5156072081078307, + "y": 6.463710143904023, + "heading": 0.07027591296878091, + "angularVelocity": 1.3867283507699126, + "velocityX": 0.27662429508110253, + "velocityY": 0.0008812299387578504, + "timestamp": 0.05067749060568617 + }, + { + "x": 2.544396066654515, + "y": 6.464564468654549, + "heading": 0.20620072889082977, + "angularVelocity": 2.6821536405513675, + "velocityX": 0.5680797964265044, + "velocityY": 0.016858071311649372, + "timestamp": 0.10135498121137233 + }, + { + "x": 2.5897390778177214, + "y": 6.46666732372773, + "heading": 0.39551260118183784, + "angularVelocity": 3.7356204900517853, + "velocityX": 0.8947367089663785, + "velocityY": 0.041494853988394205, + "timestamp": 0.1520324718170585 + }, + { + "x": 2.654398590715119, + "y": 6.46738940563756, + "heading": 0.6167944034832189, + "angularVelocity": 4.3664711819127175, + "velocityX": 1.2759020252305684, + "velocityY": 0.014248572713453313, + "timestamp": 0.20270996242274467 + }, + { + "x": 2.739467602918066, + "y": 6.465281363276188, + "heading": 0.858858871076778, + "angularVelocity": 4.776567756225854, + "velocityX": 1.6786350544634498, + "velocityY": -0.041597212809432765, + "timestamp": 0.25338745302843085 + }, + { + "x": 2.844505231165706, + "y": 6.458645601789904, + "heading": 1.0779317888091438, + "angularVelocity": 4.322884087472656, + "velocityX": 2.0726682989282534, + "velocityY": -0.13094100372719633, + "timestamp": 0.304064943634117 + }, + { + "x": 2.9683658258358037, + "y": 6.4481131339425675, + "heading": 1.2606572080473037, + "angularVelocity": 3.605652471230642, + "velocityX": 2.44409486716376, + "velocityY": -0.207833255385285, + "timestamp": 0.3547424342398032 + }, + { + "x": 3.1110478462155178, + "y": 6.435729773739042, + "heading": 1.4042804378608007, + "angularVelocity": 2.834063567413143, + "velocityX": 2.815491033088066, + "velocityY": -0.24435622315789604, + "timestamp": 0.40541992484548933 + }, + { + "x": 3.2717928299149217, + "y": 6.42478435503329, + "heading": 1.5028090379808443, + "angularVelocity": 1.944228077247935, + "velocityX": 3.171920743869031, + "velocityY": -0.21598186048549792, + "timestamp": 0.4560974154511755 + }, + { + "x": 3.4491659910764505, + "y": 6.417474354407812, + "heading": 1.5494205381132569, + "angularVelocity": 0.9197673281632974, + "velocityX": 3.5000383610475594, + "velocityY": -0.14424551291134025, + "timestamp": 0.5067749060568617 + }, + { + "x": 3.6408166356979788, + "y": 6.41271012244396, + "heading": 1.5494208191511818, + "angularVelocity": 0.0000055456164384325836, + "velocityX": 3.7817706111917184, + "velocityY": -0.09401081045866226, + "timestamp": 0.5574523966625479 + }, + { + "x": 3.8324672911847486, + "y": 6.407946327321931, + "heading": 1.5494211001674396, + "angularVelocity": 0.000005545188889162315, + "velocityX": 3.781770825591478, + "velocityY": -0.0940021904220796, + "timestamp": 0.608129887268234 + }, + { + "x": 4.024117946671782, + "y": 6.403182532210449, + "heading": 1.5494213811836914, + "angularVelocity": 0.000005545188770808219, + "velocityX": 3.781770825596674, + "velocityY": -0.09400219021396121, + "timestamp": 0.6588073778739202 + }, + { + "x": 4.215768602158818, + "y": 6.398418737099029, + "heading": 1.549421662199937, + "angularVelocity": 0.00000554518865069831, + "velocityX": 3.7817708255967277, + "velocityY": -0.09400219021273522, + "timestamp": 0.7094848684796063 + }, + { + "x": 4.407419257645857, + "y": 6.393654941987672, + "heading": 1.5494219432161769, + "angularVelocity": 0.000005545188531584207, + "velocityX": 3.7817708255967815, + "velocityY": -0.09400219021151421, + "timestamp": 0.7601623590852925 + }, + { + "x": 4.599069913132898, + "y": 6.388891146876375, + "heading": 1.5494222242324105, + "angularVelocity": 0.000005545188412800537, + "velocityX": 3.7817708255968348, + "velocityY": -0.09400219021029173, + "timestamp": 0.8108398496909787 + }, + { + "x": 4.790720568620005, + "y": 6.3841273517676544, + "heading": 1.5494225052486381, + "angularVelocity": 0.00000554518829471885, + "velocityX": 3.7817708255981217, + "velocityY": -0.09400219015946695, + "timestamp": 0.8615173402966648 + }, + { + "x": 4.982371226710398, + "y": 6.37936366139224, + "heading": 1.5494227862648886, + "angularVelocity": 0.000005545188744147069, + "velocityX": 3.7817708769677805, + "velocityY": -0.0940001234962541, + "timestamp": 0.912194830902351 + }, + { + "x": 5.174080065051167, + "y": 6.378751705216163, + "heading": 1.549423195262445, + "angularVelocity": 0.000008070596070365484, + "velocityX": 3.7829189261249443, + "velocityY": -0.012075502728391172, + "timestamp": 0.9628723215080371 + }, + { + "x": 5.350199539544593, + "y": 6.384088378759385, + "heading": 1.6010891045354454, + "angularVelocity": 1.0195040965032165, + "velocityX": 3.475299830131391, + "velocityY": 0.10530658640431838, + "timestamp": 1.0135498121137234 + }, + { + "x": 5.508914034064036, + "y": 6.3919447908409595, + "heading": 1.700642086858645, + "angularVelocity": 1.9644418287757384, + "velocityX": 3.1318538590313407, + "velocityY": 0.15502764615367012, + "timestamp": 1.0642273027194096 + }, + { + "x": 5.6491553693293675, + "y": 6.399251365477575, + "heading": 1.8420339185260208, + "angularVelocity": 2.790032220962242, + "velocityX": 2.767329905036698, + "velocityY": 0.1441779091523542, + "timestamp": 1.1149047933250957 + }, + { + "x": 5.7706469991353355, + "y": 6.404320348217774, + "heading": 2.021527738979161, + "angularVelocity": 3.5418845390304368, + "velocityX": 2.3973489680315123, + "velocityY": 0.10002434373950005, + "timestamp": 1.165582283930782 + }, + { + "x": 5.873785275932455, + "y": 6.405912807850552, + "heading": 2.2392890154787506, + "angularVelocity": 4.297001960771041, + "velocityX": 2.0351891059410097, + "velocityY": 0.031423411335980894, + "timestamp": 1.216259774536468 + }, + { + "x": 5.95797180212043, + "y": 6.406163514657056, + "heading": 2.491341752729869, + "angularVelocity": 4.973662551926692, + "velocityX": 1.6612212874354277, + "velocityY": 0.0049471038030451655, + "timestamp": 1.2669372651421542 + }, + { + "x": 6.0211013406129235, + "y": 6.405559581297543, + "heading": 2.729789922190179, + "angularVelocity": 4.705208695427311, + "velocityX": 1.2457116115652769, + "velocityY": -0.011917191484722348, + "timestamp": 1.3176147557478404 + }, + { + "x": 6.065532872021379, + "y": 6.4052262614739615, + "heading": 2.9278417065045454, + "angularVelocity": 3.9080819106726716, + "velocityX": 0.8767508192970696, + "velocityY": -0.006577275622721501, + "timestamp": 1.3682922463535265 + }, + { + "x": 6.093774182775446, + "y": 6.405215566956099, + "heading": 3.067573672201354, + "angularVelocity": 2.75727870553105, + "velocityX": 0.5572752402799367, + "velocityY": -0.00021103092781244465, + "timestamp": 1.4189697369592127 + }, + { + "x": 6.10739278793335, + "y": 6.405193328857422, + "heading": 3.14, + "angularVelocity": 1.429161683678941, + "velocityX": 0.2687308506229643, + "velocityY": -0.00043881609787088405, + "timestamp": 1.4696472275648989 + }, + { + "x": 6.10739278793335, + "y": 6.405193328857422, + "heading": 3.14, + "angularVelocity": 0, + "velocityX": -3.519710920270602e-33, + "velocityY": 0, + "timestamp": 1.520324718170585 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/yanda.chor b/src/main/deploy/choreo/yanda.chor index 8db6307..7ba516e 100644 --- a/src/main/deploy/choreo/yanda.chor +++ b/src/main/deploy/choreo/yanda.chor @@ -303,14 +303,59 @@ "first" ], "type": "StopPoint", - "uuid": "dc1473d3-c7a3-4d94-866e-bde11c323887" + "uuid": "b638f1fd-41ab-4a20-874d-d4c9429fdc74", + "direction": 0 }, { "scope": [ "last" ], "type": "StopPoint", - "uuid": "a2a32d1e-c0f3-4cf3-a34c-c6cba0be2e31" + "uuid": "ce104402-10d5-4d35-b746-dfc3342b08e0", + "direction": 0 + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + }, + "testtest": { + "waypoints": [ + { + "x": 2.5015885829925537, + "y": 6.46366548538208, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + }, + { + "x": 6.10739278793335, + "y": 6.405193328857422, + "heading": 3.14, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint", + "uuid": "e3cb23d1-a897-4c5c-9148-077712d5976d" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint", + "uuid": "a4578151-db9c-40fb-8331-1517e7c6d0ce" } ], "usesControlIntervalGuessing": true, diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 9c9e064..dcda95c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -53,6 +53,7 @@ public void robotInit() { m_chooser.addOption("Ball Speaker", Autos.AutoType.BALLSPEAKER); m_chooser.addOption("Test Path", Autos.AutoType.TESTPATH); m_chooser.setDefaultOption("Test Path", Autos.AutoType.TESTPATH); + m_chooser.setDefaultOption("test test", Autos.AutoType.TESTTEST); SmartDashboard.putData("Auto Paths", m_chooser); diff --git a/src/main/java/frc/robot/commands/Autos.java b/src/main/java/frc/robot/commands/Autos.java index 1550cca..104fec6 100644 --- a/src/main/java/frc/robot/commands/Autos.java +++ b/src/main/java/frc/robot/commands/Autos.java @@ -95,7 +95,8 @@ public enum AutoType { TEST("test", new Command[]{}), ONEBALLAMP("one ball amp", new Command[]{}), BALLSPEAKER("ball speaker", new Command[]{}), - TESTPATH("TestPath", new Command[]{new InstantCommand()}); + TESTPATH("TestPath", new Command[]{new InstantCommand()}), + TESTTEST("testtest", new Command[]{}); String name; Command[] mechCommands; @@ -105,6 +106,7 @@ private AutoType(String a, Command[] mechCommands, boolean[] parallelToPath){ name = a; this.mechCommands = mechCommands; this.parallelToPath = parallelToPath; + } private AutoType(String a, Command[] mechCommands){