Added spindexer to teleopv4

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

@@ -5,6 +5,19 @@ import com.acmerobotics.dashboard.config.Config;
 @Config
 public class ServoPositions {
 
+    public static double rapidFireBlocker_Closed = 0.3;
+    public static double rapidFireBlocker_Open = 0.5;
+
+    public static double spindexBlocker_Closed = 0.31;
+    public static double spindexBlocker_Open = 0.5;
+
+    public static double spindexer_A1 = 0.16;
+    public static double spindexer_A2 = 0.35;
+    public static double spindexer_A3 = 0.54;
+    public static double spindexer_B1 = 0.73;
+    public static double spindexer_B2 = 0.92;
+
+
     public static double spindexer_intakePos1 = 0.18; //0.13;
 
     public static double spindexer_intakePos2 = 0.37; //0.33;//0.5;
@@ -21,13 +34,13 @@ public class ServoPositions {
 
     public static double shootAllSpindexerSpeedIncrease = 0.01;
 
-    public static double transferServo_out = 0.15;
+    public static double transferServo_out = 0.57;
 
-    public static double transferServo_in = 0.38;
+    public static double transferServo_in = 0.77;
 
     public static double hoodAuto = 0.27;
 
-    public static double hoodOffset = -0.05; // offset from 0.93 (or position at 0,0 in targeting class)
+    public static double hoodOffset = 0; // offset from 0.93 (or position at 0,0 in targeting class)
 
     public static double turret_redClose = 0;
     public static double turret_blueClose = 0;
@@ -44,4 +57,10 @@ public class ServoPositions {
     public static double redTurretShootPos = 0.05;
     public static double blueTurretShootPos = -0.05;
 
+    public static double tilt1_down = 0.6;
+    public static double tilt2_down = 0.4;
+    public static double tilt1_up = 0.08;
+    public static double tilt2_up = 0.97;
+
+
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java

@@ -17,13 +17,10 @@ import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
 @Config
 public class Constants {
     public static FollowerConstants followerConstants = new FollowerConstants()
-            .mass(15.5)
+            .mass(14.37888)
             .forwardZeroPowerAcceleration(-29.512)
             .lateralZeroPowerAcceleration(-72.872)
-            .translationalPIDFCoefficients(new PIDFCoefficients(0.35, 0, 0.03, 0.012))
-            .headingPIDFCoefficients(new PIDFCoefficients(1, 0, 0.025, 0.02))
-            .drivePIDFCoefficients(new FilteredPIDFCoefficients(0.03, 0, 0, 0.6, 0.03))
-            .centripetalScaling(0.0005);
+            .centripetalScaling(0);
 
     public static MecanumConstants driveConstants = new MecanumConstants()
             .maxPower(1)
@@ -42,8 +39,8 @@ public class Constants {
     public static PathConstraints pathConstraints = new PathConstraints(0.99, 100, breakingStrength, 1);
 
     public static PinpointConstants localizerConstants = new PinpointConstants()
-            .forwardPodY(-7.5)
-            .strafePodX(-3.75)
+            .forwardPodY(-3.676)
+            .strafePodX(3.780)
             .distanceUnit(DistanceUnit.INCH)
             .hardwareMapName("pinpoint")
             .encoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD)

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

@@ -1,32 +1,49 @@
 package org.firstinspires.ftc.teamcode.teleop;
 
+import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseH;
+import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseX;
+import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseY;
+
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.pedropathing.follower.Follower;
+import com.pedropathing.geometry.Pose;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
-import org.firstinspires.ftc.teamcode.utilsv2.Drivetrain;
-import org.firstinspires.ftc.teamcode.utils.Robot;
+import org.firstinspires.ftc.teamcode.constants.Color;
+import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
+import org.firstinspires.ftc.teamcode.utilsv2.*;
 
 @TeleOp
 @Config
 public class TeleopV4 extends LinearOpMode {
     Robot robot;
     Drivetrain drivetrain;
+    Shooter shooter;
     MultipleTelemetry TELE;
+    Follower follower;
+    SpindexerTransferIntake spindexerTransferIntake;
     @Override
     public void runOpMode() throws InterruptedException {
 
-        robot = new Robot(hardwareMap);
+        robot = Robot.getInstance(hardwareMap);
 
         TELE = new MultipleTelemetry(
                 FtcDashboard.getInstance().getTelemetry(), telemetry
         );
 
         drivetrain = new Drivetrain(robot, TELE);
+        follower = Constants.createFollower(hardwareMap);
+        Pose start = new Pose(teleStartPoseX, teleStartPoseY, Math.toRadians(teleStartPoseH));
+        follower.setStartingPose(start);
+
+        shooter = new Shooter(robot, TELE, follower, Color.redAlliance);
+        shooter.setState(Shooter.ShooterState.TRACK_GOAL);
+        spindexerTransferIntake = new SpindexerTransferIntake(robot, TELE);
+        spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID);
 
-        drivetrain.setTelemetry(true);
 
         waitForStart();
 
@@ -42,6 +59,43 @@ public class TeleopV4 extends LinearOpMode {
                     gamepad1.left_stick_x
             );
 
+            shooter.update();
+            spindexerTransferIntake.update();
+
+            SpindexerTransferIntake.RapidMode state = spindexerTransferIntake.getRapidState();
+
+            if (gamepad1.xWasPressed() &&
+                    (state == SpindexerTransferIntake.RapidMode.INTAKE ||
+                            state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF ||
+                            state == SpindexerTransferIntake.RapidMode.BEFORE_PULSE_OUT ||
+                            state == SpindexerTransferIntake.RapidMode.PULSE_OUT ||
+                            state == SpindexerTransferIntake.RapidMode.PULSE_IN ||
+                            state == SpindexerTransferIntake.RapidMode.HOLD_BALLS)) {
+
+                spindexerTransferIntake.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
+            }
+
+            if (gamepad1.aWasPressed() &&
+                    (state == SpindexerTransferIntake.RapidMode.INTAKE ||
+                            state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF)) {
+
+                spindexerTransferIntake.setRapidMode(
+                        SpindexerTransferIntake.RapidMode.HOLD_BALLS
+                );
+            }
+
+            if (gamepad1.yWasPressed()
+                    && state == SpindexerTransferIntake.RapidMode.HOLD_BALLS) {
+
+                spindexerTransferIntake.setRapidMode(
+                        SpindexerTransferIntake.RapidMode.INTAKE
+                );
+            }
+
+
+
+
+
             TELE.update();
         }
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/Hardware_Tester.java

@@ -4,6 +4,7 @@ import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.NormalizedRGBA;
 
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
@@ -52,6 +53,11 @@ public class Hardware_Tester extends LinearOpMode {
         robot = new Robot(hardwareMap);
         TELE = new MultipleTelemetry();
 
+        robot.shooter1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        robot.shooter2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+        waitForStart();
+
         if (isStopRequested()) return;
         while (opModeIsActive()){
             // Non-subsystem based components
@@ -116,9 +122,9 @@ public class Hardware_Tester extends LinearOpMode {
 
             // Sensor Data
 
-            TELE.addData("Beam Break 1?", robot.beam1.isPressed());
-            TELE.addData("Beam Break 2?", robot.beam2.isPressed());
-            TELE.addData("Beam Break 3?", robot.beam3.isPressed());
+//            TELE.addData("Beam Break 1?", robot.beam1.isPressed());
+//            TELE.addData("Beam Break 2?", robot.beam2.isPressed());
+//            TELE.addData("Beam Break 3?", robot.beam3.isPressed());
 
             NormalizedRGBA revColor = robot.revSensor.getNormalizedColors();
             TELE.addData("REV Distance", robot.revSensor.getDistance(DistanceUnit.MM));

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

@@ -83,6 +83,7 @@ public class Robot {
         backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
 
         intake = hardwareMap.get(DcMotorEx.class, "intake");
+        intake.setDirection(DcMotorSimple.Direction.REVERSE);
 
         shooter1 = hardwareMap.get(DcMotorEx.class, "shooter1");
 
@@ -103,11 +104,12 @@ public class Robot {
 
         turr2 = hardwareMap.get(Servo.class, "turr2");
 
-        spin1 = hardwareMap.get(Servo.class, "spin2");
+        spin1 = hardwareMap.get(Servo.class, "spin1");
 
-        spin2 = hardwareMap.get(Servo.class, "spin1");
+        spin2 = hardwareMap.get(Servo.class, "spin2");
 
         transfer = hardwareMap.get(DcMotorEx.class, "transfer");
+        transfer.setDirection(DcMotorSimple.Direction.REVERSE);
 
         transferServo = hardwareMap.get(Servo.class, "transferServo");
 
@@ -121,27 +123,27 @@ public class Robot {
         tilt1 = hardwareMap.get(Servo.class, "tilt1");
         tilt2 = hardwareMap.get(Servo.class, "tilt2");
 
-        beam1 = hardwareMap.get(TouchSensor.class, "beam1");
-        beam2 = hardwareMap.get(TouchSensor.class, "beam2");
-        beam3 = hardwareMap.get(TouchSensor.class, "beam3");
+//        beam1 = hardwareMap.get(TouchSensor.class, "beam1");
+//        beam2 = hardwareMap.get(TouchSensor.class, "beam2");
+//        beam3 = hardwareMap.get(TouchSensor.class, "beam3");
 
         revSensor = hardwareMap.get(RevColorSensorV3.class, "rev");
 
         // Below is disregarded
 
-        turr1Pos = hardwareMap.get(AnalogInput.class, "t1Pos"); // Encoder of turret plugged in intake port
-
-        spin1Pos = hardwareMap.get(AnalogInput.class, "spin1Pos");
-
-        spin2Pos = hardwareMap.get(AnalogInput.class, "spin2Pos");
-
-        transferServoPos = hardwareMap.get(AnalogInput.class, "tSPos");
-
-        color1 = hardwareMap.get(RevColorSensorV3.class, "c1");
-
-        color2 = hardwareMap.get(RevColorSensorV3.class, "c2");
-
-        color3 = hardwareMap.get(RevColorSensorV3.class, "c3");
+//        turr1Pos = hardwareMap.get(AnalogInput.class, "t1Pos"); // Encoder of turret plugged in intake port
+//
+//        spin1Pos = hardwareMap.get(AnalogInput.class, "spin1Pos");
+//
+//        spin2Pos = hardwareMap.get(AnalogInput.class, "spin2Pos");
+//
+//        transferServoPos = hardwareMap.get(AnalogInput.class, "tSPos");
+//
+//        color1 = hardwareMap.get(RevColorSensorV3.class, "c1");
+//
+//        color2 = hardwareMap.get(RevColorSensorV3.class, "c2");
+//
+//        color3 = hardwareMap.get(RevColorSensorV3.class, "c3");
 
         if (usingLimelight) {
             limelight = hardwareMap.get(Limelight3A.class, "limelight");
@@ -150,7 +152,8 @@ public class Robot {
             aprilTagProcessor = AprilTagProcessor.easyCreateWithDefaults();
         }
 
-        light = hardwareMap.get(Servo.class, "light");
+//        light = hardwareMap.get(Servo.class, "light");
+
         voltage = hardwareMap.voltageSensor.iterator().next();
     }
 

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

@@ -25,10 +25,10 @@ public class Turret {
 
     public static double turretTolerance = 0.02;
     public static double turrPosScalar = 0.00011264432;
-    public static double turret180Range = 0.55;
-    public static double turrDefault = 0.35;
-    public static double turrMin = 0;
-    public static double turrMax = 0.69;
+    public static double turret180Range = 0.58;
+    public static double turrDefault = 0.51;
+    public static double turrMin = 0.05;
+    public static double turrMax = 0.95;
     public static boolean limelightUsed = true;
     public static double limelightPosOffset = 5;
     public static double manualOffset = 0.0;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Drivetrain.java

@@ -4,8 +4,6 @@ import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.qualcomm.robotcore.hardware.DcMotor;
 
-import org.firstinspires.ftc.teamcode.utils.Robot;
-
 @Config
 public class Drivetrain {
 
@@ -18,8 +16,8 @@ public class Drivetrain {
 
     private static final double STRAFE_MULTIPLIER = 1.2;
 
-    public static double FORWARD_ROTATION_CORRECTION = 0.03;
-    public static double STRAFE_ROTATION_CORRECTION = -0.03;
+    public static double FORWARD_ROTATION_CORRECTION = 0;
+    public static double STRAFE_ROTATION_CORRECTION = -0;
 
     private boolean tele = false;
 

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

@@ -5,7 +5,7 @@ import com.qualcomm.robotcore.hardware.HardwareMap;
 
 import com.qualcomm.robotcore.hardware.PIDFCoefficients;
 
-import org.firstinspires.ftc.teamcode.utils.Robot;
+import org.firstinspires.ftc.teamcode.utilsv2.Robot;
 
 import java.util.LinkedList;
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java

@@ -0,0 +1,311 @@
+package org.firstinspires.ftc.teamcode.utilsv2;
+
+import com.qualcomm.hardware.limelightvision.Limelight3A;
+import com.qualcomm.hardware.rev.RevColorSensorV3;
+import com.qualcomm.robotcore.hardware.AnalogInput;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.PIDFCoefficients;
+import com.qualcomm.robotcore.hardware.Servo;
+import com.qualcomm.robotcore.hardware.TouchSensor;
+import com.qualcomm.robotcore.hardware.VoltageSensor;
+
+import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
+import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
+
+public class Robot {
+    // Singleton instance
+    private static Robot instance;
+
+    /**
+     * Returns the existing Robot instance or creates one if it doesn't exist.
+     */
+    public static Robot getInstance(HardwareMap hardwareMap) {
+        if (instance == null) {
+            instance = new Robot(hardwareMap);
+        }
+        return instance;
+    }
+
+    /**
+     * Optional: clears the singleton.
+     * Useful when switching OpModes.
+     */
+    public static void resetInstance() {
+        instance = null;
+    }
+
+    public static boolean usingLimelight = true;
+    public static boolean usingCamera = false;
+    public DcMotorEx frontLeft;
+    public DcMotorEx frontRight;
+    public DcMotorEx backLeft;
+    public DcMotorEx backRight;
+    public DcMotorEx intake;
+    public DcMotorEx transfer;
+    public PIDFCoefficients shooterPIDF;
+    public static double shooterPIDF_P = 255;
+    public static double shooterPIDF_I = 0.0;
+    public static double shooterPIDF_D = 0.0;
+    public static double shooterPIDF_F = 75;
+    //    public double[] shooterPIDF_StepSizes = {10.0, 1.0, 0.001, 0.0001};
+    public DcMotorEx shooter1;
+    public DcMotorEx shooter2;
+    public Servo hood;
+    public Servo transferServo;
+    public Servo spindexBlocker;
+    public Servo rapidFireBlocker;
+    public Servo tilt1;
+    public Servo tilt2;
+    public Servo turr1;
+    public Servo turr2;
+    public Servo spin1;
+    public Servo spin2;
+    public TouchSensor insideBeam;
+    public TouchSensor outsideBeam;
+
+    public RevColorSensorV3 revSensor;
+
+    public VoltageSensor voltage;
+
+    // Below is disregarded
+    public AnalogInput spin1Pos;
+    public AnalogInput spin2Pos;
+    public AnalogInput turr1Pos;
+    public AnalogInput transferServoPos;
+    public AprilTagProcessor aprilTagProcessor;
+    public WebcamName webcam;
+    public RevColorSensorV3 color1;
+    public RevColorSensorV3 color2;
+    public RevColorSensorV3 color3;
+    public Limelight3A limelight;
+    public Servo light;
+
+    public Robot(HardwareMap hardwareMap) {
+
+        //Define components w/ hardware map
+        frontLeft = hardwareMap.get(DcMotorEx.class, "fl");
+        frontRight = hardwareMap.get(DcMotorEx.class, "fr");
+        backLeft = hardwareMap.get(DcMotorEx.class, "bl");
+        backRight = hardwareMap.get(DcMotorEx.class, "br");
+        frontLeft.setDirection(DcMotorSimple.Direction.REVERSE);
+        backLeft.setDirection(DcMotorSimple.Direction.REVERSE);
+
+        frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+        frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+        backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+        backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+
+        intake = hardwareMap.get(DcMotorEx.class, "intake");
+        intake.setDirection(DcMotorSimple.Direction.REVERSE);
+
+        shooter1 = hardwareMap.get(DcMotorEx.class, "shooter1");
+
+        shooter2 = hardwareMap.get(DcMotorEx.class, "shooter2");
+
+        shooter1.setDirection(DcMotorSimple.Direction.REVERSE);
+        shooterPIDF = new PIDFCoefficients(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, shooterPIDF_F);
+        shooter1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF);
+        shooter1.setVelocity(0);
+        shooter2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF);
+        shooter2.setVelocity(0);
+
+        hood = hardwareMap.get(Servo.class, "hood");
+
+        turr1 = hardwareMap.get(Servo.class, "turr1");
+
+        turr2 = hardwareMap.get(Servo.class, "turr2");
+
+        spin1 = hardwareMap.get(Servo.class, "spin1");
+
+        spin2 = hardwareMap.get(Servo.class, "spin2");
+
+        transfer = hardwareMap.get(DcMotorEx.class, "transfer");
+        transfer.setDirection(DcMotorSimple.Direction.REVERSE);
+
+        transferServo = hardwareMap.get(Servo.class, "transferServo");
+
+        transfer.setDirection(DcMotorSimple.Direction.REVERSE);
+        transfer.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+        spindexBlocker = hardwareMap.get(Servo.class, "spinB");
+
+        rapidFireBlocker = hardwareMap.get(Servo.class, "rapidB");
+
+        tilt1 = hardwareMap.get(Servo.class, "tilt1");
+        tilt2 = hardwareMap.get(Servo.class, "tilt2");
+
+        insideBeam = hardwareMap.get(TouchSensor.class, "beam1");
+        outsideBeam = hardwareMap.get(TouchSensor.class, "beam2");
+
+        revSensor = hardwareMap.get(RevColorSensorV3.class, "rev");
+
+        // Below is disregarded
+
+//        turr1Pos = hardwareMap.get(AnalogInput.class, "t1Pos"); // Encoder of turret plugged in intake port
+//
+//        spin1Pos = hardwareMap.get(AnalogInput.class, "spin1Pos");
+//
+//        spin2Pos = hardwareMap.get(AnalogInput.class, "spin2Pos");
+//
+//        transferServoPos = hardwareMap.get(AnalogInput.class, "tSPos");
+//
+//        color1 = hardwareMap.get(RevColorSensorV3.class, "c1");
+//
+//        color2 = hardwareMap.get(RevColorSensorV3.class, "c2");
+//
+//        color3 = hardwareMap.get(RevColorSensorV3.class, "c3");
+
+        if (usingLimelight) {
+            limelight = hardwareMap.get(Limelight3A.class, "limelight");
+        } else if (usingCamera) {
+            webcam = hardwareMap.get(WebcamName.class, "Webcam 1");
+            aprilTagProcessor = AprilTagProcessor.easyCreateWithDefaults();
+        }
+
+//        light = hardwareMap.get(Servo.class, "light");
+
+        voltage = hardwareMap.voltageSensor.iterator().next();
+    }
+
+    // Voids below are used to minimize hardware calls to minimize loop times
+
+    // Used to cut off digits that are negligible
+    private final int maxDigits = 5;
+    private final int roundingFactor = (int) Math.pow(10, maxDigits);
+
+    private double prevFrontLeftPower = -10.501;
+    public void setFrontLeftPower(double pow){
+        pow = (double) Math.round(pow * roundingFactor) / roundingFactor;
+        if (pow != prevFrontLeftPower){
+            frontLeft.setPower(pow);
+        }
+        prevFrontLeftPower = pow;
+    }
+
+    private double prevFrontRightPower = -10.501;
+    public void setFrontRightPower(double pow){
+        pow = (double) Math.round(pow * roundingFactor) / roundingFactor;
+        if (pow != prevFrontRightPower){
+            frontRight.setPower(pow);
+        }
+        prevFrontRightPower = pow;
+    }
+
+    private double prevBackLeftPower = -10.501;
+    public void setBackLeftPower(double pow){
+        pow = (double) Math.round(pow * roundingFactor) / roundingFactor;
+        if (pow != prevBackLeftPower){
+            backLeft.setPower(pow);
+        }
+        prevBackLeftPower = pow;
+    }
+
+    private double prevBackRightPower = -10.501;
+    public void setBackRightPower(double pow){
+        pow = (double) Math.round(pow * roundingFactor) / roundingFactor;
+        if (pow != prevBackRightPower){
+            backRight.setPower(pow);
+        }
+        prevBackRightPower = pow;
+    }
+
+    private double prevIntakePower = -10.501;
+    public void setIntakePower(double pow){
+        pow = (double) Math.round(pow * roundingFactor) / roundingFactor;
+        if (pow != prevIntakePower){
+            intake.setPower(pow);
+        }
+        prevIntakePower = pow;
+    }
+
+    private double prevTransferPower = -10.501;
+    public void setTransferPower(double pow){
+        pow = (double) Math.round(pow * roundingFactor) / roundingFactor;
+        if (pow != prevTransferPower){
+            transfer.setPower(pow);
+        }
+        prevTransferPower = pow;
+    }
+
+    // shooter motors are done in separate class
+
+    private double prevHoodPos = -10.501;
+    public void setHoodPos(double pos){
+        pos = (double) Math.round(pos * roundingFactor) / roundingFactor;
+        if (pos != prevHoodPos){
+            hood.setPosition(pos);
+        }
+        prevHoodPos = pos;
+    }
+
+    private double prevTransferServoPos = -10.501;
+    public void setTransferServoPos(double pos){
+        pos = (double) Math.round(pos * roundingFactor) / roundingFactor;
+        if (pos != prevTransferServoPos){
+            transferServo.setPosition(pos);
+        }
+        prevTransferServoPos = pos;
+    }
+
+    private double prevSpinPos = -10.501;
+    public void setSpinPos(double pos){
+        pos = (double) Math.round(pos * roundingFactor) / roundingFactor;
+        if (pos != prevSpinPos){
+            spin1.setPosition(pos);
+            spin2.setPosition(pos);
+        }
+        prevSpinPos = pos;
+    }
+
+    private double prevTurretPos = -10.501;
+    public void setTurretPos(double pos){
+        pos = (double) Math.round(pos * roundingFactor) / roundingFactor;
+        if (pos != prevTurretPos){
+            turr1.setPosition(pos);
+            turr2.setPosition(pos);
+        }
+        prevTurretPos = pos;
+    }
+
+    private double prevTilt1Pos = -10.501;
+    public void setTilt1Pos(double pos){
+        pos = (double) Math.round(pos * roundingFactor) / roundingFactor;
+        if (pos != prevTilt1Pos){
+            tilt1.setPosition(pos);
+        }
+        prevTilt1Pos = pos;
+    }
+
+    private double prevTilt2Pos = -10.501;
+    public void setTilt2Pos(double pos){
+        pos = (double) Math.round(pos * roundingFactor) / roundingFactor;
+        if (pos != prevTilt2Pos){
+            tilt2.setPosition(pos);
+        }
+        prevTilt2Pos = pos;
+    }
+
+    private double prevSpindexBlockerPos = -10.501;
+    public void setSpindexBlockerPos(double pos){
+        pos = (double) Math.round(pos * roundingFactor) / roundingFactor;
+        if (pos != prevSpindexBlockerPos){
+            spindexBlocker.setPosition(pos);
+        }
+        prevSpindexBlockerPos = pos;
+    }
+
+    private double prevRapidFireBlockerPos = -10.501;
+    public void setRapidFireBlockerPos(double pos){
+        pos = (double) Math.round(pos * roundingFactor) / roundingFactor;
+        if (pos != prevRapidFireBlockerPos){
+            rapidFireBlocker.setPosition(pos);
+        }
+        prevRapidFireBlockerPos = pos;
+    }
+
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java

@@ -3,7 +3,6 @@ package org.firstinspires.ftc.teamcode.utilsv2;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.pedropathing.follower.Follower;
 
-import org.firstinspires.ftc.teamcode.utils.Robot;
 
 public class Shooter {
 
@@ -33,11 +32,10 @@ public class Shooter {
 
         if (redAlliance) {
             goalX = 144;
-            goalY = 144;
         } else {
             goalX = 0;
-            goalY = 144;
         }
+        goalY = 144;
     }
 
     public void setRedAlliance(boolean input) {
@@ -47,7 +45,7 @@ public class Shooter {
     private double flywheelVelocity = 0.0;
     private double turretPosition = 0.5;
 
-    enum ShooterState {
+    public enum ShooterState {
         READ_OBELISK,
         TRACK_GOAL,
         MANUAL_FLYWHEEL_TRACK_TURR,

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java

@@ -0,0 +1,183 @@
+package org.firstinspires.ftc.teamcode.utilsv2;
+
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+import org.firstinspires.ftc.teamcode.constants.ServoPositions;
+
+public class SpindexerTransferIntake {
+
+    private final Robot robot;
+
+    public SpindexerTransferIntake(Robot rob, MultipleTelemetry TELE) {
+        this.robot = rob;
+    }
+
+    private final double sensorDistanceThreshold = 4.0;
+    private final long pulseTime = 50; // ms
+
+    public enum SpindexerMode {
+        RAPID,
+        SORTED
+    }
+
+    public enum RapidMode {
+        INTAKE,
+        TRANSFER_OFF,
+        BEFORE_PULSE_OUT,
+        PULSE_OUT,
+        PULSE_IN,
+        HOLD_BALLS,
+        OPEN_GATE,
+        SHOOT
+    }
+
+    private SpindexerMode mode = SpindexerMode.RAPID;
+    private RapidMode rapidMode = RapidMode.INTAKE;
+
+    /**
+     * Time when current state was entered.
+     */
+    private long stateStartTime = System.currentTimeMillis();
+
+    public void setRapidMode(RapidMode newMode) {
+        if (rapidMode != newMode) {
+            rapidMode = newMode;
+            stateStartTime = System.currentTimeMillis();
+        }
+    }
+
+    public void setSpindexerMode(SpindexerMode spindexerMode) {
+        this.mode = spindexerMode;
+    }
+
+    public RapidMode getRapidState(){
+        return this.rapidMode;
+    }
+
+    private long stateTime() {
+        return System.currentTimeMillis() - stateStartTime;
+    }
+
+    public void update() {
+
+        switch (mode) {
+
+            case RAPID:
+
+                robot.setSpindexBlockerPos(
+                        ServoPositions.spindexBlocker_Open
+                );
+
+                switch (rapidMode) {
+
+                    case INTAKE:
+
+                        robot.setIntakePower(1);
+                        robot.setTransferPower(1);
+                        robot.setRapidFireBlockerPos(
+                                ServoPositions.rapidFireBlocker_Closed
+                        );
+                        robot.setSpinPos(
+                                ServoPositions.spindexer_A2
+                        );
+                        robot.setTransferServoPos(
+                                ServoPositions.transferServo_out
+                        );
+
+                        if (robot.insideBeam.isPressed() && robot.revSensor.getDistance(DistanceUnit.CM) < sensorDistanceThreshold) {
+
+                            setRapidMode(RapidMode.TRANSFER_OFF);
+                        }
+
+
+                        break;
+
+                    case TRANSFER_OFF:
+
+                        robot.setTransferPower(0.3);
+
+                        if (robot.insideBeam.isPressed() && robot.outsideBeam.isPressed()) {
+                            setRapidMode(RapidMode.BEFORE_PULSE_OUT);
+                        }
+
+                        break;
+
+                    case BEFORE_PULSE_OUT:
+
+                        robot.setIntakePower(1.0);
+
+                        if (stateTime() >= 300) {
+                            setRapidMode(RapidMode.PULSE_OUT);
+                        }
+
+                        break;
+
+                    case PULSE_OUT:
+
+                        robot.setIntakePower(-0.1);
+
+                        if (stateTime() >= pulseTime) {
+                            setRapidMode(RapidMode.PULSE_IN);
+                        }
+
+                        break;
+
+                    case PULSE_IN:
+
+                        robot.setIntakePower(1.0);
+
+                        if (stateTime() >= 200) {
+                            setRapidMode(RapidMode.HOLD_BALLS);
+                        }
+
+
+                        break;
+
+                    case HOLD_BALLS:
+
+                        if (robot.insideBeam.isPressed()
+                                && robot.outsideBeam.isPressed()) {
+
+                            robot.setIntakePower(0.1);
+
+                        } else {
+
+                            robot.setIntakePower(1);
+                        }
+
+
+
+                        break;
+
+                    case OPEN_GATE:
+
+                        robot.setRapidFireBlockerPos(
+                                ServoPositions.rapidFireBlocker_Open
+                        );
+
+                        if (stateTime() >= 100) {
+                            setRapidMode(RapidMode.SHOOT);
+                        }
+
+                        break;
+
+                    case SHOOT:
+
+                        robot.setTransferServoPos(
+                                ServoPositions.transferServo_in
+                        );
+                        if (stateTime() >= 400) {
+                            setRapidMode(RapidMode.INTAKE);
+                        }
+                        break;
+                }
+                break;
+
+            case SORTED:
+
+                // Future sorted-intake logic
+                break;
+        }
+    }
+}

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

@@ -5,7 +5,6 @@ import com.qualcomm.hardware.limelightvision.LLResult;
 import com.qualcomm.hardware.limelightvision.LLResultTypes;
 import com.qualcomm.robotcore.util.Range;
 
-import org.firstinspires.ftc.teamcode.utils.Robot;
 
 import java.util.List;
 
@@ -13,10 +12,10 @@ import java.util.List;
 public class Turret {
     Robot robot;
 
-    private final double servoTicksPer180 = 0.6; // TODO: Tune
-    private final double neutralPosition = 0.5; //TODO: Tune
-    private final double turretMin = 0.04; //TODO: Tune
-    private final double turretMax = 0.94; //TODO: Tune
+    private final double servoTicksPer180 = 0.58;
+    private final double neutralPosition = 0.51;
+    private final double turretMin = 0.05;
+    private final double turretMax = 0.95;
     private final double hVelK = 0; // TODO: Tune
     private final double xVelK = 0; // TODO: Tune
     private final double xAccK = 0; // TODO: Tune
@@ -53,8 +52,8 @@ public class Turret {
 
         servoAngle = Range.clip(servoAngle, turretMin, turretMax);
 
-        robot.turr1.setPosition(servoAngle);
-        robot.turr2.setPosition(1.0 - servoAngle);
+        robot.setTurretPos(servoAngle);
+
 
         detectObelisk();
 
@@ -81,8 +80,8 @@ public class Turret {
     }
 
     public void manual (double pos) {
-        robot.turr1.setPosition(pos);
-        robot.turr2.setPosition(pos);
+        robot.setTurretPos(pos);
+
     }
 
 
@@ -107,7 +106,6 @@ public class Turret {
 
         servoAngle = Range.clip(servoAngle, turretMin, turretMax);
 
-        robot.turr1.setPosition(servoAngle);
-        robot.turr2.setPosition(servoAngle);
+        robot.setTurretPos(servoAngle);
     }
 }