NOT WORKING indexed auto WIP

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

@@ -0,0 +1,872 @@
+package org.firstinspires.ftc.teamcode.autonomous;
+
+import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bHGate;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bShoot1Tangent;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bShootH;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bShootX;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bShootY;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bXGate;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bYGate;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bh1;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bh2a;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bh2b;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bh2c;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bt2a;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bt2b;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bt2c;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bx1;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bx2a;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bx2b;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bx2c;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.by1;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.by2a;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.by2b;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.by2c;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rHGate;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rShoot1Tangent;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rShootH;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rShootX;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rShootY;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rXGate;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rYGate;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rh1;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rh2a;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rh2b;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rh2c;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rt2a;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rt2b;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rt2c;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rx1;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rx2a;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rx2b;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rx2c;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.ry1;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.ry2a;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.ry2b;
+import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.ry2c;
+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;
+
+import androidx.annotation.NonNull;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+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;
+import com.acmerobotics.roadrunner.ftc.Actions;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.Servo;
+
+import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
+import org.firstinspires.ftc.teamcode.utils.Flywheel;
+import org.firstinspires.ftc.teamcode.utils.Robot;
+import org.firstinspires.ftc.teamcode.utils.Servos;
+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 {
+
+    public static double shoot0Vel = 2300, shoot0Hood = 0.93;
+    public static double autoSpinStartPos = 0.2;
+    public static double shoot0SpinSpeedIncrease = 0.014;
+
+    public static double spindexerSpeedIncrease = 0.02;
+
+    public static double obeliskTurrPos = 0.53;
+    public static double gatePickupTime = 3.0;
+    public static double shoot1Turr = 0.57;
+    public static double shoot0XTolerance = 1.0;
+    public static double turretShootPos = 0.72;
+    public static double shootAllTime = 1.8;
+    public static double shoot0Time = 1.6;
+    public static double intake1Time = 3.0;
+    public static double flywheel0Time = 3.5;
+    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 ----
+    public static double pickup1Time = 3.0;
+    // ---- PICKUP POSITION TOLERANCES ----
+    public static double pickup1XTolerance = 2.0;
+    public static double pickup1YTolerance = 2.0;
+    // ---- OBELISK DETECTION ----
+    public static double obelisk1Time = 1.5;
+    public static double obelisk1XTolerance = 2.0;
+    public static double obelisk1YTolerance = 2.0;
+    public static double shoot1ToleranceX = 2.0;
+    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;
+    MecanumDrive drive;
+    Servos servos;
+    Spindexer spindexer;
+    Flywheel flywheel;
+    Turret turret;
+    Targeting targeting;
+    Targeting.Settings targetingSettings;
+    private double x1, y1, h1;
+
+    private double x2a, y2a, h2a, t2a;
+
+    private double x2b, y2b, h2b, t2b;
+    private double x2c, y2c, h2c, t2c;
+
+    private double xShoot, yShoot, hShoot;
+    private double xGate, yGate, hGate;
+
+    private double shoot1Tangent;
+
+    // ---- 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) {
+                    stamp = System.currentTimeMillis();
+                }
+                ticker++;
+
+                if ((System.currentTimeMillis() - stamp) < (goToDetectTime)) { //0.25-0.4 ish???
+                    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);
+
+                turret.manualSetTurret(turretShootPos);
+
+                robot.intake.setPower(-((System.currentTimeMillis() - stamp)) / 1000);
+
+                return (System.currentTimeMillis() - stamp) < (time * 1000);
+
+            }
+        };
+    }
+
+    public Action shootAll(int vel, double shootTime, double spindexSpeed) {
+        return new Action() {
+            int ticker = 1;
+
+            double stamp = 0.0;
+
+            double velo = vel;
+
+            int shooterTicker = 0;
+
+            @Override
+            public boolean run(@NonNull TelemetryPacket telemetryPacket) {
+                TELE.addData("Velocity", velo);
+                TELE.addLine("shooting");
+                TELE.update();
+
+                flywheel.manageFlywheel(vel);
+                velo = flywheel.getVelo();
+
+                drive.updatePoseEstimate();
+
+                teleStart = drive.localizer.getPose();
+
+                robot.intake.setPower(-0.3);
+
+                if (ticker == 1) {
+                    stamp = getRuntime();
+                }
+                ticker++;
+
+                robot.intake.setPower(0);
+
+                if (getRuntime() - stamp < shootTime) {
+
+                    if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) {
+                        robot.spin1.setPosition(autoSpinStartPos);
+                        robot.spin2.setPosition(1 - autoSpinStartPos);
+                    } else {
+                        robot.transferServo.setPosition(transferServo_in);
+                        shooterTicker++;
+                        double prevSpinPos = robot.spin1.getPosition();
+                        robot.spin1.setPosition(prevSpinPos + spindexSpeed);
+                        robot.spin2.setPosition(1 - prevSpinPos - spindexSpeed);
+                    }
+
+                    return true;
+
+                } else {
+                    robot.transferServo.setPosition(transferServo_out);
+                    //spindexPos = spindexer_intakePos1;
+
+                    spindexer.resetSpindexer();
+                    spindexer.processIntake();
+
+                    return false;
+
+                }
+
+            }
+        };
+    }
+
+    public Action shootAllAuto(double shootTime, double spindexSpeed) {
+        return new Action() {
+            int ticker = 1;
+
+            double stamp = 0.0;
+
+            double velo = 0.0;
+
+            int shooterTicker = 0;
+
+            @Override
+            public boolean run(@NonNull TelemetryPacket telemetryPacket) {
+                TELE.addData("Velocity", velo);
+                TELE.addLine("shooting");
+                TELE.update();
+
+                velo = flywheel.getVelo();
+
+                drive.updatePoseEstimate();
+
+                teleStart = drive.localizer.getPose();
+
+                robot.intake.setPower(-0.3);
+
+                if (ticker == 1) {
+                    stamp = getRuntime();
+                }
+                ticker++;
+
+                robot.intake.setPower(0);
+
+                if (getRuntime() - stamp < shootTime) {
+
+                    if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) {
+                        robot.spin1.setPosition(autoSpinStartPos);
+                        robot.spin2.setPosition(1 - autoSpinStartPos);
+                    } else {
+                        robot.transferServo.setPosition(transferServo_in);
+                        shooterTicker++;
+                        double prevSpinPos = robot.spin1.getPosition();
+                        robot.spin1.setPosition(prevSpinPos + spindexSpeed);
+                        robot.spin2.setPosition(1 - prevSpinPos - spindexSpeed);
+                    }
+
+                    return true;
+
+                } else {
+                    robot.transferServo.setPosition(transferServo_out);
+                    //spindexPos = spindexer_intakePos1;
+
+                    spindexer.resetSpindexer();
+                    spindexer.processIntake();
+
+                    return false;
+
+                }
+
+            }
+        };
+    }
+
+    public Action intake(double intakeTime) {
+        return new Action() {
+            double stamp = 0.0;
+            int ticker = 0;
+
+            @Override
+            public boolean run(@NonNull TelemetryPacket telemetryPacket) {
+                if (ticker == 0) {
+                    stamp = System.currentTimeMillis();
+                }
+                ticker++;
+
+                spindexer.processIntake();
+                robot.intake.setPower(1);
+
+                motif = turret.detectObelisk();
+
+                spindexer.ballCounterLight();
+
+                return (System.currentTimeMillis() - stamp) < (intakeTime * 1000);
+
+            }
+        };
+    }
+
+    public Action detectObelisk(
+            double time,
+            double posX,
+            double posY,
+            double posXTolerance,
+            double posYTolerance,
+            double turrPos
+    ) {
+
+        boolean timeFallback = (time != 0.501);
+        boolean posXFallback = (posX != 0.501);
+        boolean posYFallback = (posY != 0.501);
+
+        return new Action() {
+
+            double stamp = 0.0;
+            int ticker = 0;
+
+            @Override
+            public boolean run(@NonNull TelemetryPacket telemetryPacket) {
+
+                drive.updatePoseEstimate();
+                Pose2d currentPose = drive.localizer.getPose();
+
+                if (ticker == 0) {
+                    stamp = System.currentTimeMillis();
+                }
+
+                ticker++;
+
+                motif = turret.detectObelisk();
+
+                robot.turr1.setPosition(turrPos);
+                robot.turr2.setPosition(1 - turrPos);
+
+                boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000;
+                boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance;
+                boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance;
+
+                boolean shouldFinish = timeDone || xDone || yDone;
+
+                return !shouldFinish;
+
+            }
+        };
+    }
+
+    public Action manageFlywheel(
+            double vel,
+            double hoodPos,
+            double time,
+            double posX,
+            double posY,
+            double posXTolerance,
+            double posYTolerance
+    ) {
+
+        boolean timeFallback = (time != 0.501);
+        boolean posXFallback = (posX != 0.501);
+        boolean posYFallback = (posY != 0.501);
+
+        return new Action() {
+
+            double stamp = 0.0;
+            int ticker = 0;
+
+            @Override
+            public boolean run(@NonNull TelemetryPacket telemetryPacket) {
+
+                drive.updatePoseEstimate();
+                Pose2d currentPose = drive.localizer.getPose();
+
+                if (ticker == 0) {
+                    stamp = System.currentTimeMillis();
+                }
+
+                ticker++;
+
+                flywheel.manageFlywheel(vel);
+                robot.hood.setPosition(hoodPos);
+
+                boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000;
+                boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance;
+                boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance;
+
+                boolean shouldFinish = timeDone || xDone || yDone;
+
+                return !shouldFinish;
+
+            }
+        };
+    }
+
+    public Action manageShooterAuto(
+            double time,
+            double posX,
+            double posY,
+            double posXTolerance,
+            double posYTolerance
+    ) {
+
+        boolean timeFallback = (time != 0.501);
+        boolean posXFallback = (posX != 0.501);
+        boolean posYFallback = (posY != 0.501);
+
+        return new Action() {
+
+            double stamp = 0.0;
+            int ticker = 0;
+
+            @Override
+            public boolean run(@NonNull TelemetryPacket telemetryPacket) {
+
+                drive.updatePoseEstimate();
+                Pose2d currentPose = drive.localizer.getPose();
+
+                if (ticker == 0) {
+                    stamp = System.currentTimeMillis();
+                }
+
+                ticker++;
+
+                double robotX = drive.localizer.getPose().position.x;
+                double robotY = drive.localizer.getPose().position.y;
+
+                double robotHeading = drive.localizer.getPose().heading.toDouble();
+
+                double goalX = -15;
+                double goalY = 0;
+
+                double dx = robotX - goalX;  // delta x from robot to goal
+                double dy = robotY - goalY;  // delta y from robot to goal
+                Pose2d deltaPose = new Pose2d(dx, dy, robotHeading);
+
+                double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
+
+                targetingSettings = targeting.calculateSettings
+                        (robotX, robotY, robotHeading, 0.0, false);
+
+                turret.trackGoal(deltaPose);
+
+                robot.hood.setPosition(targetingSettings.hoodAngle);
+
+                flywheel.manageFlywheel(targetingSettings.flywheelRPM);
+
+                boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000;
+                boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance;
+                boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance;
+
+                boolean shouldFinish = timeDone || xDone || yDone;
+
+                return !shouldFinish;
+
+            }
+        };
+    }
+
+    public Action manageFlywheelAuto(
+            double time,
+            double posX,
+            double posY,
+            double posXTolerance,
+            double posYTolerance
+    ) {
+
+        boolean timeFallback = (time != 0.501);
+        boolean posXFallback = (posX != 0.501);
+        boolean posYFallback = (posY != 0.501);
+
+        return new Action() {
+
+            double stamp = 0.0;
+            int ticker = 0;
+
+            @Override
+            public boolean run(@NonNull TelemetryPacket telemetryPacket) {
+
+                drive.updatePoseEstimate();
+                Pose2d currentPose = drive.localizer.getPose();
+
+                if (ticker == 0) {
+                    stamp = System.currentTimeMillis();
+                }
+
+                ticker++;
+
+                double robotX = drive.localizer.getPose().position.x;
+                double robotY = drive.localizer.getPose().position.y;
+
+                double robotHeading = drive.localizer.getPose().heading.toDouble();
+
+                double goalX = -15;
+                double goalY = 0;
+
+                double dx = robotX - goalX;  // delta x from robot to goal
+                double dy = robotY - goalY;  // delta y from robot to goal
+                Pose2d deltaPose = new Pose2d(dx, dy, robotHeading);
+
+                double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
+
+                targetingSettings = targeting.calculateSettings
+                        (robotX, robotY, robotHeading, 0.0, false);
+
+                robot.hood.setPosition(targetingSettings.hoodAngle);
+
+                flywheel.manageFlywheel(targetingSettings.flywheelRPM);
+
+                boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000;
+                boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance;
+                boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance;
+
+                boolean shouldFinish = timeDone || xDone || yDone;
+
+                return !shouldFinish;
+
+            }
+        };
+    }
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+
+        hardwareMap.get(Servo.class, "light").setPosition(0);
+
+        robot = new Robot(hardwareMap);
+
+        TELE = new MultipleTelemetry(
+                telemetry, FtcDashboard.getInstance().getTelemetry()
+        );
+
+        flywheel = new Flywheel(hardwareMap);
+
+        targeting = new Targeting();
+        targetingSettings = new Targeting.Settings(0.0, 0.0);
+
+        spindexer = new Spindexer(hardwareMap);
+
+        servos = new Servos(hardwareMap);
+
+        robot.limelight.pipelineSwitch(1);
+
+        turret = new Turret(robot, TELE, robot.limelight);
+
+        turret.manualSetTurret(0.4);
+
+        drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
+
+        TrajectoryActionBuilder shoot0 = null;
+
+        TrajectoryActionBuilder pickup1 = null;
+        TrajectoryActionBuilder shoot1 = null;
+        TrajectoryActionBuilder pickup2 = 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);
+
+        robot.transferServo.setPosition(transferServo_out);
+
+        robot.light.setPosition(1);
+
+        while (opModeInInit()) {
+
+            robot.hood.setPosition(shoot0Hood);
+
+            if (gamepad2.crossWasPressed()) {
+                redAlliance = !redAlliance;
+            }
+
+            if (redAlliance) {
+                robot.light.setPosition(0.28);
+
+                // ===== FIRST SHOT =====
+                x1 = rx1;
+                y1 = ry1;
+                h1 = rh1;
+
+                // ===== 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 =====
+                x1 = bx1;
+                y1 = by1;
+                h1 = bh1;
+
+                // ===== 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;
+            }
+
+            shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
+                    .strafeToLinearHeading(new Vector2d(x1, y1), h1);
+
+            pickup1 = drive.actionBuilder(new Pose2d(x1, y1, h1))
+                    .strafeToLinearHeading(new Vector2d(x2a, y2a), h2a)
+                    .strafeToLinearHeading(new Vector2d(x2b, y2b), h2b,
+                            new TranslationalVelConstraint(pickup1Speed));
+
+            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();
+
+        if (isStopRequested()) return;
+
+        if (opModeIsActive()) {
+
+            robot.transfer.setPower(1);
+
+            assert shoot0 != null;
+
+            Actions.runBlocking(
+                    new ParallelAction(
+                            shoot0.build(),
+                            manageFlywheel(
+                                    shoot0Vel,
+                                    shoot0Hood,
+                                    flywheel0Time,
+                                    x1,
+                                    0.501,
+                                    shoot0XTolerance,
+                                    0.501
+                            )
+
+                    )
+            );
+
+            Actions.runBlocking(
+                    shootAll((int) shoot0Vel, shoot0Time, shoot0SpinSpeedIncrease)
+            );
+
+            //WORKING SHOOT ALL for the preload
+
+            //Pickup first pile
+
+            Actions.runBlocking(
+                    new SequentialAction(
+                            new ParallelAction(
+                                    pickup1.build(),
+                                    manageFlywheel(
+                                            shootAllVelocity,
+                                            shootAllHood,
+                                            pickup1Time,
+                                            x2b,
+                                            y2b,
+                                            pickup1XTolerance,
+                                            pickup1YTolerance
+                                    ),
+                                    intake(intake1Time),
+                                    detectObelisk(
+                                            obelisk1Time,
+                                            x2b,
+                                            y2c,
+                                            obelisk1XTolerance,
+                                            obelisk1YTolerance,
+                                            obeliskTurrPos
+                                    )
+                            ),
+                            new ParallelAction(
+                                    openGate.build() //TODO: Add other managing stuff here
+                            )
+                    )
+
+            );
+
+            motif = turret.detectObelisk(); //Detect Obelisk if not alr
+
+            Actions.runBlocking(
+                    new ParallelAction(
+                            manageFlywheel(
+                                    shootAllVelocity,
+                                    shootAllHood,
+                                    shoot1Time,
+                                    0.501,
+                                    0.501,
+                                    0.501,
+                                    0.501
+                            ),
+                            shoot1.build(),
+                            prepareShootIndexed(shoot1Time)
+                    )
+            );
+
+            Actions.runBlocking(
+                    new ParallelAction(
+                            manageShooterAuto(
+                                    shootAllTime,
+                                    0.501,
+                                    0.501,
+                                    0.501,
+                                    0.501
+                            ),
+                            shootAllIndexed(shootAllTime, spindexerSpeedIncrease)
+                    )
+
+            );
+
+            //Shoot from first pile
+
+        }
+
+    }
+}

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

