NOT WORKING indexed auto WIP

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Indexed.java

@@ -1,51 +1,51 @@
 package org.firstinspires.ftc.teamcode.autonomous;
 
 import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
-import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bHGate;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bHGate;
-import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bShoot1Tangent;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bShoot1Tangent;
-import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bShootH;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bShootH;
-import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bShootX;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bShootX;
-import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bShootY;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bShootY;
-import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bXGate;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bXGate;
-import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bYGate;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bYGate;
-import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bh1;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bh1;
-import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bh2a;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bh2a;
-import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bh2b;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bh2b;
-import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bh2c;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bh2c;
-import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bt2a;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bt2a;
-import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bt2b;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bt2b;
-import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bt2c;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bt2c;
-import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bx1;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bx1;
-import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bx2a;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bx2a;
-import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bx2b;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bx2b;
-import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bx2c;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bx2c;
-import static org.firstinspires.ftc.teamcode.constants.Poses_V2.by1;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.by1;
-import static org.firstinspires.ftc.teamcode.constants.Poses_V2.by2a;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.by2a;
-import static org.firstinspires.ftc.teamcode.constants.Poses_V2.by2b;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.by2b;
-import static org.firstinspires.ftc.teamcode.constants.Poses_V2.by2c;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.by2c;
-import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rHGate;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rHGate;
-import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rShoot1Tangent;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rShoot1Tangent;
-import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rShootH;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rShootH;
-import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rShootX;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rShootX;
-import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rShootY;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rShootY;
-import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rXGate;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rXGate;
-import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rYGate;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rYGate;
-import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rh1;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rh1;
-import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rh2a;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rh2a;
-import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rh2b;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rh2b;
-import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rh2c;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rh2c;
-import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rt2a;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rt2a;
-import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rt2b;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rt2b;
-import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rt2c;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rt2c;
-import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rx1;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rx1;
-import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rx2a;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rx2a;
-import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rx2b;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rx2b;
-import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rx2c;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rx2c;
-import static org.firstinspires.ftc.teamcode.constants.Poses_V2.ry1;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.ry1;
-import static org.firstinspires.ftc.teamcode.constants.Poses_V2.ry2a;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.ry2a;
-import static org.firstinspires.ftc.teamcode.constants.Poses_V2.ry2b;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.ry2b;
-import static org.firstinspires.ftc.teamcode.constants.Poses_V2.ry2c;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.ry2c;
-import static org.firstinspires.ftc.teamcode.constants.Poses_V2.teleStart;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.teleStart;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
 
@@ -58,6 +58,7 @@ import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
 import com.acmerobotics.roadrunner.Action;
 import com.acmerobotics.roadrunner.ParallelAction;
 import com.acmerobotics.roadrunner.Pose2d;
+import com.acmerobotics.roadrunner.SequentialAction;
 import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
 import com.acmerobotics.roadrunner.TranslationalVelConstraint;
 import com.acmerobotics.roadrunner.Vector2d;
@@ -74,6 +75,9 @@ import org.firstinspires.ftc.teamcode.utils.Spindexer;
 import org.firstinspires.ftc.teamcode.utils.Targeting;
 import org.firstinspires.ftc.teamcode.utils.Turret;
 
