From 142256c19cc5fcc1b273733334a64884f161a075 Mon Sep 17 00:00:00 2001
From: Scott Barnes <scott@tearabite.com>
Date: Sat, 18 Nov 2023 15:35:16 -0600
Subject: [PATCH] Working auto with park

---
 TeamCode/src/main/java/opmodes/AutoBase.java  | 39 +++++++++++--------
 .../{LeftAuto.java => BlueFrontStage.java}    |  8 ++--
 2 files changed, 27 insertions(+), 20 deletions(-)
 rename TeamCode/src/main/java/opmodes/{LeftAuto.java => BlueFrontStage.java} (74%)

diff --git a/TeamCode/src/main/java/opmodes/AutoBase.java b/TeamCode/src/main/java/opmodes/AutoBase.java
index 8ae0c69..6d09d86 100644
--- a/TeamCode/src/main/java/opmodes/AutoBase.java
+++ b/TeamCode/src/main/java/opmodes/AutoBase.java
@@ -34,11 +34,13 @@ public abstract class AutoBase extends LinearOpMode {
     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) {
+    protected AutoBase(CenterStageCommon.Alliance alliance, Pose2d initialPosition, Pose2d rendezvous, Pose2d park) {
         this.alliance = alliance;
         this.initialPosition = initialPosition;
         this.rendezvous = rendezvous;
+        this.park = park;
     }
 
     @Override
@@ -86,8 +88,7 @@ public abstract class AutoBase extends LinearOpMode {
         moveToBackstage();
         prepareToScore();
         scorePreloadedPixel();
-
-        // TODO Tommy: Park
+        park();
     }
 
     private void dislodgePropAndPlacePixelLeft() {
@@ -102,8 +103,17 @@ public abstract class AutoBase extends LinearOpMode {
         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)));
@@ -166,17 +176,6 @@ public abstract class AutoBase extends LinearOpMode {
         this.robot.getDrive().followTrajectorySequence(builder.build());
     }
 
-    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 determinePropLocation() {
         setPropLocationIfVisible(Center, Unknown);
@@ -209,4 +208,12 @@ public abstract class AutoBase extends LinearOpMode {
             this.propLocation = ifNotVisible;
         }
     }
+
+    public void park() {
+        double currentX = this.robot.getDrive().getPoseEstimate().getX();
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        builder.strafeTo(new Vector2d(currentX, park.getY()));
+        builder.lineToConstantHeading(park.vec());
+        this.robot.getDrive().followTrajectorySequence(builder.build());
+    }
 }
diff --git a/TeamCode/src/main/java/opmodes/LeftAuto.java b/TeamCode/src/main/java/opmodes/BlueFrontStage.java
similarity index 74%
rename from TeamCode/src/main/java/opmodes/LeftAuto.java
rename to TeamCode/src/main/java/opmodes/BlueFrontStage.java
index 2af6b6e..f0fc919 100644
--- a/TeamCode/src/main/java/opmodes/LeftAuto.java
+++ b/TeamCode/src/main/java/opmodes/BlueFrontStage.java
@@ -1,18 +1,18 @@
 package opmodes;
 
 import com.acmerobotics.roadrunner.geometry.Pose2d;
-import com.acmerobotics.roadrunner.geometry.Vector2d;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 
 import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
 
 @Autonomous(name = "Left Auto", preselectTeleOp = "MainTeleOp")
-public class LeftAuto extends AutoBase {
-    public LeftAuto() {
+public class BlueFrontStage extends AutoBase {
+    public BlueFrontStage() {
         super(
                 CenterStageCommon.Alliance.Blue,
                 new Pose2d(-36, 63, Math.toRadians(-90)),
-                new Pose2d(-36, 11));
+                new Pose2d(-36, 11),
+                new Pose2d(62, 12));
     }
 
     @Override