@@ -1,7 +1,14 @@
 package org.firstinspires.ftc.teamcode.autonomous;
 
 import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
-import static org.firstinspires.ftc.teamcode.constants.Poses_V2.*;
+import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bHGate;
+import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bShoot1Tangent;
+import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bShootH;
+import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bShootX;
+import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bShootY;
+import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bXGate;
+import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bYGate;
+import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bh1;
 import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bh2a;
 import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bh2b;
 import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bh2c;
@@ -16,6 +23,13 @@ import static org.firstinspires.ftc.teamcode.constants.Poses_V2.by1;
 import static org.firstinspires.ftc.teamcode.constants.Poses_V2.by2a;
 import static org.firstinspires.ftc.teamcode.constants.Poses_V2.by2b;
 import static org.firstinspires.ftc.teamcode.constants.Poses_V2.by2c;
+import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rHGate;
+import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rShoot1Tangent;
+import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rShootH;
+import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rShootX;
+import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rShootY;
+import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rXGate;
+import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rYGate;
 import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rh1;
 import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rh2a;
 import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rh2b;
@@ -62,46 +76,44 @@ import org.firstinspires.ftc.teamcode.utils.Turret;
 
 @Autonomous(preselectTeleOp = "TeleopV3")
 @Config
-public class Auto_LT extends LinearOpMode {
-
-    public int motif = 0;
+public class Auto_LT_Unindexed extends LinearOpMode {
 
     public static double shoot0Vel = 2300, shoot0Hood = 0.93;
     public static double autoSpinStartPos = 0.2;
     public static double shoot0SpinSpeedIncrease = 0.014;
+
+    public static double spindexerSpeedIncrease = 0.02;
+
     public static double obeliskTurrPos = 0.53;
-
+    public static double gatePickupTime = 3.0;
     public static double shoot1Turr = 0.57;
-
     public static double shoot0XTolerance = 1.0;
-    public static double shoot0Time = 1.8;
-
-    public static double intake1Time = 5.0;
-
+    public static double turretShootPos = 0.72;
+    public static double shootAllTime = 1.8;
+    public static double shoot0Time = 1.6;
+    public static double intake1Time = 3.0;
     public static double flywheel0Time = 3.5;
-    public static double pickup1Speed = 20.0;
-
+    public static double pickup1Speed = 80.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 ----
     public static double pickup1Time = 3.0;
-
     // ---- PICKUP POSITION TOLERANCES ----
     public static double pickup1XTolerance = 2.0;
     public static double pickup1YTolerance = 2.0;
-
     // ---- OBELISK DETECTION ----
     public static double obelisk1Time = 1.5;
     public static double obelisk1XTolerance = 2.0;
     public static double obelisk1YTolerance = 2.0;
-
     public static double shoot1ToleranceX = 2.0;
     public static double shoot1ToleranceY = 2.0;
     public static double shoot1Time = 2.0;
-
-
+    public static double shootCycleTime = 2.5;
+    public int motif = 0;
     Robot robot;
     MultipleTelemetry TELE;
     MecanumDrive drive;
@@ -109,6 +121,8 @@ public class Auto_LT extends LinearOpMode {
     Spindexer spindexer;
     Flywheel flywheel;
     Turret turret;
+    Targeting targeting;
+    Targeting.Settings targetingSettings;
     private double x1, y1, h1;
 
     private double x2a, y2a, h2a, t2a;
@@ -117,6 +131,9 @@ public class Auto_LT extends LinearOpMode {
     private double x2c, y2c, h2c, t2c;
 
     private double xShoot, yShoot, hShoot;
+    private double xGate, yGate, hGate;
+
+    private double shoot1Tangent;
 
     public Action prepareShootAll(double time) {
         return new Action() {
@@ -130,25 +147,22 @@ public class Auto_LT extends LinearOpMode {
                 }
                 ticker++;
 
-                robot.turr1.setPosition(shoot1Turr);
-                robot.turr1.setPosition(1-shoot1Turr);
-
-
                 robot.spin1.setPosition(autoSpinStartPos);
                 robot.spin2.setPosition(1 - autoSpinStartPos);
 
                 robot.transferServo.setPosition(transferServo_out);
 
+                turret.manualSetTurret(turretShootPos);
+
                 robot.intake.setPower(-((System.currentTimeMillis() - stamp)) / 1000);
 
                 return (System.currentTimeMillis() - stamp) < (time * 1000);
 
-
             }
         };
     }
 
-    public Action shootAll(int vel, double shootTime) {
+    public Action shootAll(int vel, double shootTime, double spindexSpeed) {
         return new Action() {
             int ticker = 1;
 
@@ -189,8 +203,69 @@ public class Auto_LT extends LinearOpMode {
                         robot.transferServo.setPosition(transferServo_in);
                         shooterTicker++;
                         double prevSpinPos = robot.spin1.getPosition();
-                        robot.spin1.setPosition(prevSpinPos + shoot0SpinSpeedIncrease);
-                        robot.spin2.setPosition(1 - prevSpinPos - shoot0SpinSpeedIncrease);
+                        robot.spin1.setPosition(prevSpinPos + spindexSpeed);
+                        robot.spin2.setPosition(1 - prevSpinPos - spindexSpeed);
+                    }
+
+                    return true;
+
+                } else {
+                    robot.transferServo.setPosition(transferServo_out);
+                    //spindexPos = spindexer_intakePos1;
+
+                    spindexer.resetSpindexer();
+                    spindexer.processIntake();
+
+                    return false;
+
+                }
+
+            }
+        };
+    }
+
+    public Action shootAllAuto(double shootTime, double spindexSpeed) {
+        return new Action() {
+            int ticker = 1;
+
+            double stamp = 0.0;
+
+            double velo = 0.0;
+
+            int shooterTicker = 0;
+
+            @Override
+            public boolean run(@NonNull TelemetryPacket telemetryPacket) {
+                TELE.addData("Velocity", velo);
+                TELE.addLine("shooting");
+                TELE.update();
+
+                velo = flywheel.getVelo();
+
+                drive.updatePoseEstimate();
+
+                teleStart = drive.localizer.getPose();
+
+                robot.intake.setPower(-0.3);
+
+                if (ticker == 1) {
+                    stamp = getRuntime();
+                }
+                ticker++;
+
+                robot.intake.setPower(0);
+
+                if (getRuntime() - stamp < shootTime) {
+
+                    if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) {
+                        robot.spin1.setPosition(autoSpinStartPos);
+                        robot.spin2.setPosition(1 - autoSpinStartPos);
+                    } else {
+                        robot.transferServo.setPosition(transferServo_in);
+                        shooterTicker++;
+                        double prevSpinPos = robot.spin1.getPosition();
+                        robot.spin1.setPosition(prevSpinPos + spindexSpeed);
+                        robot.spin2.setPosition(1 - prevSpinPos - spindexSpeed);
                     }
 
                     return true;
@@ -227,14 +302,14 @@ public class Auto_LT extends LinearOpMode {
 
                 motif = turret.detectObelisk();
                 
-                return (System.currentTimeMillis() - stamp) < (intakeTime * 1000);
+                spindexer.ballCounterLight();
 
+                return (System.currentTimeMillis() - stamp) < (intakeTime * 1000);
 
             }
         };
     }
 
-
     public Action detectObelisk(
             double time,
             double posX,
@@ -328,6 +403,133 @@ public class Auto_LT extends LinearOpMode {
         };
     }
 
+    public Action manageShooterAuto(
+            double time,
+            double posX,
+            double posY,
+            double posXTolerance,
+            double posYTolerance
+    ) {
+
+        boolean timeFallback = (time != 0.501);
+        boolean posXFallback = (posX != 0.501);
+        boolean posYFallback = (posY != 0.501);
+
+        return new Action() {
+
+            double stamp = 0.0;
+            int ticker = 0;
+
+            @Override
+            public boolean run(@NonNull TelemetryPacket telemetryPacket) {
+
+                drive.updatePoseEstimate();
+                Pose2d currentPose = drive.localizer.getPose();
+
+                if (ticker == 0) {
+                    stamp = System.currentTimeMillis();
+                }
+
+                ticker++;
+
+                double robotX = drive.localizer.getPose().position.x;
+                double robotY = drive.localizer.getPose().position.y;
+
+                double robotHeading = drive.localizer.getPose().heading.toDouble();
+
+                double goalX = -15;
+                double goalY = 0;
+
+                double dx = robotX - goalX;  // delta x from robot to goal
+                double dy = robotY - goalY;  // delta y from robot to goal
+                Pose2d deltaPose = new Pose2d(dx, dy, robotHeading);
+
+                double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
+
+                targetingSettings = targeting.calculateSettings
+                        (robotX, robotY, robotHeading, 0.0, false);
+
+                turret.trackGoal(deltaPose);
+
+                robot.hood.setPosition(targetingSettings.hoodAngle);
+
+                flywheel.manageFlywheel(targetingSettings.flywheelRPM);
+
+                boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000;
+                boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance;
+                boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance;
+
+                boolean shouldFinish = timeDone || xDone || yDone;
+
+                return !shouldFinish;
+
+            }
+        };
+    }
+
+    public Action manageFlywheelAuto(
+            double time,
+            double posX,
+            double posY,
+            double posXTolerance,
+            double posYTolerance
+    ) {
+
+        boolean timeFallback = (time != 0.501);
+        boolean posXFallback = (posX != 0.501);
+        boolean posYFallback = (posY != 0.501);
+
+        return new Action() {
+
+            double stamp = 0.0;
+            int ticker = 0;
+
+            @Override
+            public boolean run(@NonNull TelemetryPacket telemetryPacket) {
+
+                drive.updatePoseEstimate();
+                Pose2d currentPose = drive.localizer.getPose();
+
+                if (ticker == 0) {
+                    stamp = System.currentTimeMillis();
+                }
+
+                ticker++;
+
+                double robotX = drive.localizer.getPose().position.x;
+                double robotY = drive.localizer.getPose().position.y;
+
+                double robotHeading = drive.localizer.getPose().heading.toDouble();
+
+                double goalX = -15;
+                double goalY = 0;
+
+                double dx = robotX - goalX;  // delta x from robot to goal
+                double dy = robotY - goalY;  // delta y from robot to goal
+                Pose2d deltaPose = new Pose2d(dx, dy, robotHeading);
+
+                double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
+
+                targetingSettings = targeting.calculateSettings
+                        (robotX, robotY, robotHeading, 0.0, false);
+
+
+                robot.hood.setPosition(targetingSettings.hoodAngle);
+
+                flywheel.manageFlywheel(targetingSettings.flywheelRPM);
+
+                boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000;
+                boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance;
+                boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance;
+
+                boolean shouldFinish = timeDone || xDone || yDone;
+
+                return !shouldFinish;
+
+            }
+        };
+    }
+
     @Override
     public void runOpMode() throws InterruptedException {
 
@@ -341,8 +543,8 @@ public class Auto_LT extends LinearOpMode {
 
         flywheel = new Flywheel(hardwareMap);
 
-        Targeting targeting = new Targeting();
-        Targeting.Settings targetingSettings = new Targeting.Settings(0.0, 0.0);
+        targeting = new Targeting();
+        targetingSettings = new Targeting.Settings(0.0, 0.0);
 
         spindexer = new Spindexer(hardwareMap);
 
@@ -356,10 +558,11 @@ public class Auto_LT extends LinearOpMode {
 
         drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
 
-        TrajectoryActionBuilder shoot0;
-
-        TrajectoryActionBuilder pickup1;
-        TrajectoryActionBuilder shoot1;
+        TrajectoryActionBuilder shoot0 = null;
+        TrajectoryActionBuilder pickup1 = null;
+        TrajectoryActionBuilder shoot1 = null;
+        TrajectoryActionBuilder gatePickup = null;
+        TrajectoryActionBuilder shootCycle = null;
 
         robot.spin1.setPosition(autoSpinStartPos);
         robot.spin2.setPosition(1 - autoSpinStartPos);
@@ -401,6 +604,12 @@ public class Auto_LT extends LinearOpMode {
                 xShoot = rShootX;
                 yShoot = rShootY;
                 hShoot = rShootH;
+                shoot1Tangent = rShoot1Tangent;
+                
+                xGate = rXGate;
+                yGate = rYGate;
+                hGate = rHGate;
+                
 
             } else {
                 robot.light.setPosition(0.6);
@@ -427,7 +636,14 @@ public class Auto_LT extends LinearOpMode {
                 xShoot = bShootX;
                 yShoot = bShootY;
                 hShoot = bShootH;
+
+                shoot1Tangent = bShoot1Tangent;
+
+                xGate = bXGate;
+                yGate = bYGate;
+                hGate = bHGate;
             }
+
             shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
                     .strafeToLinearHeading(new Vector2d(x1, y1), h1);
 
@@ -436,19 +652,18 @@ public class Auto_LT extends LinearOpMode {
                     .strafeToLinearHeading(new Vector2d(x2b, y2b), h2b,
                             new TranslationalVelConstraint(pickup1Speed));
             shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, h2b))
-                    .strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot);
+                    .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))
+                    .strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot);
 
         }
 
-        shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
-                .strafeToLinearHeading(new Vector2d(x1, y1), h1);
-        pickup1 = drive.actionBuilder(new Pose2d(x1, y1, h1))
-                .strafeToLinearHeading(new Vector2d(x2a, y2a), h2a)
-                .strafeToLinearHeading(new Vector2d(x2b, y2b), h2b,
-                        new TranslationalVelConstraint(pickup1Speed));
-        shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, h2b))
-                .strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot);
         waitForStart();
 
         if (isStopRequested()) return;
