From 012ebd1d1a500634b01071f75b559e8126809ac3 Mon Sep 17 00:00:00 2001 From: Stephen Chen Date: Sat, 16 Nov 2024 16:58:58 -0500 Subject: [PATCH] new auton paths yes sirski fanum tax ohio rizz skibidi --- .../com/example/meepmeep/PathVisualizer.java | 4 +- .../meepmeep/positions/blue/far-basket.json | 30 +++++++++-- .../meepmeep/positions/red/close-basket.json | 5 ++ .../meepmeep/positions/red/far-basket.json | 23 +++++++-- .../trajectories/BlueCloseBasket.java | 2 + .../meepmeep/trajectories/BlueFarBasket.java | 51 ++++++++++++++++--- .../meepmeep/trajectories/RedCloseBasket.java | 2 + .../meepmeep/trajectories/RedFarBasket.java | 34 +++++++++++-- 8 files changed, 129 insertions(+), 22 deletions(-) diff --git a/PathVisualizer/src/main/java/com/example/meepmeep/PathVisualizer.java b/PathVisualizer/src/main/java/com/example/meepmeep/PathVisualizer.java index 7a31b7e3..9ca1486e 100644 --- a/PathVisualizer/src/main/java/com/example/meepmeep/PathVisualizer.java +++ b/PathVisualizer/src/main/java/com/example/meepmeep/PathVisualizer.java @@ -17,9 +17,9 @@ public static void main(String[] args) { .build(); - BlueCloseBasket trajectories = new BlueCloseBasket(); + // BlueCloseBasket trajectories = new BlueCloseBasket(); // BlueFarBasket trajectories = new BlueFarBasket(); - // RedCloseBasket trajectories = new RedCloseBasket(); + RedCloseBasket trajectories = new RedCloseBasket(); // RedFarBasket trajectories = new RedFarBasket(); myBot.runAction(trajectories.start(myBot.getDrive().actionBuilder(trajectories.getStart()))); diff --git a/PathVisualizer/src/main/java/com/example/meepmeep/positions/blue/far-basket.json b/PathVisualizer/src/main/java/com/example/meepmeep/positions/blue/far-basket.json index 2e2218a6..d5ddfec2 100644 --- a/PathVisualizer/src/main/java/com/example/meepmeep/positions/blue/far-basket.json +++ b/PathVisualizer/src/main/java/com/example/meepmeep/positions/blue/far-basket.json @@ -7,7 +7,22 @@ "RUNG": { "x": 0.0, "y": 38.0, - "heading": 90.0 + "heading": 270.0 + }, + "RUNG1": { + "x": -2, + "y": 38.0, + "heading": 270.0 + }, + "RUNG2": { + "x": -4, + "y": 38.0, + "heading": 270.0 + }, + "RUNG3": { + "x": -6, + "y": 38.0, + "heading": 270.0 }, "MID_SPIKEMARK": { "x": -58.0, @@ -24,9 +39,14 @@ "y": 49.0, "heading": 116.5 }, - "SUBMERSIBLE": { - "x": 0.0, - "y": 38.0, - "heading": 90.0 + "SCORE": { + "x": -23.0, + "y": 49.0, + "heading": 315 + }, + "PARK": { + "x": -46.0, + "y": 58.0, + "heading": 270 } } \ No newline at end of file diff --git a/PathVisualizer/src/main/java/com/example/meepmeep/positions/red/close-basket.json b/PathVisualizer/src/main/java/com/example/meepmeep/positions/red/close-basket.json index 0444137f..e0494f09 100644 --- a/PathVisualizer/src/main/java/com/example/meepmeep/positions/red/close-basket.json +++ b/PathVisualizer/src/main/java/com/example/meepmeep/positions/red/close-basket.json @@ -28,5 +28,10 @@ "x": -55.0, "y": -55.0, "heading": 225.0 + }, + "SUBMERSIBLE": { + "x": -34.5, + "y": -11.5, + "heading": 180.0 } } \ No newline at end of file diff --git a/PathVisualizer/src/main/java/com/example/meepmeep/positions/red/far-basket.json b/PathVisualizer/src/main/java/com/example/meepmeep/positions/red/far-basket.json index 293d709d..58e42ee9 100644 --- a/PathVisualizer/src/main/java/com/example/meepmeep/positions/red/far-basket.json +++ b/PathVisualizer/src/main/java/com/example/meepmeep/positions/red/far-basket.json @@ -9,6 +9,21 @@ "y": -38.0, "heading": 90.0 }, + "RUNG1": { + "x": 3.0, + "y": -38.0, + "heading": 90.0 + }, + "RUNG2": { + "x": 6.0, + "y": -38.0, + "heading": 90.0 + }, + "RUNG3": { + "x": 9.0, + "y": -38.0, + "heading": 90.0 + }, "MID_SPIKEMARK": { "x": 58.0, "y": -49.0, @@ -29,9 +44,9 @@ "y": -49.0, "heading": 135 }, - "SUBMERSIBLE": { - "x": -34.5, - "y": -11.5, - "heading": 180.0 + "PARK": { + "x": 46.0, + "y": -58, + "heading": 90 } } \ No newline at end of file diff --git a/PathVisualizer/src/main/java/com/example/meepmeep/trajectories/BlueCloseBasket.java b/PathVisualizer/src/main/java/com/example/meepmeep/trajectories/BlueCloseBasket.java index 4324dc01..f464386e 100644 --- a/PathVisualizer/src/main/java/com/example/meepmeep/trajectories/BlueCloseBasket.java +++ b/PathVisualizer/src/main/java/com/example/meepmeep/trajectories/BlueCloseBasket.java @@ -88,6 +88,8 @@ public Action start(TrajectoryActionBuilder builder) { .splineToLinearHeading(SCORE, Math.toRadians(93)) .setTangent(Math.toRadians(180)) .splineToLinearHeading(SUBMERSIBLE, Math.toRadians(270)) + .setTangent(Math.toRadians(90)) + .splineToLinearHeading(SCORE, Math.toRadians(0)) .build(); } diff --git a/PathVisualizer/src/main/java/com/example/meepmeep/trajectories/BlueFarBasket.java b/PathVisualizer/src/main/java/com/example/meepmeep/trajectories/BlueFarBasket.java index a0edd5cd..3549d7c3 100644 --- a/PathVisualizer/src/main/java/com/example/meepmeep/trajectories/BlueFarBasket.java +++ b/PathVisualizer/src/main/java/com/example/meepmeep/trajectories/BlueFarBasket.java @@ -11,7 +11,7 @@ import java.util.Scanner; public class BlueFarBasket { - private final Pose2d START, RUNGS, LEFT_SPIKEMARK, MID_SPIKEMARK, RIGHT_SPIKEMARK, SUBMERSIBLE; + private final Pose2d START, RUNGS, RUNGS1, RUNGS2, RUNGS3, LEFT_SPIKEMARK, MID_SPIKEMARK, RIGHT_SPIKEMARK, SCORE, PARK; public BlueFarBasket() { String jsonString = ""; @@ -42,6 +42,24 @@ public BlueFarBasket() { Math.toRadians(positions.getJSONObject("RUNG").getDouble("heading")) ); + this.RUNGS1 = new Pose2d( + positions.getJSONObject("RUNG1").getDouble("x"), + positions.getJSONObject("RUNG1").getDouble("y"), + Math.toRadians(positions.getJSONObject("RUNG1").getDouble("heading")) + ); + + this.RUNGS2 = new Pose2d( + positions.getJSONObject("RUNG2").getDouble("x"), + positions.getJSONObject("RUNG2").getDouble("y"), + Math.toRadians(positions.getJSONObject("RUNG2").getDouble("heading")) + ); + + this.RUNGS3 = new Pose2d( + positions.getJSONObject("RUNG3").getDouble("x"), + positions.getJSONObject("RUNG3").getDouble("y"), + Math.toRadians(positions.getJSONObject("RUNG3").getDouble("heading")) + ); + this.LEFT_SPIKEMARK = new Pose2d( positions.getJSONObject("LEFT_SPIKEMARK").getDouble("x"), positions.getJSONObject("LEFT_SPIKEMARK").getDouble("y"), @@ -60,10 +78,16 @@ public BlueFarBasket() { Math.toRadians(positions.getJSONObject("RIGHT_SPIKEMARK").getDouble("heading")) ); - this.SUBMERSIBLE = new Pose2d( - positions.getJSONObject("SUBMERSIBLE").getDouble("x"), - positions.getJSONObject("SUBMERSIBLE").getDouble("y"), - Math.toRadians(positions.getJSONObject("SUBMERSIBLE").getDouble("heading")) + this.SCORE = new Pose2d( + positions.getJSONObject("SCORE").getDouble("x"), + positions.getJSONObject("SCORE").getDouble("y"), + Math.toRadians(positions.getJSONObject("SCORE").getDouble("heading")) + ); + + this.PARK = new Pose2d( + positions.getJSONObject("PARK").getDouble("x"), + positions.getJSONObject("PARK").getDouble("y"), + Math.toRadians(positions.getJSONObject("PARK").getDouble("heading")) ); } @@ -75,11 +99,24 @@ public Action start(TrajectoryActionBuilder builder) { .turnTo(Math.toRadians(90)) .turnTo(this.RIGHT_SPIKEMARK.heading) .turnTo(Math.toRadians(90)) + .turnTo(Math.toRadians((270))) // Start of the cycling .setTangent(Math.toRadians(335)) - .splineToLinearHeading(this.SUBMERSIBLE, Math.toRadians(0)) + .splineToLinearHeading(this.RUNGS, Math.toRadians(0)) .setTangent(Math.toRadians(180)) - .splineToLinearHeading(this.MID_SPIKEMARK, Math.toRadians(150)) + .splineToLinearHeading(this.SCORE, Math.toRadians(150)) + .setTangent(Math.toRadians(315)) + .splineToLinearHeading(this.RUNGS1, Math.toRadians(0)) + .setTangent(Math.toRadians(180)) + .splineToLinearHeading(this.SCORE, Math.toRadians(150)) + .setTangent(Math.toRadians(315)) + .splineToLinearHeading(this.RUNGS2, Math.toRadians(0)) + .setTangent(Math.toRadians(180)) + .splineToLinearHeading(this.SCORE, Math.toRadians(150)) + .setTangent(Math.toRadians(315)) + .splineToLinearHeading(this.RUNGS3, Math.toRadians(0)) + .setTangent(Math.toRadians(180)) + .splineToLinearHeading(this.PARK, Math.toRadians(90)) .build(); } diff --git a/PathVisualizer/src/main/java/com/example/meepmeep/trajectories/RedCloseBasket.java b/PathVisualizer/src/main/java/com/example/meepmeep/trajectories/RedCloseBasket.java index b690f286..6450f538 100644 --- a/PathVisualizer/src/main/java/com/example/meepmeep/trajectories/RedCloseBasket.java +++ b/PathVisualizer/src/main/java/com/example/meepmeep/trajectories/RedCloseBasket.java @@ -88,6 +88,8 @@ public Action start(TrajectoryActionBuilder builder) { .splineToLinearHeading(this.SCORE, Math.toRadians(273)) .setTangent(Math.toRadians(0)) .splineToLinearHeading(this.SUBMERSIBLE, Math.toRadians(90)) + .setTangent(Math.toRadians(270)) + .splineToLinearHeading(this.SCORE, Math.toRadians(180)) .build(); } diff --git a/PathVisualizer/src/main/java/com/example/meepmeep/trajectories/RedFarBasket.java b/PathVisualizer/src/main/java/com/example/meepmeep/trajectories/RedFarBasket.java index db0af7bc..6e3b4888 100644 --- a/PathVisualizer/src/main/java/com/example/meepmeep/trajectories/RedFarBasket.java +++ b/PathVisualizer/src/main/java/com/example/meepmeep/trajectories/RedFarBasket.java @@ -11,7 +11,7 @@ import java.util.Scanner; public class RedFarBasket { - private final Pose2d START, RUNGS, LEFT_SPIKEMARK, MID_SPIKEMARK, RIGHT_SPIKEMARK, SCORE; + private final Pose2d START, RUNGS, RUNGS1, RUNGS2, RUNGS3, LEFT_SPIKEMARK, MID_SPIKEMARK, RIGHT_SPIKEMARK, SCORE, PARK; public RedFarBasket() { String jsonString = ""; @@ -42,6 +42,24 @@ public RedFarBasket() { Math.toRadians(positions.getJSONObject("RUNG").getDouble("heading")) ); + this.RUNGS1 = new Pose2d( + positions.getJSONObject("RUNG1").getDouble("x"), + positions.getJSONObject("RUNG1").getDouble("y"), + Math.toRadians(positions.getJSONObject("RUNG1").getDouble("heading")) + ); + + this.RUNGS2 = new Pose2d( + positions.getJSONObject("RUNG2").getDouble("x"), + positions.getJSONObject("RUNG2").getDouble("y"), + Math.toRadians(positions.getJSONObject("RUNG2").getDouble("heading")) + ); + + this.RUNGS3 = new Pose2d( + positions.getJSONObject("RUNG3").getDouble("x"), + positions.getJSONObject("RUNG3").getDouble("y"), + Math.toRadians(positions.getJSONObject("RUNG3").getDouble("heading")) + ); + this.LEFT_SPIKEMARK = new Pose2d( positions.getJSONObject("LEFT_SPIKEMARK").getDouble("x"), positions.getJSONObject("LEFT_SPIKEMARK").getDouble("y"), @@ -65,6 +83,12 @@ public RedFarBasket() { positions.getJSONObject("SCORE").getDouble("y"), Math.toRadians(positions.getJSONObject("SCORE").getDouble("heading")) ); + + this.PARK = new Pose2d( + positions.getJSONObject("PARK").getDouble("x"), + positions.getJSONObject("PARK").getDouble("y"), + Math.toRadians(positions.getJSONObject("PARK").getDouble("heading")) + ); } public Action start(TrajectoryActionBuilder builder) { @@ -81,15 +105,17 @@ public Action start(TrajectoryActionBuilder builder) { .setTangent(Math.toRadians(0)) .splineToLinearHeading(this.SCORE, Math.toRadians(330)) .setTangent(Math.toRadians(135)) - .splineToLinearHeading(this.RUNGS, Math.toRadians(180)) + .splineToLinearHeading(this.RUNGS1, Math.toRadians(180)) .setTangent(Math.toRadians(0)) .splineToLinearHeading(this.SCORE, Math.toRadians(330)) .setTangent(Math.toRadians(135)) - .splineToLinearHeading(this.RUNGS, Math.toRadians(180)) + .splineToLinearHeading(this.RUNGS2, Math.toRadians(180)) .setTangent(Math.toRadians(0)) .splineToLinearHeading(this.SCORE, Math.toRadians(330)) .setTangent(Math.toRadians(135)) - .splineToLinearHeading(this.RUNGS, Math.toRadians(180)) + .splineToLinearHeading(this.RUNGS3, Math.toRadians(180)) + .setTangent(Math.toRadians(0)) + .splineToLinearHeading(this.PARK, Math.toRadians(270)) .build(); }