auto actions class tested: works but needs remodeling

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

@@ -3,25 +3,13 @@ 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.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,15 @@ 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 autoSpinStartPos = 0.2;
-    public static double shoot0SpinSpeedIncrease = 0.02;
+    public static double shoot0SpinSpeedIncrease = 0.005;
 
-    public static double spindexerSpeedIncrease = 0.03;
+    public static double spindexerSpeedIncrease = 0.005;
-    public static double finalSpindexerSpeedIncrease = 0.03;
+    public static double finalSpindexerSpeedIncrease = 0.005;
 
     // These values are ADDED to turrDefault
     public static double redObeliskTurrPos1 = 0.12;
@@ -111,8 +97,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 +116,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,8 +149,6 @@ 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);

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

@@ -31,6 +31,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;
@@ -64,6 +65,7 @@ public class Auto_LT_Far extends LinearOpMode {
     Turret turret;
     Targeting targeting;
     Targeting.Settings targetingSettings;
+    AutoActions autoActions;
     double firstSpindexShootPos = autoSpinStartPos;
     boolean shootForward = true;
     double xShoot, yShoot, hShoot;
@@ -83,588 +85,7 @@ public class Auto_LT_Far extends LinearOpMode {
     public static int pickupStackSpeed = 15;
     int prevMotif = 0;
 
-    public Action prepareShootAll(double colorSenseTime, double time, int motif_id) {
-        return new Action() {
-            double stamp = 0.0;
-            int ticker = 0;
 
-            double spindexerWiggle = 0.01;
-
-            boolean decideGreenSlot = false;
-
-            @Override
-            public boolean run(@NonNull TelemetryPacket telemetryPacket) {
-                if (ticker == 0) {
-                    stamp = System.currentTimeMillis();
-                }
-                ticker++;
-                servos.setTransferPos(transferServo_out);
-                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);
-
-                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;
@@ -700,6 +121,8 @@ public class Auto_LT_Far extends LinearOpMode {
 
         drive = new MecanumDrive(hardwareMap, autoStart);
 
+        autoActions = new AutoActions(robot, drive, TELE, servos, flywheel, spindexer, targeting, targetingSettings, turret);
+
         servos.setSpinPos(autoSpinStartPos);
 
         servos.setTransferPos(transferServo_out);
@@ -850,7 +273,7 @@ public class Auto_LT_Far extends LinearOpMode {
     void startAuto(){
         Actions.runBlocking(
                 new ParallelAction(
-                        manageFlywheel(
+                        autoActions.manageFlywheel(
                                 shoot0Vel,
                                 shoot0Hood,
                                 flywheel0Time,
@@ -864,7 +287,7 @@ public class Auto_LT_Far extends LinearOpMode {
         );
 
         Actions.runBlocking(
-                shootAll((int) shoot0Vel, shoot0Time, shoot0SpinSpeedIncrease)
+                autoActions.shootAll((int) shoot0Vel, shoot0Time, shoot0SpinSpeedIncrease)
         );
     }
 

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

@@ -15,6 +15,7 @@ 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;
@@ -30,6 +31,7 @@ import org.firstinspires.ftc.teamcode.utils.Turret;
 
 import java.util.Objects;
 
+@Config
 public class AutoActions{
     Robot robot;
     MultipleTelemetry TELE;
@@ -246,7 +248,7 @@ public class AutoActions{
 
                 turret.trackGoal(deltaPose);
 
-                if ((System.currentTimeMillis() - stamp < shootTime && servos.getSpinPos() < spinEndPos) || shooterTicker == 0) {
+                if ((System.currentTimeMillis() - stamp < shootTime*1000 && servos.getSpinPos() < spinEndPos) || shooterTicker == 0) {
 
                     if (shooterTicker == 0 && !servos.spinEqual(spinStartPos)) {
                         servos.setSpinPos(spinStartPos);
@@ -325,9 +327,9 @@ public class AutoActions{
 
                 turret.trackGoal(deltaPose);
 
-                if (System.currentTimeMillis() - stamp < shootTime) {
+                if (System.currentTimeMillis() - stamp < shootTime*1000) {
 
-                    if (System.currentTimeMillis() - stamp < firstShootTime) {
+                    if (System.currentTimeMillis() - stamp < firstShootTime*1000) {
                         servos.setTransferPos(transferServo_out);
                         servos.setSpinPos(firstSpindexShootPos);
                     } else {

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

@@ -81,12 +81,16 @@ 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;
         }
+        TELE.addLine("Moved Turret");
+        TELE.update();
         prevTurrPos = pos;
     }