From 2b167a78caff6450a5ea2647ffcebfe7e8dd2466 Mon Sep 17 00:00:00 2001
From: Thomas <@TOMMY>
Date: Fri, 1 Dec 2023 07:57:36 -0600
Subject: [PATCH] Claw, Gantry, and Lift

---
 .../src/main/java/opmodes/RedBackStage.java   | 61 +++++++++++++++++++
 .../src/main/java/opmodes/RedFrontStage.java  |  2 +-
 2 files changed, 62 insertions(+), 1 deletion(-)
 create mode 100644 TeamCode/src/main/java/opmodes/RedBackStage.java

diff --git a/TeamCode/src/main/java/opmodes/RedBackStage.java b/TeamCode/src/main/java/opmodes/RedBackStage.java
new file mode 100644
index 0000000..f544091
--- /dev/null
+++ b/TeamCode/src/main/java/opmodes/RedBackStage.java
@@ -0,0 +1,61 @@
+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 {
+    private final Pose2d rendezvous = new Pose2d(12, 11);
+
+    public RedBackStage() {
+        super(
+                CenterStageCommon.Alliance.Red,
+                new Pose2d(12, 63, Math.toRadians(-90)),
+                new Pose2d(62, -62));
+    }
+
+    protected void propLeft() {
+        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();
+    }
+
+    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(36, 25, Math.toRadians(-33)));
+        builder.addDisplacementMarker(10, () -> {
+            this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
+        });
+        this.robot.getDrive().followTrajectorySequence(builder.build());
+
+        openAndLiftClaw();
+    }
+}
+
diff --git a/TeamCode/src/main/java/opmodes/RedFrontStage.java b/TeamCode/src/main/java/opmodes/RedFrontStage.java
index 29d3e4b..2f656d1 100644
--- a/TeamCode/src/main/java/opmodes/RedFrontStage.java
+++ b/TeamCode/src/main/java/opmodes/RedFrontStage.java
@@ -47,7 +47,7 @@ public class RedFrontStage extends AutoBase {
         openAndLiftClaw();
 
         builder = this.robot.getTrajectorySequenceBuilder();
-        builder.turn(Math.toRadians(90));
+        builder.turn(Math.toRadians(-90));
         this.robot.getDrive().followTrajectorySequence(builder.build());
     }