+import java.util.ArrayList;
+import java.util.List;
+
 @Autonomous(preselectTeleOp = "TeleopV3")
 @Config
 public class Auto_LT_Indexed extends LinearOpMode {
@@ -93,11 +97,14 @@ public class Auto_LT_Indexed extends LinearOpMode {
     public static double shoot0Time = 1.6;
     public static double intake1Time = 3.0;
     public static double flywheel0Time = 3.5;
-    public static double pickup1Speed = 80.0;
+    public static double pickup1Speed = 20.0;
+    public static double pickup2Speed = 20.0;
+    public static double pickup3Speed = 20.0;
+
     // ---- SECOND SHOT / PICKUP ----
     public static double shoot1Vel = 2300;
     public static double shoot1Hood = 0.93;
-    
+
     public static double shootAllVelocity = 2500;
     public static double shootAllHood = 0.78;
     // ---- PICKUP TIMING ----
@@ -113,6 +120,11 @@ public class Auto_LT_Indexed extends LinearOpMode {
     public static double shoot1ToleranceY = 2.0;
     public static double shoot1Time = 2.0;
     public static double shootCycleTime = 2.5;
+
+    List<Boolean> s1 = new ArrayList<>();
+
+    List<Boolean> s2 = new ArrayList<>();
+    List<Boolean> s3 = new ArrayList<>();
     public int motif = 0;
     Robot robot;
     MultipleTelemetry TELE;
@@ -135,11 +147,32 @@ public class Auto_LT_Indexed extends LinearOpMode {
 
     private double shoot1Tangent;
 
-    public Action prepareShootAll(double time) {
+    // ---- OPEN GATE / MIDFIELD ----
+    private double x3, y3, h3, t3;
+
+    // ---- PICKUP 2 ----
+    private double x4a, y4a, h4a;
+    private double x4b, y4b, h4b;
+
+    // ---- PICKUP 3 ----
+    private double x5a, y5a, h5a;
+    private double x5b, y5b, h5b;
+
+    // ---- PARK ----
+    private double xPark, yPark, hPark;
+
+    public Action prepareShootIndexed(double time) {
         return new Action() {
             double stamp = 0.0;
             int ticker = 0;
 
+            boolean testColor = false;
+
+            int s1Green = 0;
+            int s2Green = 0;
+            int s3Green = 0;
+
+
             @Override
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
                 if (ticker == 0) {
@@ -147,8 +180,56 @@ public class Auto_LT_Indexed extends LinearOpMode {
                 }
                 ticker++;
 
-                robot.spin1.setPosition(autoSpinStartPos);
+                if ((System.currentTimeMillis() - stamp) < (goToDetectTime)) { //0.25-0.4 ish???
-                robot.spin2.setPosition(1 - autoSpinStartPos);
+                    robot.spin1.setPosition(colorDetectPos); //0.43
+                    robot.spin2.setPosition(1 - colorDetectPos);
+                } else if ((System.currentTimeMillis() - stamp) < (colorSenseTime + goToDetectTime)) {
+
+
+                    double green1 = robot.color1.getNormalizedColors().green;
+                    double red1 = robot.color1.getNormalizedColors().red;
+                    double blue1 = robot.color1.getNormalizedColors().blue;
+
+                    double gP1 = green1 / (green1 + red1 + blue1);
+
+
+                    if (gP1 >= color1Thresh) {
+                        s1Green ++;
+                    }
+
+                    double green2 = robot.color2.getNormalizedColors().green;
+                    double red2 = robot.color2.getNormalizedColors().red;
+                    double blue2 = robot.color2.getNormalizedColors().blue;
+
+                    double gP2 = green2 / (green2 + red2 + blue2);
+
+                    if (gP2 >= color2Thresh) {
+                        s2Green ++;
+                    }
+
+
+                    double green3 = robot.color3.getNormalizedColors().green;
+                    double red3 = robot.color3.getNormalizedColors().red;
+                    double blue3 = robot.color3.getNormalizedColors().blue;
+
+                    double gP3 = green3 / (green3 + red3 + blue3);
+
+                    if (gP3 >= color3Thresh) {
+                        s3Green ++;
+                    }
+
+
+                    testColor = true;
+
+
+
+                } else {
+
+                    double spindexPos;
+                    if (motif == 21) {
+                        spindexPos =
+                    }
+                }
 
                 robot.transferServo.setPosition(transferServo_out);
 
@@ -301,7 +382,7 @@ public class Auto_LT_Indexed extends LinearOpMode {
                 robot.intake.setPower(1);
 
                 motif = turret.detectObelisk();
-                
+
                 spindexer.ballCounterLight();
 
                 return (System.currentTimeMillis() - stamp) < (intakeTime * 1000);
@@ -513,7 +594,6 @@ public class Auto_LT_Indexed extends LinearOpMode {
                 targetingSettings = targeting.calculateSettings
                         (robotX, robotY, robotHeading, 0.0, false);
 
-
                 robot.hood.setPosition(targetingSettings.hoodAngle);
 
                 flywheel.manageFlywheel(targetingSettings.flywheelRPM);
@@ -559,10 +639,15 @@ public class Auto_LT_Indexed extends LinearOpMode {
         drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
 
         TrajectoryActionBuilder shoot0 = null;
+
         TrajectoryActionBuilder pickup1 = null;
         TrajectoryActionBuilder shoot1 = null;
-        TrajectoryActionBuilder gatePickup = null;
+        TrajectoryActionBuilder pickup2 = null;
-        TrajectoryActionBuilder shootCycle = null;
+        TrajectoryActionBuilder shoot2 = null;
+        TrajectoryActionBuilder pickup3 = null;
+        TrajectoryActionBuilder shoot3 = null;
+        TrajectoryActionBuilder openGate = null;
+        TrajectoryActionBuilder park = null;
 
         robot.spin1.setPosition(autoSpinStartPos);
         robot.spin2.setPosition(1 - autoSpinStartPos);
@@ -582,63 +667,69 @@ public class Auto_LT_Indexed extends LinearOpMode {
             if (redAlliance) {
                 robot.light.setPosition(0.28);
 
-                // ---- FIRST SHOT ----
+                // ===== FIRST SHOT =====
                 x1 = rx1;
                 y1 = ry1;
                 h1 = rh1;
 
-                // ---- PICKUP PATH ----
+                // ===== PICKUP PATH =====
                 x2a = rx2a;
                 y2a = ry2a;
                 h2a = rh2a;
                 t2a = rt2a;
+
                 x2b = rx2b;
                 y2b = ry2b;
                 h2b = rh2b;
                 t2b = rt2b;
+
                 x2c = rx2c;
                 y2c = ry2c;
                 h2c = rh2c;
                 t2c = rt2c;
 
+                // ===== SHOOT POSE =====
                 xShoot = rShootX;
                 yShoot = rShootY;
                 hShoot = rShootH;
                 shoot1Tangent = rShoot1Tangent;
-                
+
+                // ===== GATE =====
                 xGate = rXGate;
                 yGate = rYGate;
                 hGate = rHGate;
-                
 
             } else {
                 robot.light.setPosition(0.6);
 
-                // ---- FIRST SHOT ----
+                // ===== FIRST SHOT =====
                 x1 = bx1;
                 y1 = by1;
                 h1 = bh1;
 
-                // ---- PICKUP PATH ----
+                // ===== PICKUP PATH =====
                 x2a = bx2a;
                 y2a = by2a;
                 h2a = bh2a;
                 t2a = bt2a;
+
                 x2b = bx2b;
                 y2b = by2b;
                 h2b = bh2b;
                 t2b = bt2b;
+
                 x2c = bx2c;
                 y2c = by2c;
                 h2c = bh2c;
                 t2c = bt2c;
 
+                // ===== SHOOT POSE =====
                 xShoot = bShootX;
                 yShoot = bShootY;
                 hShoot = bShootH;
-
                 shoot1Tangent = bShoot1Tangent;
 
+                // ===== GATE =====
                 xGate = bXGate;
                 yGate = bYGate;
                 hGate = bHGate;
@@ -651,17 +742,30 @@ public class Auto_LT_Indexed extends LinearOpMode {
                     .strafeToLinearHeading(new Vector2d(x2a, y2a), h2a)
                     .strafeToLinearHeading(new Vector2d(x2b, y2b), h2b,
                             new TranslationalVelConstraint(pickup1Speed));
-            shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, h2b))
-                    .setReversed(true)
-                    .splineTo(new Vector2d(x2a, y2a), shoot1Tangent)
-                    .splineToSplineHeading(new Pose2d(xShoot, yShoot, hShoot), shoot1Tangent);
-            
-            gatePickup = drive.actionBuilder(new Pose2d(xShoot, yShoot, hShoot))
-                    .strafeToLinearHeading(new Vector2d(xGate, yGate), hGate);
 
-            shootCycle = drive.actionBuilder(new Pose2d(xGate, yGate, hGate))
+            openGate = drive.actionBuilder(new Pose2d(x2b, y2b, h2b))
+                    .setReversed(true)
+                    .splineToConstantHeading(new Vector2d(x3, y3), t3);
+
+            shoot1 = drive.actionBuilder(new Pose2d(x3, y3, h2b))
                     .strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot);
 
+            pickup2 = drive.actionBuilder(new Pose2d(xShoot, yShoot, hShoot))
+                    .strafeToLinearHeading(new Vector2d(x4a, y4a), h4a)
+                    .strafeToLinearHeading(new Vector2d(x4b, y4b), h4b,
+                            new TranslationalVelConstraint(pickup2Speed));
+            shoot2 = drive.actionBuilder(new Pose2d(x4b, y4b, h4b))
+                    .strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot);
+
+            pickup3 = drive.actionBuilder(new Pose2d(xShoot, yShoot, hShoot))
+                    .strafeToLinearHeading(new Vector2d(x5a, y5a), h5a)
+                    .strafeToLinearHeading(new Vector2d(x5b, y5b), h5b,
+                            new TranslationalVelConstraint(pickup3Speed));
+            shoot3 = drive.actionBuilder(new Pose2d(x5b, y5b, h5b))
+                    .strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot);
+            park = drive.actionBuilder(new Pose2d(xShoot, yShoot, hShoot))
+                    .strafeToLinearHeading(new Vector2d(xPark, yPark), hPark);
+
         }
 
         waitForStart();
@@ -694,36 +798,46 @@ public class Auto_LT_Indexed extends LinearOpMode {
                     shootAll((int) shoot0Vel, shoot0Time, shoot0SpinSpeedIncrease)
             );
 
+            //WORKING SHOOT ALL for the preload
+
+            //Pickup first pile
+
             Actions.runBlocking(
-                    new ParallelAction(
+                    new SequentialAction(
-                            pickup1.build(),
+                            new ParallelAction(
-                            manageFlywheel(
+                                    pickup1.build(),
-                                    shootAllVelocity, 
+                                    manageFlywheel(
-                                    shootAllHood,
+                                            shootAllVelocity,
-                                    pickup1Time,
+                                            shootAllHood,
-                                    x2b,
+                                            pickup1Time,
-                                    y2b,
+                                            x2b,
-                                    pickup1XTolerance,
+                                            y2b,
-                                    pickup1YTolerance
+                                            pickup1XTolerance,
+                                            pickup1YTolerance
+                                    ),
+                                    intake(intake1Time),
+                                    detectObelisk(
+                                            obelisk1Time,
+                                            x2b,
+                                            y2c,
+                                            obelisk1XTolerance,
+                                            obelisk1YTolerance,
+                                            obeliskTurrPos
+                                    )
                             ),
-                            intake(intake1Time),
+                            new ParallelAction(
-                            detectObelisk(
+                                    openGate.build() //TODO: Add other managing stuff here
-                                    obelisk1Time,
-                                    x2b,
-                                    y2c,
-                                    obelisk1XTolerance,
-                                    obelisk1YTolerance,
-                                    obeliskTurrPos
                             )
                     )
+
             );
 
-            motif = turret.detectObelisk();
+            motif = turret.detectObelisk(); //Detect Obelisk if not alr
 
             Actions.runBlocking(
                     new ParallelAction(
                             manageFlywheel(
-                                    shootAllVelocity, 
+                                    shootAllVelocity,
                                     shootAllHood,
                                     shoot1Time,
                                     0.501,
@@ -732,7 +846,7 @@ public class Auto_LT_Indexed extends LinearOpMode {
                                     0.501
                             ),
                             shoot1.build(),
-                            prepareShootAll(shoot1Time)
+                            prepareShootIndexed(shoot1Time)
                     )
             );
 
@@ -745,58 +859,12 @@ public class Auto_LT_Indexed extends LinearOpMode {
                                     0.501,
                                     0.501
                             ),
-                            shootAllAuto(shootAllTime, spindexerSpeedIncrease)
+                            shootAllIndexed(shootAllTime, spindexerSpeedIncrease)
                     )
 
             );
 
-            while (opModeIsActive()) {
+            //Shoot from first pile
-
-                Actions.runBlocking(
-                        new ParallelAction(
-                                gatePickup.build(),
-                                manageShooterAuto(
-                                        gatePickupTime,
-                                        x2b,
-                                        y2b,
-                                        pickup1XTolerance,
-                                        pickup1YTolerance
-                                ),
-                                intake(gatePickupTime)
-
-                        )
-                );
-
-                Actions.runBlocking(
-                        new ParallelAction(
-                                manageFlywheelAuto(
-                                        shootCycleTime,
-                                        0.501,
-                                        0.501,
-                                        0.501,
-                                        0.501
-                                ),
-                                shootCycle.build(),
-                                prepareShootAll(shootCycleTime)
-                        )
-                );
-
-                Actions.runBlocking(
-                        new ParallelAction(
-                                manageShooterAuto(
-                                        shootAllTime,
-                                        0.501,
-                                        0.501,
-                                        0.501,
-                                        0.501
-                                ),
-                                shootAllAuto(shootAllTime, spindexerSpeedIncrease)
-                        )
-
-                );
-
-            }
-
 
         }
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses_LT_Indexed.java

@@ -0,0 +1,86 @@
+package org.firstinspires.ftc.teamcode.constants;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.Pose2d;
+
+@Config
+public class Poses_LT_Indexed {
+
+    // ================= FIELD / GOAL =================
+    public static double goalHeight = 42; // inches
+    public static double turretHeight = 12;
+    public static double relativeGoalHeight = goalHeight - turretHeight;
+
+    public static Pose2d goalPose = new Pose2d(-10, 0, 0);
+    public static Pose2d teleStart = new Pose2d(0, 0, 0);
+
+    // =================================================
+    // ================= RED ALLIANCE ==================
+    // =================================================
+
+    // -------- FIRST SHOT --------
+    public static double rx1 = 20, ry1 = 0, rh1 = 0;
+
+    // -------- PICKUP 1 --------
+    public static double rx2a = 55, ry2a = 39, rh2a = Math.toRadians(140), rt2a = Math.toRadians(Math.PI / 2);
+    public static double rx2b = 33, ry2b = 61, rh2b = Math.toRadians(140), rt2b = Math.toRadians(Math.PI / 2);
+    public static double rx2c = 34, ry2c = 50, rh2c = Math.toRadians(140), rt2c = Math.toRadians(Math.PI / 2);
+
+    // -------- OPEN GATE --------
+    public static double rXGate = 30, rYGate = 63, rHGate = Math.toRadians(179);
+
+    // -------- PICKUP 2 --------
+    public static double rx3 = 0, ry3 = 0, rh3 = 0, rt3 = 0;
+
+    public static double rx3a = 55, ry3a = 39, rh3a = Math.toRadians(140);
+    public static double rx3b = 33, ry3b = 61, rh3b = Math.toRadians(140);
+
+    // -------- PICKUP 3 --------
+    public static double rx4a = 72, ry4a = 55, rh4a = Math.toRadians(140);
+    public static double rx4b = 48, ry4b = 79, rh4b = Math.toRadians(140);
+
+    public static double rx5a = 0, ry5a = 0, rh5a = 0;
+    public static double rx5b = 0, ry5b = 0, rh5b = 0;
+
+    // -------- SHOOT --------
+    public static double rShootX = 40, rShootY = 7, rShootH = Math.toRadians(140);
+    public static double rShoot1Tangent = Math.toRadians(0);
+
+    // -------- PARK --------
+    public static double rXPark = 0, rYPark = 0, rHPark = 0;
+
+    // =================================================
+    // ================= BLUE ALLIANCE =================
+    // =================================================
+
+    // -------- FIRST SHOT --------
+    public static double bx1 = 20, by1 = 0, bh1 = 0;
+
+    // -------- PICKUP 1 --------
+    public static double bx2a = 45, by2a = -18, bh2a = Math.toRadians(-140), bt2a = Math.toRadians(140);
+    public static double bx2b = 25, by2b = -38, bh2b = Math.toRadians(-140), bt2b = Math.toRadians(140);
+    public static double bx2c = 34, by2c = -50, bh2c = Math.toRadians(-140), bt2c = Math.toRadians(140);
+
+    // -------- OPEN GATE --------
+    public static double bXGate = 25, bYGate = 69, bHGate = Math.toRadians(165);
+
+    // -------- PICKUP 2 --------
+    public static double bx3 = 0, by3 = 0, bh3 = 0, bt3 = 0;
+
+    public static double bx3a = 55, by3a = -43, bh3a = Math.toRadians(-140);
+    public static double bx3b = 37, by3b = -61, bh3b = Math.toRadians(-140);
+
+    // -------- PICKUP 3 --------
+    public static double bx4a = 72, by4a = -55, bh4a = Math.toRadians(-140);
+    public static double bx4b = 48, by4b = -79, bh4b = Math.toRadians(-140);
+
+    public static double bx5a = 0, by5a = 0, bh5a = 0;
+    public static double bx5b = 0, by5b = 0, bh5b = 0;
+
+    // -------- SHOOT --------
+    public static double bShootX = 20, bShootY = 30, bShootH = Math.toRadians(140);
+    public static double bShoot1Tangent = Math.toRadians(0);
+
+    // -------- PARK --------
+    public static double bXPark = 0, bYPark = 0, bHPark = 0;
+}