@@ -457,6 +672,8 @@ public class Auto_LT extends LinearOpMode {
 
             robot.transfer.setPower(1);
 
+            assert shoot0 != null;
+
             Actions.runBlocking(
                     new ParallelAction(
                             shoot0.build(),
@@ -474,15 +691,15 @@ public class Auto_LT extends LinearOpMode {
             );
 
             Actions.runBlocking(
-                    shootAll((int) shoot0Vel, shoot0Time)
+                    shootAll((int) shoot0Vel, shoot0Time, shoot0SpinSpeedIncrease)
             );
 
             Actions.runBlocking(
                     new ParallelAction(
                             pickup1.build(),
                             manageFlywheel(
-                                    shoot1Vel,
-                                    shoot1Hood,
+                                    shootAllVelocity, 
+                                    shootAllHood,
                                     pickup1Time,
                                     x2b,
                                     y2b,
@@ -506,13 +723,13 @@ public class Auto_LT extends LinearOpMode {
             Actions.runBlocking(
                     new ParallelAction(
                             manageFlywheel(
-                                    shoot1Vel,
-                                    shoot1Hood,
+                                    shootAllVelocity, 
+                                    shootAllHood,
                                     shoot1Time,
-                                    xShoot,
-                                    yShoot,
-                                    shoot1ToleranceX,
-                                    shoot1ToleranceY
+                                    0.501,
+                                    0.501,
+                                    0.501,
+                                    0.501
                             ),
                             shoot1.build(),
                             prepareShootAll(shoot1Time)
@@ -520,19 +737,67 @@ public class Auto_LT extends LinearOpMode {
             );
 
             Actions.runBlocking(
-                    shootAll((int) shoot1Vel, shoot0Time)
+                    new ParallelAction(
+                            manageShooterAuto(
+                                    shootAllTime,
+                                    0.501,
+                                    0.501,
+                                    0.501,
+                                    0.501
+                            ),
+                            shootAllAuto(shootAllTime, spindexerSpeedIncrease)
+                    )
+
             );
 
+            while (opModeIsActive()) {
+
+                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)
+                        )
+
+                );
+
+            }
 
 
-
-            TELE.addData("ID", motif);
-
-            TELE.update();
-
-            robot.intake.setPower(0);
-
-            sleep(5000);
         }
 
     }

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;
+}

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

@@ -20,6 +20,8 @@ public class Poses_V2 {
 
     public static double rx2c = 34, ry2c = 50, rh2c = Math.toRadians(140), rt2c  = Math.toRadians(Math.PI/2);
 
+    public static double rXGate = 30, rYGate = 63, rHGate = Math.toRadians(179);
+
     public static double rx3a = 55, ry3a = 39, rh3a = Math.toRadians(140);
     public static double rx3b = 33, ry3b = 61, rh3b = Math.toRadians(140);
 
@@ -31,9 +33,11 @@ public class Poses_V2 {
     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);
 
-    public static double rShootX = 40, rShootY = 30, rShootH = Math.toRadians(140);
+    public static double rShootX = 40, rShootY = 7, rShootH = Math.toRadians(140);
 
     public static double bShootX = 20, bShootY = 30, bShootH = Math.toRadians(140);
+    public static double bXGate = 25, bYGate = 69, bHGate = Math.toRadians(165);
+
 
     public static double bx3a = 55, by3a = -43, bh3a = Math.toRadians(-140);
     public static double bx3b = 37, by3b = -61, bh3b = Math.toRadians(-140);
@@ -42,6 +46,11 @@ public class Poses_V2 {
     public static double bx4b = 48, by4b = -79, bh4b = Math.toRadians(-140);
     public static double rfx1 = 10, rfy1 = 0, rfh1 = 0; //TODO: test this
 
+    public static double rShoot1Tangent = Math.toRadians(0);
+    public static double bShoot1Tangent = Math.toRadians(0);
+
+
+
     public static Pose2d teleStart = new Pose2d(0, 0, 0);
 
 }

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

@@ -5,16 +5,16 @@ import com.acmerobotics.dashboard.config.Config;
 @Config
 public class ServoPositions {
 
-    public static double spindexer_intakePos1 = 0.13;
+    public static double spindexer_intakePos1 = 0;
 
-    public static double spindexer_intakePos2 = 0.33;//0.5;
+    public static double spindexer_intakePos2 = 0.19;//0.5;
 
-    public static double spindexer_intakePos3 = 0.53;//0.66;
+    public static double spindexer_intakePos3 = 0.38;//0.66;
 
-    public static double spindexer_outtakeBall3 = 0.8;
+    public static double spindexer_outtakeBall3 = 0.65;
 
-    public static double spindexer_outtakeBall2 = 0.6;
-    public static double spindexer_outtakeBall1 = 0.4;
+    public static double spindexer_outtakeBall2 = 0.46;
+    public static double spindexer_outtakeBall1 = 0.27;
     public static double spinStartPos = spindexer_outtakeBall1 - 0.08;
 
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/MecanumDrive.java

@@ -201,9 +201,9 @@ public final class MecanumDrive {
         public double kA = 0.000046;
 
         // path profile parameters (in inches)
-        public double maxWheelVel = 180;
+        public double maxWheelVel = 250;
         public double minProfileAccel = -40;
-        public double maxProfileAccel = 180;
+        public double maxProfileAccel = 250;
 
         // turn profile parameters (in radians)
         public double maxAngVel = 4 * Math.PI; // shared with path