From afaedd3bc6c7cfc72939ae7402f643f017132666 Mon Sep 17 00:00:00 2001 From: Jackjkliu Date: Fri, 9 Feb 2024 20:39:08 -0500 Subject: [PATCH 1/3] tweaked values --- .../meepmeeptesting/BlueBottomMeepMeep.java | 29 ++++++------------- 1 file changed, 9 insertions(+), 20 deletions(-) diff --git a/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/BlueBottomMeepMeep.java b/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/BlueBottomMeepMeep.java index 1b827a5..a616949 100644 --- a/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/BlueBottomMeepMeep.java +++ b/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/BlueBottomMeepMeep.java @@ -21,29 +21,18 @@ public static void main(String[] args) { .setColorScheme(new ColorSchemeBlueDark()) .followTrajectorySequence(drive -> drive.trajectorySequenceBuilder(new Pose2d(-36,60, Math.toRadians(-90))) - .lineToLinearHeading(new Pose2d(-35,-32, Math.toRadians(180))) + + + .lineToLinearHeading(new Pose2d(-35,32, Math.toRadians(180))) .back(4) .forward(4) - .lineToLinearHeading(new Pose2d(-30,-8, Math.toRadians(180))) - .lineToLinearHeading(new Pose2d(10,-8, Math.toRadians(180))) - .splineTo(new Vector2d(40,-36), Math.toRadians(0)) - - .lineToSplineHeading(new Pose2d(52, -30, Math.toRadians(0))) - - .lineTo(new Vector2d(52, -60)) - .lineTo(new Vector2d(60, -60)) - -// .lineToLinearHeading(new Pose2d(-35,-32, Math.toRadians(180))) -// .back(4) -// .forward(4) -// -// .lineToLinearHeading(new Pose2d(-30,-8, Math.toRadians(180))) -// .lineToLinearHeading(new Pose2d(10,-8, Math.toRadians(180))) -// .splineTo(new Vector2d(40,-36), Math.toRadians(0)) -// .lineToSplineHeading(new Pose2d(52, -30, Math.toRadians(180))) -// .lineTo(new Vector2d(52, -60)) -// .lineTo(new Vector2d(60, -60)) + .lineToLinearHeading(new Pose2d(-30,8, Math.toRadians(180))) + .lineToLinearHeading(new Pose2d(10,8, Math.toRadians(180))) + .splineTo(new Vector2d(40,36), Math.toRadians(0)) + .lineToSplineHeading(new Pose2d(52, 30, Math.toRadians(180))) + .lineTo(new Vector2d(52, 60)) + .lineTo(new Vector2d(60, 60)) .build()); meepMeep.setBackground(MeepMeep.Background.FIELD_CENTERSTAGE_JUICE_DARK) From 8faa29fc5d46e55bc723e0fa6b31db1e84ced6b8 Mon Sep 17 00:00:00 2001 From: Jackjkliu Date: Fri, 9 Feb 2024 21:11:23 -0500 Subject: [PATCH 2/3] bluebottom --- .../meepmeeptesting/BlueBottomMeepMeep.java | 17 ++++-- .../EK10582/auton/modes/BlueBottom.java | 57 +++++++++++++++---- 2 files changed, 60 insertions(+), 14 deletions(-) diff --git a/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/BlueBottomMeepMeep.java b/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/BlueBottomMeepMeep.java index a616949..70a7f5c 100644 --- a/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/BlueBottomMeepMeep.java +++ b/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/BlueBottomMeepMeep.java @@ -20,13 +20,22 @@ public static void main(String[] args) { .setConstraints(60, 60, Math.toRadians(180), Math.toRadians(180), 15) .setColorScheme(new ColorSchemeBlueDark()) .followTrajectorySequence(drive -> - drive.trajectorySequenceBuilder(new Pose2d(-36,60, Math.toRadians(-90))) - - - .lineToLinearHeading(new Pose2d(-35,32, Math.toRadians(180))) + drive.trajectorySequenceBuilder(new Pose2d(-36,60, Math.toRadians(90))) + .back(25) + .forward(10) + .strafeLeft(18) + .back(38) + + /* .lineToLinearHeading(new Pose2d(-36,34, Math.toRadians(90))) + .turn(Math.toRadians(90)) .back(4) .forward(4) + .strafeLeft(10) + .lineToLinearHeading(new Pose2d(-46,38, Math.toRadians(90))) + .forward(20) + .strafeRight(10)*/ + .lineToLinearHeading(new Pose2d(-30,8, Math.toRadians(180))) .lineToLinearHeading(new Pose2d(10,8, Math.toRadians(180))) .splineTo(new Vector2d(40,36), Math.toRadians(0)) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EK10582/auton/modes/BlueBottom.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EK10582/auton/modes/BlueBottom.java index 47a4578..bf5454f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EK10582/auton/modes/BlueBottom.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EK10582/auton/modes/BlueBottom.java @@ -21,7 +21,7 @@ public class BlueBottom extends AutonBase { public void runOpMode() { double distFromAprilTagX, distFromAprilTagForward; - Pose2d startPos = new Pose2d(-36,-60, Math.toRadians(-90)); + Pose2d startPos = new Pose2d(-36,-60, Math.toRadians(90)); waitForStart(); @@ -42,28 +42,65 @@ public void runOpMode() { switch (pos) { case LEFT: traj_pushPixel = robot.roadRunner.trajectorySequenceBuilder(startPos) - .lineToLinearHeading(new Pose2d(-35,-32, Math.toRadians(180))) - .back(4) - .forward(4) + .strafeLeft(10) + .lineToLinearHeading(new Pose2d(-46,38, Math.toRadians(90))) + .forward(20) + .strafeRight(10) .build(); traj_toBackboard = robot.roadRunner.trajectorySequenceBuilder(traj_pushPixel.end()) - .lineToLinearHeading(new Pose2d(-30,-8, Math.toRadians(180))) - .lineToLinearHeading(new Pose2d(10,-8, Math.toRadians(180))) - .splineTo(new Vector2d(40,-36), Math.toRadians(0)) + .lineToLinearHeading(new Pose2d(-30,8, Math.toRadians(180))) + .lineToLinearHeading(new Pose2d(10,8, Math.toRadians(180))) + .splineTo(new Vector2d(40,36), Math.toRadians(0)) .build(); traj_placePixel = robot.roadRunner.trajectorySequenceBuilder(traj_toBackboard.end()) - .lineToSplineHeading(new Pose2d(52, -30, Math.toRadians(0))) + .lineToSplineHeading(new Pose2d(52, 30, Math.toRadians(0))) .build(); traj_park = robot.roadRunner.trajectorySequenceBuilder(traj_placePixel.end()) - .lineTo(new Vector2d(52, -60)) - .lineTo(new Vector2d(60, -60)) + .lineTo(new Vector2d(52, 60)) + .lineTo(new Vector2d(60, 60)) .build(); break; case RIGHT: + traj_pushPixel = robot.roadRunner.trajectorySequenceBuilder(startPos) + .lineToLinearHeading(new Pose2d(-36,34, Math.toRadians(90))) + .turn(Math.toRadians(90)) + .back(4) + .forward(4) + .build(); + traj_toBackboard = robot.roadRunner.trajectorySequenceBuilder(traj_pushPixel.end()) + .lineToLinearHeading(new Pose2d(-30,8, Math.toRadians(180))) + .lineToLinearHeading(new Pose2d(10,8, Math.toRadians(180))) + .splineTo(new Vector2d(40,36), Math.toRadians(0)) + .build(); + traj_placePixel = robot.roadRunner.trajectorySequenceBuilder(traj_toBackboard.end()) + .lineToSplineHeading(new Pose2d(52, 30, Math.toRadians(0))) + .build(); + traj_park = robot.roadRunner.trajectorySequenceBuilder(traj_placePixel.end()) + .lineTo(new Vector2d(52, 60)) + .lineTo(new Vector2d(60, 60)) + .build(); break; case MIDDLE: + traj_pushPixel = robot.roadRunner.trajectorySequenceBuilder(startPos) + .back(25) + .forward(10) + .strafeLeft(18) + .back(38) + .build(); + traj_toBackboard = robot.roadRunner.trajectorySequenceBuilder(traj_pushPixel.end()) + .lineToLinearHeading(new Pose2d(-30,8, Math.toRadians(180))) + .lineToLinearHeading(new Pose2d(10,8, Math.toRadians(180))) + .splineTo(new Vector2d(40,36), Math.toRadians(0)) + .build(); + traj_placePixel = robot.roadRunner.trajectorySequenceBuilder(traj_toBackboard.end()) + .lineToSplineHeading(new Pose2d(52, 30, Math.toRadians(0))) + .build(); + traj_park = robot.roadRunner.trajectorySequenceBuilder(traj_placePixel.end()) + .lineTo(new Vector2d(52, 60)) + .lineTo(new Vector2d(60, 60)) + .build(); break; } From db2d0e21bcb7e540ce4981f673d7929fb1b7aca2 Mon Sep 17 00:00:00 2001 From: Jackjkliu Date: Fri, 16 Feb 2024 21:45:35 -0500 Subject: [PATCH 3/3] repathed the BlueBottom and RedBottom Autons --- .../meepmeeptesting/BlueBottomMeepMeep.java | 91 ++++++++++++++++--- .../EK10582/auton/modes/BlueBottom.java | 36 ++++---- .../EK10582/auton/modes/RedBottom.java | 56 ++++++++++-- 3 files changed, 142 insertions(+), 41 deletions(-) diff --git a/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/BlueBottomMeepMeep.java b/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/BlueBottomMeepMeep.java index 70a7f5c..69ab8d5 100644 --- a/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/BlueBottomMeepMeep.java +++ b/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/BlueBottomMeepMeep.java @@ -9,6 +9,8 @@ import com.noahbres.meepmeep.roadrunner.DefaultBotBuilder; import com.noahbres.meepmeep.roadrunner.entity.RoadRunnerBotEntity; +import java.util.Vector; + public class BlueBottomMeepMeep { public static void main(String[] args) { @@ -20,28 +22,87 @@ public static void main(String[] args) { .setConstraints(60, 60, Math.toRadians(180), Math.toRadians(180), 15) .setColorScheme(new ColorSchemeBlueDark()) .followTrajectorySequence(drive -> - drive.trajectorySequenceBuilder(new Pose2d(-36,60, Math.toRadians(90))) - .back(25) + drive.trajectorySequenceBuilder(new Pose2d(-36,-60, Math.toRadians(90))) + /* BBM .back(25) .forward(10) - .strafeLeft(18) - .back(38) + .splineToConstantHeading(new Vector2d(-54,24),Math.toRadians(-90)) + .splineToLinearHeading(new Pose2d(-30,8, Math.toRadians(90)),Math.toRadians(0)) + .turn(Math.toRadians(-90)) + .lineToConstantHeading(new Vector2d(15,8)) + .splineToLinearHeading(new Pose2d(40,36,Math.toRadians(180)), Math.toRadians(0)) + .lineToSplineHeading(new Pose2d(52, 36, Math.toRadians(180))) + .forward(6) + .lineTo(new Vector2d(46, 60)) + .lineTo(new Vector2d(60, 60)) */ + + /* RBM .forward(25) + .back(10) + .splineToConstantHeading(new Vector2d(-54,-24),Math.toRadians(90)) + .splineToLinearHeading(new Pose2d(-30,-8, Math.toRadians(90)),Math.toRadians(180)) + .turn(Math.toRadians(-90)) + .lineToConstantHeading(new Vector2d(15,-8)) + .splineToLinearHeading(new Pose2d(40,-36,Math.toRadians(180)), Math.toRadians(0)) + .lineToSplineHeading(new Pose2d(52, -36, Math.toRadians(180))) + .forward(6) + .lineTo(new Vector2d(46, -60)) + .lineTo(new Vector2d(60, -60)) */ - /* .lineToLinearHeading(new Pose2d(-36,34, Math.toRadians(90))) + /* BBR .lineToLinearHeading(new Pose2d(-36,34, Math.toRadians(90))) .turn(Math.toRadians(90)) .back(4) - .forward(4) + .forward(10) + .lineToLinearHeading(new Pose2d(-36,8, Math.toRadians(0))) + .lineToConstantHeading(new Vector2d(15,8)) + .splineToLinearHeading(new Pose2d(40,36,Math.toRadians(180)), Math.toRadians(0)) + .lineToSplineHeading(new Pose2d(52, 30, Math.toRadians(180))) + .forward(6) + .lineTo(new Vector2d(46, 60)) + .lineTo(new Vector2d(60, 60)) + */ + + /* RBR .lineToLinearHeading(new Pose2d(-36,-34, Math.toRadians(90))) + .turn(Math.toRadians(90)) + .back(4) + .forward(10) + .lineToLinearHeading(new Pose2d(-36,-8, Math.toRadians(0))) + .lineToConstantHeading(new Vector2d(15,-8)) + .splineToLinearHeading(new Pose2d(40,-36,Math.toRadians(180)), Math.toRadians(0)) + .lineToSplineHeading(new Pose2d(52, -42, Math.toRadians(180))) + .forward(6) + .lineTo(new Vector2d(46, -60)) + .lineTo(new Vector2d(60, -60))*/ + + - .strafeLeft(10) + + + /*BBL .strafeLeft(10) .lineToLinearHeading(new Pose2d(-46,38, Math.toRadians(90))) - .forward(20) - .strafeRight(10)*/ + .forward(10) + .strafeRight(10) + .lineToLinearHeading(new Pose2d(-36,8, Math.toRadians(0))) + .lineToConstantHeading(new Vector2d(15,8)) + .splineToLinearHeading(new Pose2d(40,36,Math.toRadians(180)), Math.toRadians(0)) + .lineToSplineHeading(new Pose2d(52, 42, Math.toRadians(180))) + .forward(6) + .lineTo(new Vector2d(46, 60)) + .lineTo(new Vector2d(60, 60)) */ + + /* RBL .strafeLeft(10) + .lineToLinearHeading(new Pose2d(-46,-38, Math.toRadians(90))) + .forward(10) + .strafeRight(10) + .lineToLinearHeading(new Pose2d(-36,-8, Math.toRadians(0))) + .lineToConstantHeading(new Vector2d(15,-8)) + .splineToLinearHeading(new Pose2d(40,-36,Math.toRadians(180)), Math.toRadians(0)) + .lineToSplineHeading(new Pose2d(52, -30, Math.toRadians(180))) + .forward(6) + .lineTo(new Vector2d(46, -60)) + .lineTo(new Vector2d(60, -60))*/ + + + - .lineToLinearHeading(new Pose2d(-30,8, Math.toRadians(180))) - .lineToLinearHeading(new Pose2d(10,8, Math.toRadians(180))) - .splineTo(new Vector2d(40,36), Math.toRadians(0)) - .lineToSplineHeading(new Pose2d(52, 30, Math.toRadians(180))) - .lineTo(new Vector2d(52, 60)) - .lineTo(new Vector2d(60, 60)) .build()); meepMeep.setBackground(MeepMeep.Background.FIELD_CENTERSTAGE_JUICE_DARK) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EK10582/auton/modes/BlueBottom.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EK10582/auton/modes/BlueBottom.java index bf5454f..7fc775f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EK10582/auton/modes/BlueBottom.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EK10582/auton/modes/BlueBottom.java @@ -44,16 +44,16 @@ public void runOpMode() { traj_pushPixel = robot.roadRunner.trajectorySequenceBuilder(startPos) .strafeLeft(10) .lineToLinearHeading(new Pose2d(-46,38, Math.toRadians(90))) - .forward(20) + .forward(10) .strafeRight(10) .build(); traj_toBackboard = robot.roadRunner.trajectorySequenceBuilder(traj_pushPixel.end()) - .lineToLinearHeading(new Pose2d(-30,8, Math.toRadians(180))) - .lineToLinearHeading(new Pose2d(10,8, Math.toRadians(180))) - .splineTo(new Vector2d(40,36), Math.toRadians(0)) + .lineToLinearHeading(new Pose2d(-36,8, Math.toRadians(180))) + .lineToLinearHeading(new Pose2d(15,8, Math.toRadians(180))) + .splineToLinearHeading(new Pose2d(40,36, Math.toRadians(180)),Math.toRadians(0)) .build(); traj_placePixel = robot.roadRunner.trajectorySequenceBuilder(traj_toBackboard.end()) - .lineToSplineHeading(new Pose2d(52, 30, Math.toRadians(0))) + .lineToSplineHeading(new Pose2d(52, 42, Math.toRadians(0))) .build(); traj_park = robot.roadRunner.trajectorySequenceBuilder(traj_placePixel.end()) .lineTo(new Vector2d(52, 60)) @@ -65,19 +65,20 @@ public void runOpMode() { .lineToLinearHeading(new Pose2d(-36,34, Math.toRadians(90))) .turn(Math.toRadians(90)) .back(4) - .forward(4) + .forward(10) .build(); traj_toBackboard = robot.roadRunner.trajectorySequenceBuilder(traj_pushPixel.end()) - .lineToLinearHeading(new Pose2d(-30,8, Math.toRadians(180))) - .lineToLinearHeading(new Pose2d(10,8, Math.toRadians(180))) - .splineTo(new Vector2d(40,36), Math.toRadians(0)) + .lineToLinearHeading(new Pose2d(-36,8, Math.toRadians(180))) + .lineToLinearHeading(new Pose2d(15,8, Math.toRadians(180))) + .splineToLinearHeading(new Pose2d(40,36, Math.toRadians(180)),Math.toRadians(0)) .build(); traj_placePixel = robot.roadRunner.trajectorySequenceBuilder(traj_toBackboard.end()) .lineToSplineHeading(new Pose2d(52, 30, Math.toRadians(0))) .build(); traj_park = robot.roadRunner.trajectorySequenceBuilder(traj_placePixel.end()) - .lineTo(new Vector2d(52, 60)) + .forward(6) + .lineTo(new Vector2d(46, 60)) .lineTo(new Vector2d(60, 60)) .build(); break; @@ -86,19 +87,20 @@ public void runOpMode() { traj_pushPixel = robot.roadRunner.trajectorySequenceBuilder(startPos) .back(25) .forward(10) - .strafeLeft(18) - .back(38) + .splineToConstantHeading(new Vector2d(-54,24),Math.toRadians(-90)) + .splineToLinearHeading(new Pose2d(-30,8, Math.toRadians(90)),Math.toRadians(0)) .build(); traj_toBackboard = robot.roadRunner.trajectorySequenceBuilder(traj_pushPixel.end()) - .lineToLinearHeading(new Pose2d(-30,8, Math.toRadians(180))) - .lineToLinearHeading(new Pose2d(10,8, Math.toRadians(180))) - .splineTo(new Vector2d(40,36), Math.toRadians(0)) + .turn(Math.toRadians(-90)) + .lineToLinearHeading(new Pose2d(15,8, Math.toRadians(180))) + .splineToLinearHeading(new Pose2d(40,36, Math.toRadians(180)),Math.toRadians(0)) .build(); traj_placePixel = robot.roadRunner.trajectorySequenceBuilder(traj_toBackboard.end()) - .lineToSplineHeading(new Pose2d(52, 30, Math.toRadians(0))) + .lineToSplineHeading(new Pose2d(52, 36, Math.toRadians(180))) .build(); traj_park = robot.roadRunner.trajectorySequenceBuilder(traj_placePixel.end()) - .lineTo(new Vector2d(52, 60)) + .forward(6) + .lineTo(new Vector2d(46, 60)) .lineTo(new Vector2d(60, 60)) .build(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EK10582/auton/modes/RedBottom.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EK10582/auton/modes/RedBottom.java index 33ded0f..a180e74 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EK10582/auton/modes/RedBottom.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EK10582/auton/modes/RedBottom.java @@ -42,28 +42,66 @@ public void runOpMode() { switch (pos) { case LEFT: traj_pushPixel = robot.roadRunner.trajectorySequenceBuilder(startPos) - .lineToLinearHeading(new Pose2d(-35,-32, Math.toRadians(180))) - .back(4) - .forward(4) + .strafeLeft(10) + .lineToLinearHeading(new Pose2d(-46,-38, Math.toRadians(90))) + .forward(10) + .strafeRight(10) .build(); traj_toBackboard = robot.roadRunner.trajectorySequenceBuilder(traj_pushPixel.end()) - .lineToLinearHeading(new Pose2d(-30,-8, Math.toRadians(180))) - .lineToLinearHeading(new Pose2d(10,-8, Math.toRadians(180))) - .splineTo(new Vector2d(40,-36), Math.toRadians(0)) + .lineToLinearHeading(new Pose2d(-36,-8, Math.toRadians(0))) + .lineToConstantHeading(new Vector2d(15,-8)) + .splineToLinearHeading(new Pose2d(40,-36,Math.toRadians(180)), Math.toRadians(0)) .build(); traj_placePixel = robot.roadRunner.trajectorySequenceBuilder(traj_toBackboard.end()) - .lineToSplineHeading(new Pose2d(52, -30, Math.toRadians(0))) + .lineToSplineHeading(new Pose2d(52, -30, Math.toRadians(180))) .build(); traj_park = robot.roadRunner.trajectorySequenceBuilder(traj_placePixel.end()) - .lineTo(new Vector2d(52, -60)) + .forward(6) + .lineTo(new Vector2d(46, -60)) .lineTo(new Vector2d(60, -60)) .build(); break; case RIGHT: - + traj_pushPixel = robot.roadRunner.trajectorySequenceBuilder(startPos) + .turn(Math.toRadians(90)) + .back(4) + .forward(10) + .build(); + traj_toBackboard = robot.roadRunner.trajectorySequenceBuilder(traj_pushPixel.end()) + .lineToLinearHeading(new Pose2d(-36,-8, Math.toRadians(0))) + .lineToConstantHeading(new Vector2d(15,-8)) + .splineToLinearHeading(new Pose2d(40,-36,Math.toRadians(180)), Math.toRadians(0)) + .build(); + traj_placePixel = robot.roadRunner.trajectorySequenceBuilder(traj_toBackboard.end()) + .lineToSplineHeading(new Pose2d(52, -42, Math.toRadians(180))) + .build(); + traj_park = robot.roadRunner.trajectorySequenceBuilder(traj_placePixel.end()) + .forward(6) + .lineTo(new Vector2d(46, -60)) + .lineTo(new Vector2d(60, -60)) + .build(); break; case MIDDLE: + traj_pushPixel = robot.roadRunner.trajectorySequenceBuilder(startPos) + .forward(25) + .back(10) + .splineToConstantHeading(new Vector2d(-54,-24),Math.toRadians(90)) + .splineToLinearHeading(new Pose2d(-30,-8, Math.toRadians(90)),Math.toRadians(180)) + .build(); + traj_toBackboard = robot.roadRunner.trajectorySequenceBuilder(traj_pushPixel.end()) + .turn(Math.toRadians(-90)) + .lineToConstantHeading(new Vector2d(15,-8)) + .splineToLinearHeading(new Pose2d(40,-36,Math.toRadians(180)), Math.toRadians(0)) + .build(); + traj_placePixel = robot.roadRunner.trajectorySequenceBuilder(traj_toBackboard.end()) + .lineToSplineHeading(new Pose2d(52, -36, Math.toRadians(180))) + .build(); + traj_park = robot.roadRunner.trajectorySequenceBuilder(traj_placePixel.end()) + .forward(6) + .lineTo(new Vector2d(46, -60)) + .lineTo(new Vector2d(60, -60)) + .build(); break; }