diff --git a/TeamCode/src/main/java/opmodes/AutoBase.java b/TeamCode/src/main/java/opmodes/AutoBase.java index 93cde28..b833740 100644 --- a/TeamCode/src/main/java/opmodes/AutoBase.java +++ b/TeamCode/src/main/java/opmodes/AutoBase.java @@ -9,10 +9,14 @@ import static org.firstinspires.ftc.teamcode.util.CenterStageCommon.PropLocation import static org.firstinspires.ftc.teamcode.util.CenterStageCommon.PropLocation.Right; import static org.firstinspires.ftc.teamcode.util.CenterStageCommon.PropLocation.Unknown; +import android.annotation.SuppressLint; + import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.geometry.Vector2d; +import com.arcrobotics.ftclib.gamepad.GamepadEx; +import com.arcrobotics.ftclib.gamepad.GamepadKeys; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.robotcore.external.Telemetry; @@ -23,82 +27,60 @@ import org.firstinspires.ftc.teamcode.vision.Detection; @Config public abstract class AutoBase extends LinearOpMode { - public static int DEPOSIT_HEIGHT = 100; - public static double SCORING_DURATION_S = 2f; // for spin of axle - public static double APRIL_TAG_RIGHT_DELTA = -8.5; - public static double APRIL_TAG_LEFT_DELTA = 7.0; - protected static double Delay = 5000; + protected AutoConfig config; protected Robot robot; - protected FtcDashboard dashboard; - protected Telemetry dashboardTelemetry; - protected CenterStageCommon.PropLocation propLocation; - protected final Pose2d initialPosition; - protected final CenterStageCommon.Alliance alliance; - protected final Pose2d parkLeft; - protected final Pose2d parkRight; - protected Pose2d park; - protected int delay = 0; - boolean leftWasPressed = false; - boolean rightWasPressed = false; - boolean upWasPressed = false; - boolean downWasPressed = false; - - - - - protected AutoBase(CenterStageCommon.Alliance alliance, Pose2d initialPosition, Pose2d parkLeft, Pose2d parkRight) { - this.alliance = alliance; - this.initialPosition = initialPosition; - this.parkLeft = parkLeft; - this.parkRight = parkRight; + protected AutoBase(AutoConfig config) { + this.config = config; } + @SuppressLint("DefaultLocale") @Override public void runOpMode() { // Initialize Robot and Dashboard - this.robot = new Robot(hardwareMap, telemetry, initialPosition, alliance); - this.dashboard = FtcDashboard.getInstance(); - this.dashboardTelemetry = dashboard.getTelemetry(); - this.park = parkLeft; + this.robot = new Robot(hardwareMap, telemetry, this.config.getInitialPosition(), this.config.getAlliance()); + GamepadEx controller1 = new GamepadEx(this.gamepad1); + GamepadEx controller2 = new GamepadEx(this.gamepad2); // Wait for match to start - while(!isStarted() && !isStopRequested()) { + while (!isStarted() && !isStopRequested()) { this.robot.update(); + controller1.readButtons(); + controller2.readButtons(); - boolean leftPressed = gamepad1.dpad_left; - boolean rightPressed = gamepad1.dpad_right; - boolean upPressed = gamepad1.dpad_up; - boolean downPressed = gamepad1.dpad_down; - this.telemetry.addData("To select parking location, use the dpad right or left. To add delay, use the dpad up to increase delay, and dpad down to decrease delay", ""); - if(leftPressed && !leftWasPressed) { - this.park = parkLeft; - } else if(rightPressed && !rightWasPressed) { - this.park = parkRight; - } else if(upPressed && !upWasPressed) { - this.delay += 1000; - } else if(downPressed && !downWasPressed) { - this.delay -= 1000; + if (controller1.wasJustPressed(GamepadKeys.Button.DPAD_LEFT) + || controller2.wasJustPressed(GamepadKeys.Button.DPAD_LEFT)) { + this.config.setParkLocation(AutoConfig.ParkLocation.Left); + } + if (controller1.wasJustPressed(GamepadKeys.Button.DPAD_RIGHT) + || controller2.wasJustPressed(GamepadKeys.Button.DPAD_RIGHT)) { + this.config.setParkLocation(AutoConfig.ParkLocation.Right); } - this.leftWasPressed = leftPressed; - this.rightWasPressed = rightPressed; - this.upWasPressed = upPressed; - this.downWasPressed = downPressed; + if (controller1.wasJustPressed(GamepadKeys.Button.DPAD_UP) + || controller2.wasJustPressed(GamepadKeys.Button.DPAD_UP)) { + this.config.increaseDelay(); + } + if (controller1.wasJustPressed(GamepadKeys.Button.DPAD_DOWN) + || controller2.wasJustPressed(GamepadKeys.Button.DPAD_DOWN)) { + this.config.decreaseDelay(); + } - this.telemetry.addData("Delay", this.delay); - this.telemetry.addData("Park set to", this.park); + this.telemetry.addLine("Press DPAD_UP to increase delay"); + this.telemetry.addLine("Press DPAD_DOWN to decrease delay"); + this.telemetry.addLine("Press DPAD_LEFT to park on the left"); + this.telemetry.addLine("Press DPAD_RIGHT to park on the right"); + this.telemetry.addData("Delay", String.format("%d second(s)", this.config.getDelay() / 1000)); + this.telemetry.addData("Park set to", this.config.getParkLocation()); this.sleep(20); } - if (isStopRequested()) { // remove later if nessacary as recent addition might be interefering + if (isStopRequested()) { return; } - // If the prop is visible at this point, then it must be in the center (2) position - determinePropLocation(); - - switch (this.propLocation) { + CenterStageCommon.PropLocation propLocation = determinePropLocation(); + switch (propLocation) { case Left: propLeft(); break; @@ -110,9 +92,10 @@ public abstract class AutoBase extends LinearOpMode { propRight(); break; } - this.sleep(delay); + + this.sleep(this.config.getDelay()); moveBackstage(); - prepareToScore(); + prepareToScore(propLocation); scorePreloadedPixel(); park(); } @@ -123,6 +106,8 @@ public abstract class AutoBase extends LinearOpMode { protected abstract void propRight(); + protected void moveBackstage() { } + protected void openAndLiftClaw() { this.robot.getClaw().openSync(); this.sleep(100); @@ -130,15 +115,15 @@ public abstract class AutoBase extends LinearOpMode { } protected void scorePreloadedPixel() { - this.robot.getGantry().setSlideTarget(DEPOSIT_HEIGHT); + this.robot.getGantry().setSlideTarget(AutoConfig.DEPOSIT_HEIGHT); this.robot.getGantry().armOut(); - while(this.robot.getGantry().isIn()) { + while (this.robot.getGantry().isIn()) { this.robot.update(); sleep(20); } this.robot.getGantry().deposit(); double startTime = this.getRuntime(); - while (this.getRuntime() < (startTime + SCORING_DURATION_S)) { + while (this.getRuntime() < (startTime + AutoConfig.SCORING_DURATION_S)) { this.robot.update(); } this.robot.getGantry().stop(); @@ -146,22 +131,18 @@ public abstract class AutoBase extends LinearOpMode { this.robot.getGantry().armInSync(); } - protected void prepareToScore() { + protected void prepareToScore(CenterStageCommon.PropLocation propLocation) { TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); this.robot.getGantry().armOut(); - this.robot.getGantry().setSlideTarget(DEPOSIT_HEIGHT); + this.robot.getGantry().setSlideTarget(AutoConfig.DEPOSIT_HEIGHT); - if (this.alliance == CenterStageCommon.Alliance.Blue) { - builder.lineToLinearHeading(new Pose2d(35, 36, 0)); - } else if (this.alliance == CenterStageCommon.Alliance.Red) { - builder.lineToLinearHeading(new Pose2d(35, -35, 0)); - } + builder.lineToLinearHeading(new Pose2d(35, Math.copySign(36, this.config.getInitialPosition().getY()), 0)); this.robot.getDrive().followTrajectorySequenceAsync(builder.build()); - Pose2d inferredPos = null; - while(this.robot.getDrive().isBusy()) { + Pose2d inferredPos; + while (this.robot.getDrive().isBusy()) { this.robot.update(); inferredPos = this.robot.getCamera().estimatePoseFromAprilTag(); @@ -176,53 +157,34 @@ public abstract class AutoBase extends LinearOpMode { inferredPos = this.robot.getDrive().getPoseEstimate(); } - // At this point we know that Y = 38 - // For 2 -> Ydelta = 0 - // For 3 -> 3 5/8 - // For 1 -> - 3 5/8 double delta = 0; - switch (this.propLocation) { + switch (propLocation) { case Left: - delta = APRIL_TAG_LEFT_DELTA; + delta = AutoConfig.APRIL_TAG_LEFT_DELTA; break; case Unknown: case Center: delta = 0; break; case Right: - delta = APRIL_TAG_RIGHT_DELTA; + delta = AutoConfig.APRIL_TAG_RIGHT_DELTA; break; } builder = this.robot.getDrive().trajectorySequenceBuilder(inferredPos); Pose2d target = new Pose2d( 60 - SCORING_DISTANCE_FROM_APRIL_TAG - CAMERA_FORWARD_OFFSET_IN, // 60 is the X position of the april tag - 36 + delta, + Math.copySign(36, this.config.getInitialPosition().getY()) + delta, 0); builder.lineToLinearHeading(target); this.robot.getDrive().followTrajectorySequenceAsync(builder.build()); - while(this.robot.getDrive().isBusy()) { + while (this.robot.getDrive().isBusy()) { this.robot.update(); } } - protected void moveBackstage() { - TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - - if (!this.isBackstage()) { - if (this.alliance == CenterStageCommon.Alliance.Blue) { - builder.lineToSplineHeading(new Pose2d(-40, 60, Math.PI)); - builder.lineToLinearHeading(new Pose2d(12, 60, Math.PI)); - } else if (this.alliance == CenterStageCommon.Alliance.Red) { - builder.lineToSplineHeading(new Pose2d(-40, -60, Math.PI)); - builder.lineToLinearHeading(new Pose2d(12, -60, Math.PI)); - } - this.robot.getDrive().followTrajectorySequence(builder.build()); - } - } - - protected void determinePropLocation() { - this.robot.getClaw().setArmPositionAsync(PICKUP_ARM_MIN); // changed from setArmPositionAsync + protected CenterStageCommon.PropLocation determinePropLocation() { + this.robot.getClaw().setArmPositionAsync(PICKUP_ARM_MIN); while (!this.robot.getClaw().isArmAtPosition() && !this.robot.getCamera().getProp().isValid()) { this.robot.update(); @@ -231,26 +193,24 @@ public abstract class AutoBase extends LinearOpMode { sleep(250); - setPropLocationIfVisible(Center, Unknown); //only works if arm is async - if (this.propLocation != Center) { - peekRight(); + CenterStageCommon.PropLocation propLocation = getPropLocationIfVisible(Center, Unknown); + if (propLocation != Center) { + Pose2d currentPose = this.robot.getDrive().getPoseEstimate(); + final double y = currentPose.getY() > 0 ? -5 : 5; + final double z = Math.toRadians(-25); + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(currentPose.plus(new Pose2d(0, y, z))); + this.robot.getDrive().followTrajectorySequence(builder.build()); + + this.sleep(250); + + propLocation = getPropLocationIfVisible(Right, Left); } + + return propLocation; } - protected void peekRight() { - Pose2d currentPose = this.robot.getDrive().getPoseEstimate(); - final double y = currentPose.getY() > 0 ? -5 : 5; - final double z = Math.toRadians(-25); - TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToLinearHeading(currentPose.plus(new Pose2d(0, y, z))); - this.robot.getDrive().followTrajectorySequence(builder.build()); - - this.sleep(250); - - setPropLocationIfVisible(Right, Left); - } - - protected void setPropLocationIfVisible(CenterStageCommon.PropLocation ifVisible, CenterStageCommon.PropLocation ifNotVisible) { + private CenterStageCommon.PropLocation getPropLocationIfVisible(CenterStageCommon.PropLocation ifVisible, CenterStageCommon.PropLocation ifNotVisible) { float seenCount = 0; float samples = 10; for (int i = 0; i < samples; i++) { @@ -260,21 +220,18 @@ public abstract class AutoBase extends LinearOpMode { } } if (seenCount / samples > 0.5) { - this.propLocation = ifVisible; + return ifVisible; } else { - this.propLocation = ifNotVisible; + return ifNotVisible; } } - public void park() { + private void park() { + Pose2d park = this.config.getSelectionParkPosition(); double currentX = this.robot.getDrive().getPoseEstimate().getX(); TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); builder.strafeTo(new Vector2d(currentX, park.getY())); builder.lineToLinearHeading(park); this.robot.getDrive().followTrajectorySequence(builder.build()); } - - protected boolean isBackstage() { - return this.initialPosition.getX() > 0; - } } diff --git a/TeamCode/src/main/java/opmodes/AutoConfig.java b/TeamCode/src/main/java/opmodes/AutoConfig.java new file mode 100644 index 0000000..23ab4f4 --- /dev/null +++ b/TeamCode/src/main/java/opmodes/AutoConfig.java @@ -0,0 +1,59 @@ +package opmodes; + +import com.acmerobotics.roadrunner.geometry.Pose2d; + +import org.firstinspires.ftc.teamcode.util.CenterStageCommon; + +import lombok.AllArgsConstructor; +import lombok.Builder; +import lombok.Data; +import lombok.NoArgsConstructor; + +@AllArgsConstructor +@NoArgsConstructor +@Builder +@Data +public class AutoConfig { + public static int DEPOSIT_HEIGHT = 100; + public static double SCORING_DURATION_S = 2f; + public static double APRIL_TAG_RIGHT_DELTA = -8.5; + public static double APRIL_TAG_LEFT_DELTA = 7.0; + + private Pose2d initialPosition; + private Pose2d leftParkPosition; + private Pose2d rightParkPosition; + private ParkLocation parkLocation = ParkLocation.Left; + private long delay; + + public Pose2d getSelectionParkPosition() { + return this.parkLocation == ParkLocation.Left + ? this.leftParkPosition + : this.rightParkPosition; + } + + protected CenterStageCommon.Alliance getAlliance() { + if (this.initialPosition == null) { + throw new RuntimeException("Thou fool! Thou must set the initial position!"); + } + return this.getInitialPosition().getY() > 0 + ? CenterStageCommon.Alliance.Blue + : CenterStageCommon.Alliance.Red; + } + + public void increaseDelay() { + this.delay += 1000; + } + + public void decreaseDelay() { + this.delay -= 1000; + } + + protected boolean isBackstage() { + return this.initialPosition.getX() > 0; + } + + public enum ParkLocation { + Left, + Right + } +} diff --git a/TeamCode/src/main/java/opmodes/BlueBackStage.java b/TeamCode/src/main/java/opmodes/BlueBackStage.java index 64900ba..84fbf63 100644 --- a/TeamCode/src/main/java/opmodes/BlueBackStage.java +++ b/TeamCode/src/main/java/opmodes/BlueBackStage.java @@ -1,29 +1,25 @@ package opmodes; -import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PICKUP_ARM_MIN; - import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.geometry.Vector2d; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder; -import org.firstinspires.ftc.teamcode.util.CenterStageCommon; @Autonomous(name = "BlueBackStage", preselectTeleOp = "MainTeleOp") public class BlueBackStage extends AutoBase { public BlueBackStage() { - super( - CenterStageCommon.Alliance.Blue, - new Pose2d(12, 63, Math.toRadians(90)), - new Pose2d(62, 62), - new Pose2d(62, 12, Math.toRadians(0))); - - + super(AutoConfig.builder() + .initialPosition(new Pose2d(12, 63, Math.toRadians(90))) + .leftParkPosition(new Pose2d(62, 62)) + .rightParkPosition(new Pose2d(62, 12, Math.toRadians(0))) + .parkLocation(AutoConfig.ParkLocation.Right) + .build()); } + @Override protected void propLeft() { - TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); builder.lineToLinearHeading(new Pose2d(17.3, 41.6, Math.toRadians(108.25))); this.robot.getDrive().followTrajectorySequence(builder.build()); @@ -35,17 +31,17 @@ public class BlueBackStage extends AutoBase { this.robot.getDrive().followTrajectorySequence(builder.build()); } + @Override protected void propCenter() { - TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToLinearHeading(new Pose2d(12, 42, initialPosition.getHeading())); + builder.lineToLinearHeading(new Pose2d(12, 42, this.config.getInitialPosition().getHeading())); this.robot.getDrive().followTrajectorySequence(builder.build()); openAndLiftClaw(); } + @Override protected void propRight() { - TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); builder.lineToLinearHeading(new Pose2d(18.25, 32, Math.toRadians(0))); this.robot.getDrive().followTrajectorySequence(builder.build()); diff --git a/TeamCode/src/main/java/opmodes/BlueFrontStage.java b/TeamCode/src/main/java/opmodes/BlueFrontStage.java index 7a4bc9f..53e445b 100644 --- a/TeamCode/src/main/java/opmodes/BlueFrontStage.java +++ b/TeamCode/src/main/java/opmodes/BlueFrontStage.java @@ -1,28 +1,24 @@ package opmodes; -import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PICKUP_ARM_MIN; - import com.acmerobotics.roadrunner.geometry.Pose2d; -import com.acmerobotics.roadrunner.geometry.Vector2d; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder; -import org.firstinspires.ftc.teamcode.util.CenterStageCommon; @Autonomous(name = "BlueFrontStage", preselectTeleOp = "MainTeleOp") public class BlueFrontStage extends AutoBase { public BlueFrontStage() { - super( - CenterStageCommon.Alliance.Blue, - new Pose2d(-36, 63, Math.toRadians(90)), - new Pose2d(62, 62), - new Pose2d(62, 12, Math.toRadians(0))); - + super(AutoConfig.builder() + .initialPosition(new Pose2d(-36, 63, Math.toRadians(90))) + .leftParkPosition(new Pose2d(62, 62)) + .rightParkPosition(new Pose2d(62, 12, Math.toRadians(0))) + .parkLocation(AutoConfig.ParkLocation.Left) + .build()); } + @Override protected void propLeft() { - TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); builder.lineToLinearHeading(new Pose2d(-43.5, 31.5, Math.toRadians(180))); this.robot.getDrive().followTrajectorySequence(builder.build()); @@ -30,21 +26,29 @@ public class BlueFrontStage extends AutoBase { openAndLiftClaw(); } + @Override protected void propCenter() { - TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToLinearHeading(new Pose2d(-36, 42, initialPosition.getHeading())); + builder.lineToLinearHeading(new Pose2d(-36, 42, this.config.getInitialPosition().getHeading())); this.robot.getDrive().followTrajectorySequence(builder.build()); openAndLiftClaw(); } + @Override protected void propRight() { - TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); builder.lineToLinearHeading(new Pose2d(-39.34, 40.45, Math.toRadians(64.3))); this.robot.getDrive().followTrajectorySequence(builder.build()); openAndLiftClaw(); } + + @Override + protected void moveBackstage() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToSplineHeading(new Pose2d(-40, 60, Math.PI)); + builder.lineToLinearHeading(new Pose2d(12, 60, Math.PI)); + this.robot.getDrive().followTrajectorySequence(builder.build()); + } } \ No newline at end of file diff --git a/TeamCode/src/main/java/opmodes/RedBackStage.java b/TeamCode/src/main/java/opmodes/RedBackStage.java index eafe38a..f690d99 100644 --- a/TeamCode/src/main/java/opmodes/RedBackStage.java +++ b/TeamCode/src/main/java/opmodes/RedBackStage.java @@ -1,27 +1,25 @@ package opmodes; -import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PICKUP_ARM_MIN; - import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.geometry.Vector2d; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder; -import org.firstinspires.ftc.teamcode.util.CenterStageCommon; @Autonomous(name = "RedBackStage", preselectTeleOp = "MainTeleOp") public class RedBackStage extends AutoBase { public RedBackStage() { - super( - CenterStageCommon.Alliance.Red, - new Pose2d(12, -63, Math.toRadians(-90)), - new Pose2d(61, -12), - new Pose2d(62, -62)); + super(AutoConfig.builder() + .initialPosition(new Pose2d(12, -63, Math.toRadians(-90))) + .leftParkPosition(new Pose2d(61, -12)) + .rightParkPosition(new Pose2d(62, -62)) + .parkLocation(AutoConfig.ParkLocation.Left) + .build()); } + @Override protected void propLeft() { - TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); builder.lineToLinearHeading(new Pose2d(18.25, -33.5, Math.toRadians(0))); this.robot.getDrive().followTrajectorySequence(builder.build()); @@ -29,17 +27,17 @@ public class RedBackStage extends AutoBase { openAndLiftClaw(); } + @Override protected void propCenter() { - TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToLinearHeading(new Pose2d(12, -42, initialPosition.getHeading())); + builder.lineToLinearHeading(new Pose2d(12, -42, this.config.getInitialPosition().getHeading())); this.robot.getDrive().followTrajectorySequence(builder.build()); openAndLiftClaw(); } + @Override protected void propRight() { - TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); builder.lineToLinearHeading(new Pose2d(15.9, -41.35, Math.toRadians(244.15))); this.robot.getDrive().followTrajectorySequence(builder.build()); diff --git a/TeamCode/src/main/java/opmodes/RedFrontStage.java b/TeamCode/src/main/java/opmodes/RedFrontStage.java index 5579422..789df7a 100644 --- a/TeamCode/src/main/java/opmodes/RedFrontStage.java +++ b/TeamCode/src/main/java/opmodes/RedFrontStage.java @@ -1,28 +1,24 @@ package opmodes; -import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PICKUP_ARM_MIN; - import com.acmerobotics.roadrunner.geometry.Pose2d; -import com.acmerobotics.roadrunner.geometry.Vector2d; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder; -import org.firstinspires.ftc.teamcode.util.CenterStageCommon; @Autonomous(name = "RedFrontStage", preselectTeleOp = "MainTeleOp") public class RedFrontStage extends AutoBase { public RedFrontStage() { - super( - CenterStageCommon.Alliance.Red, - new Pose2d(-36, -63, Math.toRadians(-90)), - new Pose2d(61, -12), - new Pose2d(62, -62)); - + super(AutoConfig.builder() + .initialPosition(new Pose2d(-36, -63, Math.toRadians(-90))) + .leftParkPosition(new Pose2d(61, -12)) + .rightParkPosition(new Pose2d(62, -62)) + .parkLocation(AutoConfig.ParkLocation.Right) + .build()); } + @Override protected void propLeft() { - TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); builder.lineToLinearHeading(new Pose2d(-40.46, -41.93, Math.toRadians(291.8))); this.robot.getDrive().followTrajectorySequence(builder.build()); @@ -30,21 +26,29 @@ public class RedFrontStage extends AutoBase { openAndLiftClaw(); } + @Override protected void propCenter() { - TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToLinearHeading(new Pose2d(-36, -42, initialPosition.getHeading())); + builder.lineToLinearHeading(new Pose2d(-36, -42, this.config.getInitialPosition().getHeading())); this.robot.getDrive().followTrajectorySequence(builder.build()); openAndLiftClaw(); } + @Override protected void propRight() { - TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); builder.lineToLinearHeading(new Pose2d(-41.82, -33.68, Math.toRadians(180))); this.robot.getDrive().followTrajectorySequence(builder.build()); openAndLiftClaw(); } + + @Override + protected void moveBackstage() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToSplineHeading(new Pose2d(-40, -60, Math.PI)); + builder.lineToLinearHeading(new Pose2d(12, -60, Math.PI)); + this.robot.getDrive().followTrajectorySequence(builder.build()); + } }