Working BlueBackstage auto

TeamCode/src/main/java/opmodes/AutoBase.java

@@ -25,7 +25,7 @@ public abstract class AutoBase extends LinearOpMode {
     public static int DEPOSIT_HEIGHT = 100;
     public static double SCORING_DURATION_S =  5f;
     public static double APRIL_TAG_RIGHT_DELTA = -8.5;
-    public static double APRIL_TAG_LEFT_DELTA = 8.0;
+    public static double APRIL_TAG_LEFT_DELTA = 7.0;
 
     protected Robot robot;
     protected FtcDashboard dashboard;
@@ -33,13 +33,11 @@ public abstract class AutoBase extends LinearOpMode {
     protected CenterStageCommon.PropLocation propLocation;
     protected final Pose2d initialPosition;
     protected final CenterStageCommon.Alliance alliance;
-    protected final Pose2d rendezvous;
     protected final Pose2d park;
 
-    protected AutoBase(CenterStageCommon.Alliance alliance, Pose2d initialPosition, Pose2d rendezvous, Pose2d park) {
+    protected AutoBase(CenterStageCommon.Alliance alliance, Pose2d initialPosition, Pose2d park) {
         this.alliance = alliance;
         this.initialPosition = initialPosition;
-        this.rendezvous = rendezvous;
         this.park = park;
     }
 
@@ -56,32 +54,24 @@ public abstract class AutoBase extends LinearOpMode {
             this.sleep(20);
         }
 
+        if (isStopRequested()) {
+            return;
+        }
+
         // If the prop is visible at this point, then it must be in the center (2) position
         determinePropLocation();
 
         TrajectorySequenceBuilder builder;
         switch (this.propLocation) {
             case Left:
-                dislodgePropAndPlacePixelLeft();
+                propLeft();
-
-                builder = this.robot.getTrajectorySequenceBuilder();
-                builder.lineToLinearHeading(rendezvous);
-                this.robot.getDrive().followTrajectorySequence(builder.build());
                 break;
             case Unknown:
             case Center:
-                dislodgePropAndPlacePixelCenter();
+                propCenter();
-
-                builder = this.robot.getTrajectorySequenceBuilder();
-                builder.turn(Math.toRadians(90));
-                this.robot.getDrive().followTrajectorySequence(builder.build());
                 break;
             case Right:
-                dislodgePropAndPlacePixelRight();
+                propRight();
-
-                builder = this.robot.getTrajectorySequenceBuilder();
-                builder.lineToLinearHeading(this.rendezvous);
-                this.robot.getDrive().followTrajectorySequence(builder.build());
                 break;
         }
 
@@ -91,42 +81,19 @@ public abstract class AutoBase extends LinearOpMode {
         park();
     }
 
-    private void dislodgePropAndPlacePixelLeft() {
+    protected abstract void propLeft();
-        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+
-        builder.lineToLinearHeading(new Pose2d(-52, 31, Math.toRadians(-180)));
+    protected abstract void propCenter();
-        builder.lineToConstantHeading(new Vector2d(-42, 31));
+
-        builder.addTemporalMarker(0.2, () -> {
+    protected abstract void propRight();
-            this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
+
-        });
+    protected void openAndLiftClaw() {
-        this.robot.getDrive().followTrajectorySequence(builder.build());
-        this.robot.getClaw().openSync();
-        this.sleep(100);
-        this.robot.getClaw().setArmPosition(PICKUP_ARM_MAX);
-    }
-    private void dislodgePropAndPlacePixelCenter() {
-        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
-        builder.lineToConstantHeading(rendezvous.vec());
-        builder.addDisplacementMarker(10, () -> {
-            this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
-        });
-        this.robot.getDrive().followTrajectorySequence(builder.build());
-        this.robot.getClaw().openSync();
-        this.sleep(100);
-        this.robot.getClaw().setArmPosition(PICKUP_ARM_MAX);
-    }
-    private void dislodgePropAndPlacePixelRight() {
-        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
-        builder.lineToLinearHeading(new Pose2d(-54, 17, Math.toRadians(-123)));
-        builder.addDisplacementMarker(10, () -> {
-            this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
-        });
-        this.robot.getDrive().followTrajectorySequence(builder.build());
         this.robot.getClaw().openSync();
         this.sleep(100);
         this.robot.getClaw().setArmPosition(PICKUP_ARM_MAX);
     }
 
-    private void scorePreloadedPixel() {
+    protected void scorePreloadedPixel() {
         this.robot.getGantry().setSlideTarget(DEPOSIT_HEIGHT);
         this.robot.getGantry().armOut();
         while(this.robot.getGantry().isIn()) {
@@ -144,7 +111,7 @@ public abstract class AutoBase extends LinearOpMode {
 
     }
 
-    private void prepareToScore() {
+    protected void prepareToScore() {
         // At this point we know that Y = 38
         // For 2 -> Ydelta = 0
         // For 3 -> 3 5/8
@@ -169,22 +136,21 @@ public abstract class AutoBase extends LinearOpMode {
         this.robot.getDrive().followTrajectorySequence(builder.build());
     }
 
-    private void moveToBackstage() {
+    protected void moveToBackstage() {
         TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
         builder.lineToLinearHeading(new Pose2d(36, 11, 0));
         builder.lineToLinearHeading(new Pose2d(36, 38, 0));
         this.robot.getDrive().followTrajectorySequence(builder.build());
     }
 
-
+    protected void determinePropLocation() {
-    private void determinePropLocation() {
         setPropLocationIfVisible(Center, Unknown);
         if (this.propLocation != Center) {
             peekRight();
         }
     }
 
-    private void peekRight() {
+    protected void peekRight() {
         TrajectorySequenceBuilder builder = this.robot.getDrive()
                 .trajectorySequenceBuilder(initialPosition);
         builder.forward(5);

TeamCode/src/main/java/opmodes/BlueBackStage.java

@@ -0,0 +1,60 @@
+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 {
+    private final Pose2d rendezvous = new Pose2d(12, 11);
+
+    public BlueBackStage() {
+        super(
+                CenterStageCommon.Alliance.Blue,
+                new Pose2d(12, 63, Math.toRadians(-90)),
+                new Pose2d(62, 62));
+    }
+
+    protected void propLeft() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        builder.lineToLinearHeading(new Pose2d(36, 25, Math.toRadians(-33)));
+        builder.addDisplacementMarker(10, () -> {
+            this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
+        });
+        this.robot.getDrive().followTrajectorySequence(builder.build());
+
+        openAndLiftClaw();
+    }
+
+    protected void propCenter() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        builder.lineToConstantHeading(rendezvous.vec());
+        builder.addDisplacementMarker(10, () -> {
+            this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
+        });
+        this.robot.getDrive().followTrajectorySequence(builder.build());
+
+        openAndLiftClaw();
+
+        builder = this.robot.getTrajectorySequenceBuilder();
+        builder.turn(Math.toRadians(90));
+        this.robot.getDrive().followTrajectorySequence(builder.build());
+    }
+
+    protected void propRight() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        builder.lineToLinearHeading(new Pose2d(32, 34, Math.toRadians(0)));
+        builder.lineToConstantHeading(new Vector2d(19, 34));
+        builder.addTemporalMarker(0.5, () -> {
+            this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
+        });
+        this.robot.getDrive().followTrajectorySequence(builder.build());
+
+        openAndLiftClaw();
+    }
+}

TeamCode/src/main/java/opmodes/BlueFrontStage.java

@@ -1,22 +1,66 @@
 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 = "Left Auto", preselectTeleOp = "MainTeleOp")
+@Autonomous(name = "BlueFrontStage", preselectTeleOp = "MainTeleOp")
 public class BlueFrontStage extends AutoBase {
+    private final Pose2d rendezvous = new Pose2d(-36, 11);
+
     public BlueFrontStage() {
         super(
                 CenterStageCommon.Alliance.Blue,
                 new Pose2d(-36, 63, Math.toRadians(-90)),
-                new Pose2d(-36, 11),
                 new Pose2d(62, 12));
     }
 
-    @Override
+    protected void propLeft() {
-    public void runOpMode() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
-        super.runOpMode();
+        builder.lineToLinearHeading(new Pose2d(-52, 31, Math.toRadians(-180)));
+        builder.lineToConstantHeading(new Vector2d(-42, 31));
+        builder.addTemporalMarker(0.2, () -> {
+            this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
+        });
+        this.robot.getDrive().followTrajectorySequence(builder.build());
+
+        openAndLiftClaw();
+
+        builder = this.robot.getTrajectorySequenceBuilder();
+        builder.lineToLinearHeading(rendezvous);
+        this.robot.getDrive().followTrajectorySequence(builder.build());
+    }
+    protected void propCenter() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        builder.lineToConstantHeading(rendezvous.vec());
+        builder.addDisplacementMarker(10, () -> {
+            this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
+        });
+        this.robot.getDrive().followTrajectorySequence(builder.build());
+
+        openAndLiftClaw();
+
+        builder = this.robot.getTrajectorySequenceBuilder();
+        builder.turn(Math.toRadians(90));
+        this.robot.getDrive().followTrajectorySequence(builder.build());
+    }
+    protected void propRight() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        builder.lineToLinearHeading(new Pose2d(-54, 17, Math.toRadians(-123)));
+        builder.addDisplacementMarker(10, () -> {
+            this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
+        });
+        this.robot.getDrive().followTrajectorySequence(builder.build());
+
+        openAndLiftClaw();
+
+        builder = this.robot.getTrajectorySequenceBuilder();
+        builder.lineToLinearHeading(this.rendezvous);
+        this.robot.getDrive().followTrajectorySequence(builder.build());
     }
 }