far auto in development part 2

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

@@ -2,26 +2,14 @@ package org.firstinspires.ftc.teamcode.autonomous;
 
 import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
 import static org.firstinspires.ftc.teamcode.constants.Front_Poses.*;
-import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodOffset;
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos;
-import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinEndPos;
-import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1;
-import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1;
-import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2;
-import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3;
-import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3b;
-import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
-import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate;
 import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed;
 import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault;
 
-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.TrajectoryActionBuilder;
@@ -40,17 +28,11 @@ import org.firstinspires.ftc.teamcode.utils.Spindexer;
 import org.firstinspires.ftc.teamcode.utils.Targeting;
 import org.firstinspires.ftc.teamcode.utils.Turret;
 
-import java.util.Objects;
-
 @Config
 @Autonomous(preselectTeleOp = "TeleopV3")
 public class Auto_LT_Close extends LinearOpMode {
-    public static double shoot0Vel = 2300, shoot0Hood = 0.93 + hoodOffset;
+    public static double shoot0Vel = 2300, shoot0Hood = 0.93;
-    public static double autoSpinStartPos = 0.2;
+    public static double spindexerSpeedIncrease = 0.008;
-    public static double shoot0SpinSpeedIncrease = 0.02;
-
-    public static double spindexerSpeedIncrease = 0.03;
-    public static double finalSpindexerSpeedIncrease = 0.03;
 
     // These values are ADDED to turrDefault
     public static double redObeliskTurrPos1 = 0.12;
@@ -65,40 +47,25 @@ public class Auto_LT_Close extends LinearOpMode {
     double obeliskTurrPos1 = 0.0;
     double obeliskTurrPos2 = 0.0;
     double obeliskTurrPos3 = 0.0;
-    public static double normalIntakeTime = 3.3;
+
-    public static double shoot1Turr = 0.57;
-    public static double shoot0XTolerance = 1.0;
     double turretShootPos = 0.0;
 
-    public static double finalShootAllTime = 3.0;
+    public static double shootAllTime = 2;
-    public static double shootAllTime = 1.8;
-    public static double shoot0Time = 1.6;
     public static double intake1Time = 3.3;
     public static double intake2Time = 3.8;
 
     public static double intake3Time = 4.2;
 
-    public static double flywheel0Time = 3.5;
+    public static double flywheel0Time = 1.5;
-    public static double pickup1Speed = 15;
+    public static double pickup1Speed = 12;
-    // ---- SECOND SHOT / PICKUP ----
+    // ---- POSITION TOLERANCES ----
-    public static double shoot1Vel = 2300;
+    public static double posXTolerance = 5.0;
-    public static double shootAllVelocity = 2500;
+    public static double posYTolerance = 5.0;
-    public static double shootAllHood = 0.78 + hoodOffset;
-    // ---- 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 shoot1Time = 2.5;
-    public static double obelisk1XTolerance = 2.0;
+    public static double shoot2Time = 2.5;
-    public static double obelisk1YTolerance = 2.0;
+    public static double shoot3Time = 2.5;
-    public static double shoot1ToleranceX = 2.0;
-    public static double shoot1ToleranceY = 2.0;
-    public static double shoot1Time = 2;
-    public static double shoot2Time = 2;
-    public static double shoot3Time = 2;
     public static double colorSenseTime = 1.2;
-
-    public static double firstShootTime = 0.3;
     public int motif = 0;
 
     Robot robot;
@@ -111,8 +78,6 @@ public class Auto_LT_Close extends LinearOpMode {
     Targeting targeting;
     Targeting.Settings targetingSettings;
     AutoActions autoActions;
-    private double firstSpindexShootPos = autoSpinStartPos;
-    private boolean shootForward = true;
     double x1, y1, h1;
 
     double x2a, y2a, h2a, t2a;
@@ -132,11 +97,6 @@ public class Auto_LT_Close extends LinearOpMode {
 
     private double shoot1Tangent;
 
-    private int driverSlotGreen = 0;
-    private int passengerSlotGreen = 0;
-
-    private int rearSlotGreen = 0;
-    private int mostGreenSlot = 0;
     int ballCycles = 3;
     int prevMotif = 0;
 
@@ -170,13 +130,11 @@ public class Auto_LT_Close extends LinearOpMode {
 
         turret = new Turret(robot, TELE, robot.limelight);
 
-        turret.setTurret(turrDefault);
-
         drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
 
         autoActions = new AutoActions(robot, drive, TELE, servos, flywheel, spindexer, targeting, targetingSettings, turret);
 
-        servos.setSpinPos(autoSpinStartPos);
+        servos.setSpinPos(spinStartPos);
 
         servos.setTransferPos(transferServo_out);
 
@@ -209,8 +167,8 @@ public class Auto_LT_Close extends LinearOpMode {
             }
 
             if (gamepad2.squareWasPressed()){
+                turret.pipelineSwitch(1);
                 robot.limelight.start();
-                robot.limelight.pipelineSwitch(1);
                 gamepad2.rumble(500);
             }
 
@@ -354,17 +312,21 @@ public class Auto_LT_Close extends LinearOpMode {
             robot.transfer.setPower(1);
 
             startAuto();
+            shoot();
 
             if (ballCycles > 0){
                 cycleStackClose();
+                shoot();
             }
 
             if (ballCycles > 1){
                 cycleStackMiddle();
+                shoot();
             }
 
             if (ballCycles > 2){
                 cycleStackFar();
+                shoot();
             }
 
             while (opModeIsActive()) {
@@ -374,6 +336,7 @@ public class Auto_LT_Close extends LinearOpMode {
                 teleStart = drive.localizer.getPose();
 
                 flywheel.manageFlywheel(0);
+                robot.transfer.setPower(0);
 
                 TELE.addLine("finished");
                 TELE.update();
@@ -383,50 +346,64 @@ public class Auto_LT_Close extends LinearOpMode {
 
     }
 
+    void shoot(){
+        Actions.runBlocking(
+                new ParallelAction(
+                        autoActions.manageShooterAuto(
+                                shootAllTime,
+                                0.501,
+                                0.501,
+                                0.501,
+                                0.501,
+                                0.501,
+                                false
+                        ),
+                        autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease)
+                )
+
+        );
+    }
+
     void startAuto() {
         assert shoot0 != null;
 
         Actions.runBlocking(
                 new ParallelAction(
                         shoot0.build(),
-                        autoActions.manageFlywheel(
+                        autoActions.manageShooterAuto(
-                                shoot0Vel,
-                                shoot0Hood,
                                 flywheel0Time,
                                 x1,
-                                0.501,
+                                y1,
-                                shoot0XTolerance,
+                                posXTolerance,
-                                0.501
+                                posYTolerance,
+                                h1,
+                                false
                         )
 
                 )
         );
-
-        Actions.runBlocking(
-                autoActions.shootAll((int) shoot0Vel, shoot0Time, shoot0SpinSpeedIncrease)
-        );
     }
 
     void cycleStackClose(){
         Actions.runBlocking(
                 new ParallelAction(
                         pickup1.build(),
-                        autoActions.manageFlywheel(
+                        autoActions.manageShooterAuto(
-                                shootAllVelocity,
-                                shootAllHood,
                                 intake1Time,
-                                0.501,
+                                x2b,
-                                0.501,
+                                y2b,
-                                pickup1XTolerance,
+                                posXTolerance,
-                                pickup1YTolerance
+                                posYTolerance,
+                                h2b,
+                                true
                         ),
                         autoActions.intake(intake1Time),
                         autoActions.detectObelisk(
                                 intake1Time,
-                                0.501,
+                                x2b,
-                                0.501,
+                                y2b,
-                                obelisk1XTolerance,
+                                posXTolerance,
-                                obelisk1YTolerance,
+                                posYTolerance,
                                 obeliskTurrPos1
                         )
 
@@ -438,35 +415,33 @@ public class Auto_LT_Close extends LinearOpMode {
         if (motif == 0) motif = 22;
         prevMotif = motif;
 
-        Actions.runBlocking(
+        double posX;
-                new ParallelAction(
+        double posY;
-                        autoActions.manageFlywheel(
+        double posH;
-                                shootAllVelocity,
+        if (ballCycles > 1){
-                                shootAllHood,
+            posX = xShoot;
-                                shoot1Time,
+            posY = yShoot;
-                                0.501,
+            posH = hShoot;
-                                0.501,
+        } else {
-                                0.501,
+            posX = xLeave;
-                                0.501
+            posY = yLeave;
-                        ),
+            posH = hLeave;
-                        shoot1.build(),
+        }
-                        autoActions.prepareShootAll(colorSenseTime, shoot1Time, motif)
-                )
-        );
-
 
         Actions.runBlocking(
                 new ParallelAction(
                         autoActions.manageShooterAuto(
-                                shootAllTime,
+                                shoot1Time,
-                                0.501,
+                                posX,
-                                0.501,
+                                posY,
-                                0.501,
+                                posXTolerance,
-                                0.501
+                                posYTolerance,
+                                posH,
+                                false
                         ),
-                        autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease)
+                        shoot1.build(),
+                        autoActions.prepareShootAll(colorSenseTime, shoot1Time, motif)
                 )
-
         );
     }
 
@@ -476,18 +451,20 @@ public class Auto_LT_Close extends LinearOpMode {
                         pickup2.build(),
                         autoActions.manageShooterAuto(
                                 intake2Time,
-                                0.501,
+                                x3b,
-                                0.501,
+                                y3b,
-                                pickup1XTolerance,
+                                posXTolerance,
-                                pickup1YTolerance
+                                posYTolerance,
+                                h3b,
+                                true
                         ),
                         autoActions.intake(intake2Time),
                         autoActions.detectObelisk(
                                 intake2Time,
-                                0.501,
+                                x3b,
-                                0.501,
+                                y3b,
-                                obelisk1XTolerance,
+                                posXTolerance,
-                                obelisk1YTolerance,
+                                posYTolerance,
                                 obeliskTurrPos2
                         )
 
@@ -499,32 +476,33 @@ public class Auto_LT_Close extends LinearOpMode {
         if (motif == 0) motif = prevMotif;
         prevMotif = motif;
 
-        Actions.runBlocking(
+        double posX;
-                new ParallelAction(
+        double posY;
-                        autoActions.manageFlywheelAuto(
+        double posH;
-                                shoot2Time,
+        if (ballCycles > 2){
-                                0.501,
+            posX = xShoot;
-                                0.501,
+            posY = yShoot;
-                                0.501,
+            posH = hShoot;
-                                0.501
+        } else {
-                        ),
+            posX = xLeave;
-                        shoot2.build(),
+            posY = yLeave;
-                        autoActions.prepareShootAll(colorSenseTime, shoot2Time, motif)
+            posH = hLeave;
-                )
+        }
-        );
 
         Actions.runBlocking(
                 new ParallelAction(
                         autoActions.manageShooterAuto(
-                                shootAllTime,
+                                shoot2Time,
-                                0.501,
+                                posX,
-                                0.501,
+                                posY,
-                                0.501,
+                                posXTolerance,
-                                0.501
+                                posYTolerance,
+                                posH,
+                                false
                         ),
-                        autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease)
+                        shoot2.build(),
+                        autoActions.prepareShootAll(colorSenseTime, shoot2Time, motif)
                 )
-
         );
     }
 
@@ -534,18 +512,20 @@ public class Auto_LT_Close extends LinearOpMode {
                         pickup3.build(),
                         autoActions.manageShooterAuto(
                                 intake3Time,
-                                0.501,
+                                x4b,
-                                0.501,
+                                y4b,
-                                pickup1XTolerance,
+                                posXTolerance,
-                                pickup1YTolerance
+                                posYTolerance,
+                                h4b,
+                                true
                         ),
                         autoActions.intake(intake3Time),
                         autoActions.detectObelisk(
                                 intake3Time,
-                                0.501,
+                                x4b,
-                                0.501,
+                                y4b,
-                                obelisk1XTolerance,
+                                posXTolerance,
-                                obelisk1YTolerance,
+                                posYTolerance,
                                 obeliskTurrPos3
                         )
 
@@ -559,30 +539,18 @@ public class Auto_LT_Close extends LinearOpMode {
 
         Actions.runBlocking(
                 new ParallelAction(
-                        autoActions.manageFlywheelAuto(
+                        autoActions.manageShooterAuto(
                                 shoot3Time,
-                                0.501,
+                                xLeave,
-                                0.501,
+                                yLeave,
-                                0.501,
+                                posXTolerance,
-                                0.501
+                                posYTolerance,
+                                hLeave,
+                                false
                         ),
                         shoot3.build(),
                         autoActions.prepareShootAll(colorSenseTime, shoot3Time, motif)
                 )
         );
-
-        Actions.runBlocking(
-                new ParallelAction(
-                        autoActions.manageShooterAuto(
-                                finalShootAllTime,
-                                0.501,
-                                0.501,
-                                0.501,
-                                0.501
-                        ),
-                        autoActions.shootAllAuto(finalShootAllTime, finalSpindexerSpeedIncrease)
-                )
-
-        );
     }
 }

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

@@ -3,8 +3,8 @@ package org.firstinspires.ftc.teamcode.autonomous;
 import static org.firstinspires.ftc.teamcode.constants.Back_Poses.*;
 import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
 import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart;
-import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodOffset;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinEndPos;
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2;
@@ -13,6 +13,7 @@ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
 import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate;
+import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed;
 import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault;
 
 import androidx.annotation.NonNull;
@@ -31,6 +32,7 @@ import com.acmerobotics.roadrunner.ftc.Actions;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
+import org.firstinspires.ftc.teamcode.autonomous.actions.AutoActions;
 import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
 import org.firstinspires.ftc.teamcode.utils.Flywheel;
 import org.firstinspires.ftc.teamcode.utils.Robot;
@@ -44,15 +46,10 @@ import java.util.Objects;
 @Config
 @Autonomous(preselectTeleOp = "TeleopV3")
 public class Auto_LT_Far extends LinearOpMode {
-    public static double shoot0Vel = 3300, shoot0Hood = 0.48 + hoodOffset;
+    public static double shoot0Vel = 3300, shoot0Hood = 0.48;
-    public static double autoSpinStartPos = 0.2;
-    public static double shoot0SpinSpeedIncrease = 0.015;
-    public static double shoot0XTolerance = 1.0;
     public static double redTurretShootPos = 0.05;
     public static double blueTurretShootPos = -0.05;
-    public static int fwdTime = 200, strafeTime = 2300;
     double xLeave, yLeave, hLeave;
-    public static int sleepTime = 1300;
     public int motif = 0;
     double turretShootPos = 0.0;
     Robot robot;
@@ -64,613 +61,43 @@ public class Auto_LT_Far extends LinearOpMode {
     Turret turret;
     Targeting targeting;
     Targeting.Settings targetingSettings;
-    double firstSpindexShootPos = autoSpinStartPos;
+    AutoActions autoActions;
-    boolean shootForward = true;
     double xShoot, yShoot, hShoot;
-    private int driverSlotGreen = 0;
-    private int passengerSlotGreen = 0;
-    int rearSlotGreen = 0;
-    int mostGreenSlot = 0;
     double pickupGateX = 0, pickupGateY = 0, pickupGateH = 0;
     double pickupZoneX = 0, pickupZoneY = 0, pickupZoneH = 0;
-    public static double firstShootTime = 0.3;
+    public static double flywheel0Time = 1.5;
-    public static double flywheel0Time = 3.5;
-    public static double shoot0Time = 2;
     boolean gatePickup = false;
     boolean stack3 = true;
     double xStackPickupA, yStackPickupA, hStackPickupA;
     double xStackPickupB, yStackPickupB, hStackPickupB;
-    public static int pickupStackSpeed = 15;
+    public static int pickupStackSpeed = 12;
     int prevMotif = 0;
-
+    public static double spindexerSpeedIncrease = 0.008;
-    public Action prepareShootAll(double colorSenseTime, double time, int motif_id) {
+    public static double shootAllTime = 2;
-        return new Action() {
+    // ---- POSITION TOLERANCES ----
-            double stamp = 0.0;
+    public static double posXTolerance = 5.0;
-            int ticker = 0;
+    public static double posYTolerance = 5.0;
-
+    public static double shootStackTime = 2;
-            double spindexerWiggle = 0.01;
+    public static double shootGateTime = 2;
-
+    public static double colorSenseTime = 1;
-            boolean decideGreenSlot = false;
+    public static double intakeStackTime = 3.3;
-
+    public static double intakeGateTime = 3;
-            @Override
+    public static double redObeliskTurrPos1 = 0.12;
-            public boolean run(@NonNull TelemetryPacket telemetryPacket) {
+    public static double redObeliskTurrPos2 = 0.13;
-                if (ticker == 0) {
+    public static double redObeliskTurrPos3 = 0.14;
-                    stamp = System.currentTimeMillis();
+    public static double blueObeliskTurrPos1 = -0.12;
-                }
+    public static double blueObeliskTurrPos2 = -0.13;
-                ticker++;
+    public static double blueObeliskTurrPos3 = -0.14;
-                servos.setTransferPos(transferServo_out);
+    double obeliskTurrPos1 = 0.0;
-                drive.updatePoseEstimate();
+    double obeliskTurrPos2 = 0.0;
-
+    double obeliskTurrPos3 = 0.0;
-                teleStart = drive.localizer.getPose();
-
-                double robX = drive.localizer.getPose().position.x;
-                double robY = drive.localizer.getPose().position.y;
-                double robotHeading = drive.localizer.getPose().heading.toDouble();
-
-                double goalX = -15;
-                double goalY = 0;
-
-                double dx = robX - goalX;  // delta x from robot to goal
-                double dy = robY - 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
-                        (robX, robY, robotHeading, 0.0, turretInterpolate);
-
-                turret.trackGoal(deltaPose);
-
-                TELE.addData("Velocity", flywheel.getVelo());
-                TELE.addData("Hood", robot.hood.getPosition());
-                TELE.addData("motif", motif_id);
-                TELE.update();
-
-                if ((System.currentTimeMillis() - stamp) < (colorSenseTime * 1000)) {
-
-                    spindexerWiggle *= -1.0;
-
-                    servos.setSpinPos(spindexer_intakePos1 + spindexerWiggle);
-
-                    spindexer.detectBalls(true, true);
-
-                    if (Objects.equals(spindexer.GetFrontDriverColor(), Spindexer.BallColor.GREEN)) {
-                        driverSlotGreen++;
-                    }
-
-                    if (Objects.equals(spindexer.GetFrontPassengerColor(), Spindexer.BallColor.GREEN)) {
-                        passengerSlotGreen++;
-                    }
-
-                    if (Objects.equals(spindexer.GetRearCenterColor(), Spindexer.BallColor.GREEN)) {
-                        rearSlotGreen++;
-                    }
-
-                    spindexer.setIntakePower(1);
-
-                    decideGreenSlot = true;
-
-                    return true;
-                } else if (decideGreenSlot) {
-
-                    if (driverSlotGreen >= passengerSlotGreen && driverSlotGreen >= rearSlotGreen) {
-                        mostGreenSlot = 3;
-                    } else if (passengerSlotGreen >= driverSlotGreen && passengerSlotGreen >= rearSlotGreen) {
-                        mostGreenSlot = 2;
-                    } else {
-                        mostGreenSlot = 1;
-                    }
-
-                    decideGreenSlot = false;
-
-                    if (motif_id == 21) {
-                        if (mostGreenSlot == 1) {
-                            firstSpindexShootPos = spindexer_outtakeBall1;
-                            shootForward = true;
-                        } else if (mostGreenSlot == 2) {
-                            firstSpindexShootPos = spindexer_outtakeBall2;
-                            shootForward = false;
-                        } else {
-                            firstSpindexShootPos = spindexer_outtakeBall3;
-                            shootForward = false;
-                        }
-                    } else if (motif_id == 22) {
-                        if (mostGreenSlot == 1) {
-                            firstSpindexShootPos = spindexer_outtakeBall2;
-                            shootForward = false;
-                        } else if (mostGreenSlot == 2) {
-                            firstSpindexShootPos = spindexer_outtakeBall3;
-                            shootForward = false;
-                        } else {
-                            firstSpindexShootPos = spindexer_outtakeBall2;
-                            shootForward = true;
-                        }
-
-                    } else {
-                        if (mostGreenSlot == 1) {
-                            firstSpindexShootPos = spindexer_outtakeBall3;
-                            shootForward = false;
-                        } else if (mostGreenSlot == 2) {
-                            firstSpindexShootPos = spindexer_outtakeBall3b;
-                            shootForward = true;
-                        } else {
-                            firstSpindexShootPos = spindexer_outtakeBall1;
-                            shootForward = true;
-                        }
-
-                    }
-
-                    return true;
-                } else if ((System.currentTimeMillis() - stamp) < (time * 1000)) {
-//                    TELE.addData("MostGreenSlot", mostGreenSlot);
-//                    TELE.update();
-                    spindexer.setIntakePower(-((System.currentTimeMillis() - stamp - colorSenseTime)) / 1000);
-
-                    servos.setSpinPos(firstSpindexShootPos);
-
-                    return true;
-                } else {
-                    return false;
-                }
-
-            }
-        };
-    }
-
-    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", flywheel.getVelo());
-                TELE.addData("Hood", robot.hood.getPosition());
-                TELE.update();
-
-                double voltage = robot.voltage.getVoltage();
-                flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
-                flywheel.manageFlywheel(vel);
-                velo = flywheel.getVelo();
-
-                drive.updatePoseEstimate();
-
-                teleStart = drive.localizer.getPose();
-
-                spindexer.setIntakePower(-0.3);
-
-                if (ticker == 1) {
-                    stamp = getRuntime();
-                }
-                ticker++;
-
-                spindexer.setIntakePower(0);
-                drive.updatePoseEstimate();
-
-                teleStart = drive.localizer.getPose();
-
-                double robX = drive.localizer.getPose().position.x;
-                double robY = drive.localizer.getPose().position.y;
-                double robotHeading = drive.localizer.getPose().heading.toDouble();
-
-                double goalX = -15;
-                double goalY = 0;
-
-                double dx = robX - goalX;  // delta x from robot to goal
-                double dy = robY - 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
-                        (robX, robY, robotHeading, 0.0, turretInterpolate);
-
-                turret.trackGoal(deltaPose);
-
-                if ((getRuntime() - stamp < shootTime && servos.getSpinPos() < spinEndPos) || shooterTicker == 0) {
-
-                    if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) {
-                        servos.setSpinPos(autoSpinStartPos);
-                    } else {
-                        servos.setTransferPos(transferServo_in);
-                        shooterTicker++;
-                        double prevSpinPos = servos.getSpinCmdPos();
-                        servos.setSpinPos(prevSpinPos + spindexSpeed);
-                    }
-
-                    return true;
-
-                } else {
-                    servos.setTransferPos(transferServo_out);
-
-                    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", flywheel.getVelo());
-                TELE.addData("Hood", robot.hood.getPosition());
-                TELE.update();
-
-                velo = flywheel.getVelo();
-
-                drive.updatePoseEstimate();
-
-                teleStart = drive.localizer.getPose();
-
-                spindexer.setIntakePower(-0.3);
-
-                if (ticker == 1) {
-                    stamp = getRuntime();
-                }
-                ticker++;
-
-                spindexer.setIntakePower(0);
-                drive.updatePoseEstimate();
-
-                teleStart = drive.localizer.getPose();
-
-                double robX = drive.localizer.getPose().position.x;
-                double robY = drive.localizer.getPose().position.y;
-                double robotHeading = drive.localizer.getPose().heading.toDouble();
-
-                double goalX = -15;
-                double goalY = 0;
-
-                double dx = robX - goalX;  // delta x from robot to goal
-                double dy = robY - 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
-                        (robX, robY, robotHeading, 0.0, turretInterpolate);
-
-                turret.trackGoal(deltaPose);
-
-                if (getRuntime() - stamp < shootTime) {
-
-                    if (getRuntime() - stamp < firstShootTime) {
-                        servos.setTransferPos(transferServo_out);
-                        servos.setSpinPos(firstSpindexShootPos);
-                    } else {
-                        servos.setTransferPos(transferServo_in);
-                        shooterTicker++;
-                        double prevSpinPos = servos.getSpinCmdPos();
-
-                        if (shootForward) {
-                            servos.setSpinPos(prevSpinPos + spindexSpeed);
-                        } else {
-                            servos.setSpinPos(prevSpinPos - spindexSpeed);
-                        }
-                    }
-
-                    return true;
-
-                } else {
-                    servos.setTransferPos(transferServo_out);
-
-                    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();
-                spindexer.setIntakePower(1);
-
-                spindexer.ballCounterLight();
-                drive.updatePoseEstimate();
-
-                teleStart = drive.localizer.getPose();
-
-                TELE.addData("Full?", spindexer.isFull());
-                TELE.update();
-
-                return ((System.currentTimeMillis() - stamp) < (intakeTime * 1000)) && !spindexer.isFull();
-            }
-        };
-    }
-
-    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();
-                    robot.limelight.pipelineSwitch(1);
-                }
-
-                ticker++;
-                motif = turret.detectObelisk();
-
-                turret.setTurret(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;
-                drive.updatePoseEstimate();
-
-                teleStart = drive.localizer.getPose();
-                TELE.addData("Velocity", flywheel.getVelo());
-                TELE.addData("Hood", robot.hood.getPosition());
-                TELE.update();
-
-                if (shouldFinish){
-                    if (redAlliance){
-                        robot.limelight.pipelineSwitch(4);
-                    } else {
-                        robot.limelight.pipelineSwitch(2);
-                    }
-                    return false;
-                } else {
-                    return true;
-                }
-
-            }
-        };
-    }
-
-    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++;
-
-                double voltage = robot.voltage.getVoltage();
-                flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
-                flywheel.manageFlywheel(vel);
-                servos.setHoodPos(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;
-                drive.updatePoseEstimate();
-
-                teleStart = drive.localizer.getPose();
-                TELE.addData("Velocity", flywheel.getVelo());
-                TELE.addData("Hood", robot.hood.getPosition());
-                TELE.update();
-
-                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);
-
-                servos.setHoodPos(targetingSettings.hoodAngle);
-
-                double voltage = robot.voltage.getVoltage();
-                flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
-                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;
-                drive.updatePoseEstimate();
-
-                teleStart = drive.localizer.getPose();
-
-                TELE.addData("Velocity", flywheel.getVelo());
-                TELE.addData("Hood", robot.hood.getPosition());
-                TELE.update();
-
-                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);
-
-                servos.setHoodPos(targetingSettings.hoodAngle);
-
-                double voltage = robot.voltage.getVoltage();
-                flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
-                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;
-                drive.updatePoseEstimate();
-
-                teleStart = drive.localizer.getPose();
-                TELE.addData("Velocity", flywheel.getVelo());
-                TELE.addData("Hood", robot.hood.getPosition());
-                TELE.update();
-
-                return !shouldFinish;
-
-            }
-        };
-    }
 
     // initialize path variables here
     TrajectoryActionBuilder leave3Ball = null;
     TrajectoryActionBuilder leaveFromShoot = null;
     TrajectoryActionBuilder pickup3 = null;
     TrajectoryActionBuilder shoot3 = null;
+    Pose2d autoStart = new Pose2d(0,0,0);
 
     @Override
     public void runOpMode() throws InterruptedException {
@@ -698,27 +125,12 @@ public class Auto_LT_Far extends LinearOpMode {
 
         turret.setTurret(turrDefault);
 
-        drive = new MecanumDrive(hardwareMap, autoStart);
+        servos.setSpinPos(spinStartPos);
-
-        servos.setSpinPos(autoSpinStartPos);
 
         servos.setTransferPos(transferServo_out);
 
         while (opModeInInit()) {
 
-            // Recalibration in initialization
-            drive.updatePoseEstimate();
-            if (gamepad2.triangle) {
-                autoStart = drive.localizer.getPose(); // use this position as starting position
-                gamepad2.rumble(1000);
-            }
-
-            if (gamepad2.squareWasPressed()){
-                robot.limelight.start();
-                robot.limelight.pipelineSwitch(1);
-                gamepad2.rumble(500);
-            }
-
             if (gamepad2.leftBumperWasPressed()){
                 gatePickup = !gatePickup;
             }
@@ -745,6 +157,10 @@ public class Auto_LT_Far extends LinearOpMode {
             if (redAlliance) {
                 robot.light.setPosition(0.28);
 
+                autoStart = new Pose2d(autoStartRX, autoStartRY, Math.toRadians(autoStartRH));
+                drive = new MecanumDrive(hardwareMap, autoStart);
+                autoActions = new AutoActions(robot, drive, TELE, servos, flywheel, spindexer, targeting, targetingSettings, turret);
+
                 xLeave = rLeaveX;
                 yLeave = rLeaveY;
                 hLeave = rLeaveH;
@@ -761,10 +177,23 @@ public class Auto_LT_Far extends LinearOpMode {
                 yStackPickupB = rStackPickupBY;
                 hStackPickupB = rStackPickupBH;
 
+                obeliskTurrPos1 = turrDefault + redObeliskTurrPos1;
+                obeliskTurrPos2 = turrDefault + redObeliskTurrPos2;
+                obeliskTurrPos3 = turrDefault + redObeliskTurrPos3;
                 turretShootPos = turrDefault + redTurretShootPos;
+
+                if (gamepad2.squareWasPressed()){
+                    turret.pipelineSwitch(4);
+                    robot.limelight.start();
+                    gamepad2.rumble(500);
+                }
             } else {
                 robot.light.setPosition(0.6);
 
+                autoStart = new Pose2d(autoStartBX, autoStartBY, Math.toRadians(autoStartBH));
+                drive = new MecanumDrive(hardwareMap, autoStart);
+                autoActions = new AutoActions(robot, drive, TELE, servos, flywheel, spindexer, targeting, targetingSettings, turret);
+
                 xLeave = bLeaveX;
                 yLeave = bLeaveY;
                 hLeave = bLeaveH;
@@ -781,7 +210,16 @@ public class Auto_LT_Far extends LinearOpMode {
                 yStackPickupB = bStackPickupBY;
                 hStackPickupB = bStackPickupBH;
 
+                obeliskTurrPos1 = turrDefault + blueObeliskTurrPos1;
+                obeliskTurrPos2 = turrDefault + blueObeliskTurrPos2;
+                obeliskTurrPos3 = turrDefault + blueObeliskTurrPos3;
                 turretShootPos = turrDefault + blueTurretShootPos;
+
+                if (gamepad2.squareWasPressed()){
+                    turret.pipelineSwitch(2);
+                    robot.limelight.start();
+                    gamepad2.rumble(500);
+                }
             }
 
             leave3Ball = drive.actionBuilder(autoStart)
@@ -798,6 +236,8 @@ public class Auto_LT_Far extends LinearOpMode {
             shoot3 = drive.actionBuilder(new Pose2d(xStackPickupB, yStackPickupB, Math.toRadians(hStackPickupB)))
                     .strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot));
 
+            limelightUsed = true;
+
             TELE.addData("Red?", redAlliance);
             TELE.addData("Turret Default", turrDefault);
             TELE.addData("Gate Cycle?", gatePickup);
@@ -814,16 +254,18 @@ public class Auto_LT_Far extends LinearOpMode {
         // Currently only shoots; keep this start and modify times and then add extra paths
         if (opModeIsActive()) {
 
-            double stamp = getRuntime();
-
             robot.transfer.setPower(1);
 
             startAuto();
+            shoot();
 
             if (stack3){
-                //cycleStackFar();
+                cycleStackFar();
+                shoot();
             }
 
+            //TODO: insert code here for gate auto
+
             if (gatePickup || stack3){
                 leave();
             } else {
@@ -838,6 +280,7 @@ public class Auto_LT_Far extends LinearOpMode {
                 teleStart = drive.localizer.getPose();
 
                 flywheel.manageFlywheel(0);
+                robot.transfer.setPower(0);
 
                 TELE.addLine("finished");
                 TELE.update();
@@ -847,25 +290,39 @@ public class Auto_LT_Far extends LinearOpMode {
 
     }
 
+    void shoot(){
+        Actions.runBlocking(
+                new ParallelAction(
+                        autoActions.manageShooterAuto(
+                                shootAllTime,
+                                0.501,
+                                0.501,
+                                0.501,
+                                0.501,
+                                0.501,
+                                false
+                        ),
+                        autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease)
+                )
+
+        );
+    }
+
     void startAuto(){
         Actions.runBlocking(
                 new ParallelAction(
-                        manageFlywheel(
+                        autoActions.manageShooterAuto(
-                                shoot0Vel,
-                                shoot0Hood,
                                 flywheel0Time,
                                 0.501,
                                 0.501,
-                                shoot0XTolerance,
+                                0.501,
-                                0.501
+                                0.501,
+                                0.501,
+                                false
                         )
 
                 )
         );
-
-        Actions.runBlocking(
-                shootAll((int) shoot0Vel, shoot0Time, shoot0SpinSpeedIncrease)
-        );
     }
 
     void leave3Ball(){
@@ -878,61 +335,51 @@ public class Auto_LT_Far extends LinearOpMode {
         Actions.runBlocking(leaveFromShoot.build());
     }
 
-//    void cycleStackFar(){
+    void cycleStackFar(){
-//        Actions.runBlocking(
+        Actions.runBlocking(
-//                new ParallelAction(
+                new ParallelAction(
-//                        pickup3.build(),
+                        pickup3.build(),
-//                        manageShooterAuto(
+                        autoActions.manageShooterAuto(
-//                                intake3Time,
+                                intakeStackTime,
-//                                0.501,
+                                xStackPickupB,
-//                                0.501,
+                                yStackPickupB,
-//                                0.501,
+                                posXTolerance,
-//                                0.501
+                                posYTolerance,
-//                        ),
+                                hStackPickupB,
-//                        intake(intake3Time),
+                                true
-//                        detectObelisk(
+                        ),
-//                                intake3Time,
+                        autoActions.intake(intakeStackTime),
-//                                0.501,
+                        autoActions.detectObelisk(
-//                                0.501,
+                                intakeStackTime,
-//                                0.501,
+                                xStackPickupB,
-//                                0.501,
+                                yStackPickupB,
-//                                obeliskTurrPos3
+                                posXTolerance,
-//                        )
+                                posYTolerance,
-//
+                                obeliskTurrPos3
-//                )
+                        )
-//        );
+
-//
+                )
-//        motif = turret.getObeliskID();
+        );
-//
+
-//        if (motif == 0) motif = prevMotif;
+        motif = turret.getObeliskID();
-//        prevMotif = motif;
+
-//
+        if (motif == 0) motif = 22;
-//        Actions.runBlocking(
+        prevMotif = motif;
-//                new ParallelAction(
+
-//                        manageFlywheelAuto(
+        Actions.runBlocking(
-//                                shoot3Time,
+                new ParallelAction(
-//                                0.501,
+                        autoActions.manageShooterAuto(
-//                                0.501,
+                                shootStackTime,
-//                                0.501,
+                                xShoot,
-//                                0.501
+                                yShoot,
-//                        ),
+                                posXTolerance,
-//                        shoot3.build(),
+                                posYTolerance,
-//                        prepareShootAll(colorSenseTime, shoot3Time, motif)
+                                hShoot,
-//                )
+                                false
-//        );
+                        ),
-//
+                        shoot3.build(),
-//        Actions.runBlocking(
+                        autoActions.prepareShootAll(colorSenseTime, shootStackTime, motif)
-//                new ParallelAction(
+                )
-//                        manageShooterAuto(
+        );
-//                                finalShootAllTime,
+    }
-//                                0.501,
-//                                0.501,
-//                                0.501,
-//                                0.501
-//                        ),
-//                        shootAllAuto(finalShootAllTime, finalSpindexerSpeedIncrease)
-//                )
-//
-//        );
-//    }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java

@@ -2,7 +2,6 @@ package org.firstinspires.ftc.teamcode.autonomous.actions;
 
 import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
 import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart;
-import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinEndPos;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1;
@@ -15,11 +14,13 @@ import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate;
 
 import androidx.annotation.NonNull;
 
+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.Pose2d;
 
+import org.firstinspires.ftc.teamcode.constants.ServoPositions;
 import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
 import org.firstinspires.ftc.teamcode.utils.Flywheel;
 import org.firstinspires.ftc.teamcode.utils.Robot;
@@ -30,6 +31,7 @@ import org.firstinspires.ftc.teamcode.utils.Turret;
 
 import java.util.Objects;
 
+@Config
 public class AutoActions{
     Robot robot;
     MultipleTelemetry TELE;
@@ -48,6 +50,7 @@ public class AutoActions{
     private boolean shootForward = true;
     public static double firstShootTime = 0.3;
     public int motif = 0;
+    double spinEndPos = ServoPositions.spinEndPos;
 
     public AutoActions(Robot rob, MecanumDrive dri, MultipleTelemetry tel, Servos ser, Flywheel fly, Spindexer spi, Targeting tar, Targeting.Settings tS, Turret tur){
         this.robot = rob;
@@ -60,6 +63,7 @@ public class AutoActions{
         this.targetingSettings = tS;
         this.turret = tur;
     }
+
     public Action prepareShootAll(double colorSenseTime, double time, int motif_id) {
         return new Action() {
             double stamp = 0.0;
@@ -69,6 +73,36 @@ public class AutoActions{
 
             boolean decideGreenSlot = false;
 
+            void spin1PosFirst(){
+                firstSpindexShootPos = spindexer_outtakeBall1;
+                shootForward = true;
+                spinEndPos = spindexer_outtakeBall3 + 0.1;
+            }
+
+            void spin2PosFirst(){
+                firstSpindexShootPos = spindexer_outtakeBall2;
+                shootForward = false;
+                spinEndPos = spindexer_outtakeBall3b - 0.1;
+            }
+
+            void reverseSpin2PosFirst(){
+                firstSpindexShootPos = spindexer_outtakeBall2;
+                shootForward = true;
+                spinEndPos = 0.95;
+            }
+
+            void spin3PosFirst(){
+                firstSpindexShootPos = spindexer_outtakeBall3;
+                shootForward = false;
+                spinEndPos = spindexer_outtakeBall1 - 0.1;
+            }
+
+            void oddSpin3PosFirst(){
+                firstSpindexShootPos = spindexer_outtakeBall3b;
+                shootForward = true;
+                spinEndPos = spindexer_outtakeBall2 + 0.1;
+            }
+
             @Override
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
                 if (ticker == 0) {
@@ -80,29 +114,6 @@ public class AutoActions{
 
                 teleStart = drive.localizer.getPose();
 
-                double robX = drive.localizer.getPose().position.x;
-                double robY = drive.localizer.getPose().position.y;
-                double robotHeading = drive.localizer.getPose().heading.toDouble();
-
-                double goalX = -15;
-                double goalY = 0;
-
-                double dx = robX - goalX;  // delta x from robot to goal
-                double dy = robY - 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
-                        (robX, robY, robotHeading, 0.0, turretInterpolate);
-
-                turret.trackGoal(deltaPose);
-
-                TELE.addData("Velocity", flywheel.getVelo());
-                TELE.addData("Hood", robot.hood.getPosition());
-                TELE.addData("motif", motif_id);
-                TELE.update();
-
                 if ((System.currentTimeMillis() - stamp) < (colorSenseTime * 1000)) {
 
                     spindexerWiggle *= -1.0;
@@ -142,45 +153,33 @@ public class AutoActions{
 
                     if (motif_id == 21) {
                         if (mostGreenSlot == 1) {
-                            firstSpindexShootPos = spindexer_outtakeBall1;
+                            spin1PosFirst();
-                            shootForward = true;
                         } else if (mostGreenSlot == 2) {
-                            firstSpindexShootPos = spindexer_outtakeBall2;
+                            spin2PosFirst();
-                            shootForward = false;
                         } else {
-                            firstSpindexShootPos = spindexer_outtakeBall3;
+                            spin3PosFirst();
-                            shootForward = false;
                         }
                     } else if (motif_id == 22) {
                         if (mostGreenSlot == 1) {
-                            firstSpindexShootPos = spindexer_outtakeBall2;
+                            spin2PosFirst();
-                            shootForward = false;
                         } else if (mostGreenSlot == 2) {
-                            firstSpindexShootPos = spindexer_outtakeBall3;
+                            spin3PosFirst();
-                            shootForward = false;
                         } else {
-                            firstSpindexShootPos = spindexer_outtakeBall2;
+                            reverseSpin2PosFirst();
-                            shootForward = true;
                         }
 
                     } else {
                         if (mostGreenSlot == 1) {
-                            firstSpindexShootPos = spindexer_outtakeBall3;
+                            spin3PosFirst();
-                            shootForward = false;
                         } else if (mostGreenSlot == 2) {
-                            firstSpindexShootPos = spindexer_outtakeBall3b;
+                            oddSpin3PosFirst();
-                            shootForward = true;
                         } else {
-                            firstSpindexShootPos = spindexer_outtakeBall1;
+                            spin1PosFirst();
-                            shootForward = true;
                         }
-
                     }
 
                     return true;
                 } else if ((System.currentTimeMillis() - stamp) < (time * 1000)) {
-//                    TELE.addData("MostGreenSlot", mostGreenSlot);
-//                    TELE.update();
                     spindexer.setIntakePower(-((System.currentTimeMillis() - stamp - colorSenseTime)) / 1000);
 
                     servos.setSpinPos(firstSpindexShootPos);
@@ -194,85 +193,7 @@ public class AutoActions{
         };
     }
 
-    public Action shootAll(int vel, double shootTime, double spindexSpeed) {
+    private boolean doneShooting = false;
-        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", flywheel.getVelo());
-                TELE.addData("Hood", robot.hood.getPosition());
-                TELE.update();
-
-                double voltage = robot.voltage.getVoltage();
-                flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
-                flywheel.manageFlywheel(vel);
-                velo = flywheel.getVelo();
-
-                drive.updatePoseEstimate();
-
-                teleStart = drive.localizer.getPose();
-
-                spindexer.setIntakePower(-0.3);
-
-                if (ticker == 1) {
-                    stamp = System.currentTimeMillis();
-                }
-                ticker++;
-
-                spindexer.setIntakePower(0);
-                drive.updatePoseEstimate();
-
-                teleStart = drive.localizer.getPose();
-
-                double robX = drive.localizer.getPose().position.x;
-                double robY = drive.localizer.getPose().position.y;
-                double robotHeading = drive.localizer.getPose().heading.toDouble();
-
-                double goalX = -15;
-                double goalY = 0;
-
-                double dx = robX - goalX;  // delta x from robot to goal
-                double dy = robY - 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
-                        (robX, robY, robotHeading, 0.0, turretInterpolate);
-
-                turret.trackGoal(deltaPose);
-
-                if ((System.currentTimeMillis() - stamp < shootTime && servos.getSpinPos() < spinEndPos) || shooterTicker == 0) {
-
-                    if (shooterTicker == 0 && !servos.spinEqual(spinStartPos)) {
-                        servos.setSpinPos(spinStartPos);
-                    } else {
-                        servos.setTransferPos(transferServo_in);
-                        shooterTicker++;
-                        double prevSpinPos = servos.getSpinCmdPos();
-                        servos.setSpinPos(prevSpinPos + spindexSpeed);
-                    }
-
-                    return true;
-
-                } else {
-                    servos.setTransferPos(transferServo_out);
-
-                    spindexer.resetSpindexer();
-                    spindexer.processIntake();
-
-                    return false;
-
-                }
-
-            }
-        };
-    }
-
     public Action shootAllAuto(double shootTime, double spindexSpeed) {
         return new Action() {
             int ticker = 1;
@@ -285,9 +206,6 @@ public class AutoActions{
 
             @Override
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
-                TELE.addData("Velocity", flywheel.getVelo());
-                TELE.addData("Hood", robot.hood.getPosition());
-                TELE.update();
 
                 velo = flywheel.getVelo();
 
@@ -295,51 +213,37 @@ public class AutoActions{
 
                 teleStart = drive.localizer.getPose();
 
-                spindexer.setIntakePower(-0.3);
+                spindexer.setIntakePower(-0.1);
 
                 if (ticker == 1) {
                     stamp = System.currentTimeMillis();
                 }
                 ticker++;
 
-                spindexer.setIntakePower(0);
+                double prevSpinPos = servos.getSpinCmdPos();
-                drive.updatePoseEstimate();
 
-                teleStart = drive.localizer.getPose();
+                boolean end;
+                if (shootForward){
+                    end = prevSpinPos > spinEndPos;
+                } else {
+                    end = prevSpinPos < spinEndPos;
+                }
 
-                double robX = drive.localizer.getPose().position.x;
+                if (System.currentTimeMillis() - stamp < shootTime*1000 && (!end || shooterTicker < 2)) {
-                double robY = drive.localizer.getPose().position.y;
-                double robotHeading = drive.localizer.getPose().heading.toDouble();
 
-                double goalX = -15;
+                    if (!servos.spinEqual(firstSpindexShootPos) && shooterTicker < 3) {
-                double goalY = 0;
-
-                double dx = robX - goalX;  // delta x from robot to goal
-                double dy = robY - 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
-                        (robX, robY, robotHeading, 0.0, turretInterpolate);
-
-                turret.trackGoal(deltaPose);
-
-                if (System.currentTimeMillis() - stamp < shootTime) {
-
-                    if (System.currentTimeMillis() - stamp < firstShootTime) {
                         servos.setTransferPos(transferServo_out);
                         servos.setSpinPos(firstSpindexShootPos);
                     } else {
                         servos.setTransferPos(transferServo_in);
                         shooterTicker++;
-                        double prevSpinPos = servos.getSpinCmdPos();
 
                         if (shootForward) {
                             servos.setSpinPos(prevSpinPos + spindexSpeed);
                         } else {
                             servos.setSpinPos(prevSpinPos - spindexSpeed);
                         }
+
                     }
 
                     return true;
@@ -349,6 +253,7 @@ public class AutoActions{
 
                     spindexer.resetSpindexer();
                     spindexer.processIntake();
+                    doneShooting = true;
 
                     return false;
 
@@ -377,14 +282,12 @@ public class AutoActions{
 
                 teleStart = drive.localizer.getPose();
 
-                TELE.addData("Full?", spindexer.isFull());
-                TELE.update();
-
                 return ((System.currentTimeMillis() - stamp) < (intakeTime * 1000)) && !spindexer.isFull();
             }
         };
     }
 
+    private boolean detectingObelisk = false;
     public Action detectObelisk(
             double time,
             double posX,
@@ -405,13 +308,13 @@ public class AutoActions{
 
             @Override
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
-
+                detectingObelisk = true;
                 drive.updatePoseEstimate();
                 Pose2d currentPose = drive.localizer.getPose();
 
                 if (ticker == 0) {
                     stamp = System.currentTimeMillis();
-                    robot.limelight.pipelineSwitch(1);
+                    turret.pipelineSwitch(1);
                 }
 
                 ticker++;
@@ -423,20 +326,17 @@ public class AutoActions{
                 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;
+                boolean shouldFinish = timeDone || (xDone && yDone) || spindexer.isFull();
-                drive.updatePoseEstimate();
 
-                teleStart = drive.localizer.getPose();
+                teleStart = currentPose;
-                TELE.addData("Velocity", flywheel.getVelo());
-                TELE.addData("Hood", robot.hood.getPosition());
-                TELE.update();
 
                 if (shouldFinish){
                     if (redAlliance){
-                        robot.limelight.pipelineSwitch(4);
+                        turret.pipelineSwitch(4);
                     } else {
-                        robot.limelight.pipelineSwitch(2);
+                        turret.pipelineSwitch(2);
                     }
+                    detectingObelisk = false;
                     return false;
                 } else {
                     return true;
@@ -446,76 +346,26 @@ public class AutoActions{
         };
     }
 
-    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++;
-
-                double voltage = robot.voltage.getVoltage();
-                flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
-                flywheel.manageFlywheel(vel);
-                servos.setHoodPos(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;
-                drive.updatePoseEstimate();
-
-                teleStart = drive.localizer.getPose();
-                TELE.addData("Velocity", flywheel.getVelo());
-                TELE.addData("Hood", robot.hood.getPosition());
-                TELE.update();
-
-                return !shouldFinish;
-
-            }
-        };
-    }
-
     public Action manageShooterAuto(
             double time,
             double posX,
             double posY,
             double posXTolerance,
-            double posYTolerance
+            double posYTolerance,
+            double posH,
+            boolean whileIntaking
     ) {
 
-        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;
+            int shootingTicker = 0;
+            double shootingStamp = 0;
+
+            final boolean timeFallback = (time != 0.501);
+            final boolean posXFallback = (posX != 0.501);
+            final boolean posYFallback = (posY != 0.501);
 
             @Override
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
@@ -529,96 +379,32 @@ public class AutoActions{
 
                 ticker++;
 
-                double robotX = drive.localizer.getPose().position.x;
+                double robotX = currentPose.position.x;
-                double robotY = drive.localizer.getPose().position.y;
+                double robotY = currentPose.position.y;
 
-                double robotHeading = drive.localizer.getPose().heading.toDouble();
+                double robotHeading = currentPose.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);
+                Pose2d deltaPose;
-
+                if (posX != 0.501) {
-                double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
+                    deltaPose = new Pose2d(posX, posY, Math.toRadians(posH));
-
+                } else {
-                targetingSettings = targeting.calculateSettings
+                    deltaPose = new Pose2d(robotX, robotY, robotHeading);
-                        (robotX, robotY, robotHeading, 0.0, false);
-
-                turret.trackGoal(deltaPose);
-
-                servos.setHoodPos(targetingSettings.hoodAngle);
-
-                double voltage = robot.voltage.getVoltage();
-                flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
-                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;
-                drive.updatePoseEstimate();
-
-                teleStart = drive.localizer.getPose();
-
-                TELE.addData("Velocity", flywheel.getVelo());
-                TELE.addData("Hood", robot.hood.getPosition());
-                TELE.update();
-
-                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);
 
+                if (!detectingObelisk) {
+                    turret.trackGoal(deltaPose);
+                }
+
                 servos.setHoodPos(targetingSettings.hoodAngle);
 
                 double voltage = robot.voltage.getVoltage();
@@ -626,18 +412,26 @@ public class AutoActions{
                 flywheel.manageFlywheel(targetingSettings.flywheelRPM);
 
                 boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000;
-                boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance;
+                boolean xDone = posXFallback && Math.abs(robotX - posX) < posXTolerance;
-                boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance;
+                boolean yDone = posYFallback && Math.abs(robotY - posY) < posYTolerance;
+                boolean shouldFinish;
+                if (whileIntaking) {
+                    shouldFinish = timeDone || (xDone && yDone) || spindexer.isFull();
+                } else {
+                    shouldFinish = timeDone || (xDone && yDone) || doneShooting;
+                }
 
-                boolean shouldFinish = timeDone || xDone || yDone;
+                teleStart = currentPose;
-                drive.updatePoseEstimate();
 
-                teleStart = drive.localizer.getPose();
+                TELE.addData("Steady?", flywheel.getSteady());
-                TELE.addData("Velocity", flywheel.getVelo());
-                TELE.addData("Hood", robot.hood.getPosition());
                 TELE.update();
 
-                return !shouldFinish;
+                if (shouldFinish) {
+                    doneShooting = false;
+                    return false;
+                } else {
+                    return true;
+                }
 
             }
         };

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

@@ -5,11 +5,11 @@ import com.acmerobotics.roadrunner.Pose2d;
 
 @Config
 public class Back_Poses {
-    public static double rLeaveX = 90, rLeaveY = 80, rLeaveH = 50.1;
+    public static double rLeaveX = 90, rLeaveY = 50, rLeaveH = 50.1;
-    public static double bLeaveX = 90, bLeaveY = -80, bLeaveH = -50;
+    public static double bLeaveX = 90, bLeaveY = -50, bLeaveH = -50;
 
-    public static double rShootX = 95, rShootY = 85, rShootH = 90;
+    public static double rShootX = 100, rShootY = 55, rShootH = 90;
-    public static double bShootX = 95, bShootY = -85, bShootH = -90;
+    public static double bShootX = 100, bShootY = -55, bShootH = -90;
 
     public static double rStackPickupAX = 75, rStackPickupAY = 53, rStackPickupAH = 140;
     public static double bStackPickupAX = 75, bStackPickupAY = -53, bStackPickupAH = -140;
@@ -17,7 +17,7 @@ public class Back_Poses {
     public static double rStackPickupBX = 50, rStackPickupBY = 78, rStackPickupBH = 140.1;
     public static double bStackPickupBX = 50, bStackPickupBY = -78, bStackPickupBH = -140.1;
 
-    public static Pose2d autoStart = new Pose2d(0, 0, 0); // TODO: find this position
+    public static double autoStartRX = 103, autoStartRY = 63.5, autoStartRH = 50;
-
+    public static double autoStartBX = 103, autoStartBY = -63.5, autoStartBH = -50;
 
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java

@@ -116,10 +116,10 @@ public class TeleopV3 extends LinearOpMode {
         while (opModeInInit()) {
             robot.limelight.start();
             if (redAlliance) {
-                robot.limelight.pipelineSwitch(4);
+                turret.pipelineSwitch(4);
                 light.setManualLightColor(Color.LightRed);
             } else {
-                robot.limelight.pipelineSwitch(2);
+                turret.pipelineSwitch(2);
                 light.setManualLightColor(Color.LightBlue);
 
             }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java

@@ -38,8 +38,8 @@ public class Flywheel {
     }
 
     // Set the robot PIDF for the next cycle.
-    private double prevF = 0.501;
+    private double prevF = 0;
-    public static int voltagePIDFDifference = 5;
+    public static double voltagePIDFDifference = 0.8;
     public void setPIDF(double p, double i, double d, double f) {
         shooterPIDF1.p = p;
         shooterPIDF1.i = i;
@@ -78,7 +78,7 @@ public class Flywheel {
 
         }
         // really should be a running average of the last 5
-        steady = (Math.abs(targetVelocity - velo) < 200.0);
+        steady = (Math.abs(commandedVelocity - velo) < 200.0);
 
         return powPID;
     }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java

@@ -1,5 +1,7 @@
 package org.firstinspires.ftc.teamcode.utils;
 
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodOffset;
+
 import com.acmerobotics.dashboard.config.Config;
 import com.arcrobotics.ftclib.controller.PIDFController;
 import com.qualcomm.robotcore.hardware.HardwareMap;
@@ -72,7 +74,7 @@ public class Servos {
 
     public double setHoodPos(double pos){
         if (firstHoodPos || !servoPosEqual(pos, prevHoodPos)) {
-            robot.hood.setPosition(pos);
+            robot.hood.setPosition(pos + hoodOffset);
             firstHoodPos = false;
         }
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java

@@ -186,7 +186,7 @@ public class Targeting {
         if (true) { //!interpolate) {
             if ((robotGridY < 6) && (robotGridX < 6)) {
                 recommendedSettings.flywheelRPM = KNOWNTARGETING[robotGridX][robotGridY].flywheelRPM;
-                recommendedSettings.hoodAngle = KNOWNTARGETING[robotGridX][robotGridY].hoodAngle + ServoPositions.hoodOffset;
+                recommendedSettings.hoodAngle = KNOWNTARGETING[robotGridX][robotGridY].hoodAngle;
             }
             return recommendedSettings;
         } else {

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java

@@ -14,6 +14,7 @@ import com.qualcomm.robotcore.util.ElapsedTime;
 
 import org.firstinspires.ftc.robotcore.external.navigation.Pose3D;
 import org.firstinspires.ftc.teamcode.constants.Color;
+import org.firstinspires.ftc.teamcode.constants.StateEnums;
 
 import java.util.List;
 
@@ -24,7 +25,7 @@ public class Turret {
     public static double turretTolerance = 0.02;
     public static double turrPosScalar = 0.00011264432;
     public static double turret180Range = 0.4;
-    public static double turrDefault = 0.39;
+    public static double turrDefault = 0.37;
     public static double turrMin = 0.15;
     public static double turrMax = 0.85;
     public static boolean limelightUsed = true;
@@ -48,6 +49,7 @@ public class Turret {
     double limelightPosX = 0.0;
     double limelightPosY = 0.0;
     LLResult result;
+
     boolean bearingAligned = false;
     private boolean lockOffset = false;
     private int obeliskID = 0;
@@ -56,6 +58,7 @@ public class Turret {
     private double lightColor = Color.LightRed;
     private int currentTrackCount = 0;
     private double permanentOffset = 0.0;
+    private int prevPipeline = -1;
     private PIDController bearingPID;
 
     private double prevTurretPos = 0.0;
@@ -81,14 +84,22 @@ public class Turret {
         return turrPosScalar * (robot.turr1Pos.getVoltage() / 3.3) + turrDefault;
 
     }
-    private double prevTurrPos = 0.501;
+    private double prevTurrPos = 0;
+    private boolean isFirstTurretPos = true;
     public void setTurret(double pos) {
-        if (prevTurrPos != 0.501 && prevTurrPos != pos){
+        if (isFirstTurretPos || prevTurrPos != pos){
             robot.turr1.setPosition(pos);
             robot.turr2.setPosition(1-pos);
+            isFirstTurretPos = false;
         }
         prevTurrPos = pos;
     }
+    public void pipelineSwitch(int pipeline){
+        if (prevPipeline != pipeline){
+            robot.limelight.pipelineSwitch(pipeline);
+        }
+        prevPipeline = pipeline;
+    }
 
     public boolean turretEqual(double pos) {
         return Math.abs(pos - this.getTurrPos()) < turretTolerance;