Merge branch 'update-teleop' into danielv5

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

@@ -5,7 +5,7 @@ import com.acmerobotics.dashboard.config.Config;
 @Config
 public class ServoPositions {
 
-    public static double rapidFireBlocker_Closed = 0.3;
+    public static double rapidFireBlocker_Closed = 0.35;
     public static double rapidFireBlocker_Open = 0.5;
 
     public static double spindexBlocker_Closed = 0.31;

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

@@ -1,32 +1,50 @@
 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.constants.Color;
-import org.firstinspires.ftc.teamcode.utils.Robot;
+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 +60,40 @@ 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/NewShooterTest.java

@@ -0,0 +1,126 @@
+package org.firstinspires.ftc.teamcode.tests;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+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 org.firstinspires.ftc.teamcode.constants.ServoPositions;
+import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
+import org.firstinspires.ftc.teamcode.utilsv2.Robot;
+import org.firstinspires.ftc.teamcode.utilsv2.Shooter;
+
+@Config
+@TeleOp
+public class NewShooterTest extends LinearOpMode {
+
+    Robot robot;
+
+    public static boolean intake = true;
+    public static boolean shoot = false;
+    public static double intakePower = 1.0;
+    public static double transferShootPower = -1;
+    public static double transferIntakePower = -1.0;
+    public static double turretPos = 0.51;
+    public static double hoodPos = 0.51;
+    public static double flywheel = 0;
+
+    private enum ShootState {
+        IDLE,
+        WAIT_GATE,
+        WAIT_PUSH
+    }
+
+    private ShootState shootState = ShootState.IDLE;
+    private long timestamp = 0;
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+
+        Robot.resetInstance();
+
+        robot = Robot.getInstance(hardwareMap);
+
+        Shooter shooter = new Shooter(
+                robot,
+                new MultipleTelemetry(
+                        telemetry,
+                        FtcDashboard.getInstance().getTelemetry()
+                ),
+                Constants.createFollower(hardwareMap),
+                true
+        );
+
+        shooter.setState(Shooter.ShooterState.MANUAL);
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        while (opModeIsActive()) {
+
+            robot.setHoodPos(hoodPos);
+            shooter.setTurretPosition(turretPos);
+            shooter.setFlywheelVelocity(flywheel);
+            robot.setSpinPos(ServoPositions.spindexer_A2);
+
+            if (intake && !shoot) {
+
+                shootState = ShootState.IDLE;
+
+                robot.setRapidFireBlockerPos(
+                        ServoPositions.rapidFireBlocker_Closed);
+
+                robot.setTransferPower(transferIntakePower);
+                robot.setIntakePower(intakePower);
+                robot.setTransferServoPos(
+                        ServoPositions.transferServo_out);
+            }
+
+            else if (shoot) {
+                robot.setIntakePower(intakePower);
+
+
+                switch (shootState) {
+
+                    case IDLE:
+
+                        robot.setTransferPower(transferShootPower);
+
+                        timestamp = System.currentTimeMillis();
+                        shootState = ShootState.WAIT_GATE;
+
+                        break;
+
+                    case WAIT_GATE:
+
+                        if (System.currentTimeMillis() - timestamp >= 300) {
+
+                            robot.setRapidFireBlockerPos(
+                                    ServoPositions.rapidFireBlocker_Open);
+
+                            timestamp = System.currentTimeMillis();
+                            shootState = ShootState.WAIT_PUSH;
+                        }
+
+                        break;
+
+                    case WAIT_PUSH:
+
+                        if (System.currentTimeMillis() - timestamp >= 100) {
+
+                            robot.setTransferServoPos(
+                                    ServoPositions.transferServo_in);
+
+                            shootState = ShootState.IDLE;
+                        }
+
+                        break;
+                }
+            }
+
+            shooter.update();
+        }
+    }
+}

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 FORWARD_ROTATION_CORRECTION = 0;
-    public static double STRAFE_ROTATION_CORRECTION = -0.03;
+    public static double STRAFE_ROTATION_CORRECTION = -0;
 
     private boolean tele = false;
 

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

@@ -0,0 +1,131 @@
+package org.firstinspires.ftc.teamcode.utilsv2;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import com.qualcomm.robotcore.hardware.PIDFCoefficients;
+
+import org.firstinspires.ftc.teamcode.utilsv2.Robot;
+
+import java.util.LinkedList;
+
+public class Flywheel {
+    Robot robot;
+
+    public PIDFCoefficients shooterPIDF1, shooterPIDF2;
+
+    private double velo = 0.0;
+    private double velo1 = 0.0;
+    private double velo2 = 0.0;
+
+    private double averageVelocity = 0.0;
+
+    double targetVelocity = 0.0;
+
+    boolean steady = false;
+
+    private final LinkedList<Double> velocityHistory = new LinkedList<>();
+
+    public Flywheel(Robot rob) {
+
+        robot = rob;
+        shooterPIDF1 = new PIDFCoefficients(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F);
+        shooterPIDF2 = new PIDFCoefficients(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F);
+    }
+
+    public double getVelo() {
+        return velo;
+    }
+
+    public double getVelo1() {
+        return velo1;
+    }
+
+    public double getVelo2() {
+        return velo2;
+    }
+
+    public double getAverageVelocity() {
+        return averageVelocity;
+    }
+
+    public boolean getSteady() {
+        return steady;
+    }
+
+    // Set the robot PIDF for the next cycle.
+    private double prevF = 0;
+
+    public static double voltagePIDFDifference = 0.8;
+
+    public void setPIDF(double p, double i, double d, double f) {
+
+        shooterPIDF1.p = p;
+        shooterPIDF1.i = i;
+        shooterPIDF1.d = d;
+        shooterPIDF1.f = f;
+
+        shooterPIDF2.p = p;
+        shooterPIDF2.i = i;
+        shooterPIDF2.d = d;
+        shooterPIDF2.f = f;
+
+        if (Math.abs(prevF - f) > voltagePIDFDifference) {
+
+            robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1);
+
+            robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF2);
+
+            prevF = f;
+        }
+    }
+
+    // Convert from RPM to Ticks per Second
+    private double RPM_to_TPS(double RPM) {
+        return (RPM * 28.0) / 60.0;
+    }
+
+    // Convert from Ticks per Second to RPM
+    private double TPS_to_RPM(double TPS) {
+        return (TPS * 60.0) / 28.0;
+    }
+
+    private void updateVelocityAverage(double newVelocity) {
+
+        velocityHistory.add(newVelocity);
+
+        int velocityHistorySize = 5;
+        if (velocityHistory.size() > velocityHistorySize) {
+            velocityHistory.removeFirst();
+        }
+
+        double sum = 0.0;
+
+        for (double v : velocityHistory) {
+            sum += v;
+        }
+
+        averageVelocity = sum / velocityHistory.size();
+    }
+
+    public void manageFlywheel(double commandedVelocity) {
+
+        if (Math.abs(targetVelocity - commandedVelocity) > 0.0001) {
+            targetVelocity = commandedVelocity;
+        }
+
+        robot.shooter1.setVelocity(RPM_to_TPS(targetVelocity));
+
+        robot.shooter2.setVelocity(RPM_to_TPS(targetVelocity));
+
+        velo1 = TPS_to_RPM(robot.shooter1.getVelocity());
+
+        velo2 = TPS_to_RPM(robot.shooter2.getVelocity());
+
+        velo = (velo1 + velo2) / 2.0;
+
+        updateVelocityAverage(velo);
+
+        steady = (Math.abs(commandedVelocity - averageVelocity) < 50);
+    }
+}

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");
+
+
+        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

@@ -0,0 +1,160 @@
+package org.firstinspires.ftc.teamcode.utilsv2;
+
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.pedropathing.follower.Follower;
+
+
+public class Shooter {
+
+    Robot robot;
+    Flywheel fly;
+    Turret turr;
+    VelocityCommander commander;
+
+    double goalX = 0.0;
+    double goalY = 0.0;
+    double obeliskX = 72;
+    double obeliskY = 144;
+
+    private boolean red = true;
+
+
+    Follower follow;
+
+    public Shooter(Robot rob, MultipleTelemetry TELE, Follower follower, boolean redAlliance) {
+        this.robot = rob;
+        this.fly = new Flywheel(rob);
+        this.turr = new Turret(rob);
+        this.follow = follower;
+        this.commander = new VelocityCommander();
+
+        setRedAlliance(redAlliance);
+
+        if (redAlliance) {
+            goalX = 144;
+        } else {
+            goalX = 0;
+        }
+        goalY = 144;
+    }
+
+    public void setRedAlliance(boolean input) {
+        this.red = input;
+    }
+
+    private double flywheelVelocity = 0.0;
+    private double turretPosition = 0.5;
+
+    public enum ShooterState {
+        READ_OBELISK,
+        TRACK_GOAL,
+        MANUAL_FLYWHEEL_TRACK_TURR,
+        MANUAL_TURRET_TRACK_FLY,
+        MANUAL,
+        NOTHING
+    }
+
+
+    private ShooterState state = ShooterState.NOTHING;
+
+    public void setState(ShooterState shooterState) {
+        this.state = shooterState;
+    }
+
+    public void setTurretPosition(double input) {
+        this.turretPosition = input;
+    }
+
+    public void setFlywheelVelocity(double input) {
+        this.flywheelVelocity = input;
+    }
+
+    public int getObeliskID() {
+        return turr.getObeliskID();
+    }
+
+
+    public void update() {
+
+        switch (state) {
+            case NOTHING:
+                break;
+            case MANUAL:
+                fly.manageFlywheel(flywheelVelocity);
+                turr.manual(turretPosition);
+                break;
+            case TRACK_GOAL:
+                turr.trackGoal(
+                        (follow.getPose().getX() - goalX),
+                        (follow.getPose().getY() - goalY),
+                        follow.getHeading(),
+                        follow.getAngularVelocity(),
+                        follow.getVelocity().getXComponent(),
+                        follow.getAcceleration().getXComponent(),
+                        follow.getVelocity().getYComponent(),
+                        follow.getAcceleration().getYComponent()
+                );
+
+                flywheelVelocity = commander.getVeloPredictive(
+                        (follow.getPose().getX() - goalX),
+                        (follow.getPose().getY() - goalY),
+                        follow.getVelocity().getXComponent(),
+                        follow.getAcceleration().getXComponent(),
+                        follow.getVelocity().getYComponent(),
+                        follow.getAcceleration().getYComponent()
+                );
+
+                fly.manageFlywheel(flywheelVelocity);
+                break;
+            case READ_OBELISK:
+                turr.trackObelisk(
+                        (follow.getPose().getX() - goalX),
+                        (follow.getPose().getY() - goalY),
+                        follow.getHeading()
+                );
+
+                flywheelVelocity = commander.getVeloPredictive(
+                        (follow.getPose().getX() - goalX),
+                        (follow.getPose().getY() - goalY),
+                        follow.getVelocity().getXComponent(),
+                        follow.getAcceleration().getXComponent(),
+                        follow.getVelocity().getYComponent(),
+                        follow.getAcceleration().getYComponent()
+                );
+
+                fly.manageFlywheel(flywheelVelocity);
+                break;
+
+            case MANUAL_TURRET_TRACK_FLY:
+                turr.manual(turretPosition);
+                flywheelVelocity = commander.getVeloPredictive(
+                        (follow.getPose().getX() - goalX),
+                        (follow.getPose().getY() - goalY),
+                        follow.getVelocity().getXComponent(),
+                        follow.getAcceleration().getXComponent(),
+                        follow.getVelocity().getYComponent(),
+                        follow.getAcceleration().getYComponent()
+                );
+
+                fly.manageFlywheel(flywheelVelocity);
+                break;
+
+            case MANUAL_FLYWHEEL_TRACK_TURR:
+                turr.trackGoal(
+                        (follow.getPose().getX() - goalX),
+                        (follow.getPose().getY() - goalY),
+                        follow.getHeading(),
+                        follow.getAngularVelocity(),
+                        follow.getVelocity().getXComponent(),
+                        follow.getAcceleration().getXComponent(),
+                        follow.getVelocity().getYComponent(),
+                        follow.getAcceleration().getYComponent()
+                );
+                fly.manageFlywheel(flywheelVelocity);
+                break;
+
+        }
+
+    }
+
+}

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

@@ -0,0 +1,111 @@
+package org.firstinspires.ftc.teamcode.utilsv2;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.qualcomm.hardware.limelightvision.LLResult;
+import com.qualcomm.hardware.limelightvision.LLResultTypes;
+import com.qualcomm.robotcore.util.Range;
+
+
+import java.util.List;
+
+@Config
+public class Turret {
+    Robot robot;
+
+    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
+    private final double yVelK = 0; // TODO: Tune
+    private final double yAccK = 0; // TODO: Tune
+
+    private int obeliskID = 0;
+
+
+
+    public Turret(Robot rob) {
+        this.robot = rob;
+    }
+
+    private double wrapAngle(double angle) {
+        while (angle > Math.PI) angle -= 2.0 * Math.PI;
+        while (angle < -Math.PI) angle += 2.0 * Math.PI;
+        return angle;
+    }
+
+    public void trackObelisk(double dx, double dy, double h) {
+
+        double heading = wrapAngle(h);
+
+        double fieldRelativeHeading = Math.atan2(dy, dx);
+
+        double desiredAngle = fieldRelativeHeading - heading;
+        double angleDelta = desiredAngle - Math.PI;
+        angleDelta = wrapAngle(angleDelta);
+
+        double servoTicksFromNeutral = (angleDelta / (2.0 * Math.PI)) * (2.0 * servoTicksPer180);
+
+        double servoAngle = neutralPosition + servoTicksFromNeutral;
+
+        servoAngle = Range.clip(servoAngle, turretMin, turretMax);
+
+        robot.setTurretPos(servoAngle);
+
+
+        detectObelisk();
+
+    }
+
+    public int getObeliskID() {
+        return obeliskID;
+    }
+
+    private int detectObelisk() {
+        robot.limelight.pipelineSwitch(1);
+        LLResult result = robot.limelight.getLatestResult();
+        if (result != null && result.isValid()) {
+            List<LLResultTypes.FiducialResult> fiducials = result.getFiducialResults();
+            double prevTx  = -1000;
+            for (LLResultTypes.FiducialResult fiducial : fiducials) {
+                double currentTx = fiducial.getTargetXDegrees();
+                if (currentTx > prevTx){
+                    obeliskID = fiducial.getFiducialId();
+                }
+            }
+        }
+        return obeliskID;
+    }
+
+    public void manual (double pos) {
+        robot.setTurretPos(pos);
+
+    }
+
+
+    public void trackGoal(double dx, double dy, double h, double hVel, double xVel, double xAcc, double yVel, double yAcc) {
+        // dx, dy, dz is target - robot
+        // h is the raw heading where 0 degrees is positive x in the system of x, y
+
+        double predictedDx = dx - (xVel * xVelK) - (0.5 * xAcc * xAccK); // Negative bc dx = target - robot
+        double predictedDy = dy - (yVel * yVelK) - (0.5 * yAcc * yAccK);  // Negative bc dy = target - robot
+        double predictedH = h + (hVel * hVelK); // Positive bc h = robot heading
+
+        predictedH = wrapAngle(predictedH);
+
+        double fieldRelativeHeading = Math.atan2(predictedDy, predictedDx);
+
+        double angleDelta = fieldRelativeHeading - predictedH;
+        angleDelta = wrapAngle(angleDelta);
+
+        double servoTicksFromNeutral = (angleDelta / (2.0 * Math.PI)) * (2.0 * servoTicksPer180);
+
+        double servoAngle = neutralPosition + servoTicksFromNeutral;
+
+        servoAngle = Range.clip(servoAngle, turretMin, turretMax);
+
+        robot.setTurretPos(servoAngle);
+    }
+}

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

@@ -0,0 +1,34 @@
+package org.firstinspires.ftc.teamcode.utilsv2;
+
+public class VelocityCommander {
+    private final double goalH = 20.0; //TODO: Tune
+    private final double xVelK = 0; // TODO: Tune
+    private final double xAccK = 0; // TODO: Tune
+    private final double yVelK = 0; // TODO: Tune
+    private final double yAccK = 0; // TODO: Tune
+
+    public VelocityCommander() {
+
+    }
+
+    private double distToRPM (double dist){
+        return Math.sqrt(dist*dist + goalH*goalH);
+        //TODO: Add regression here using goalH
+    }
+
+    public double getVeloStationary (double distance){
+        return distToRPM(distance);
+    }
+
+    public double getVeloPredictive(double dx, double dy, double xVel, double xAcc, double yVel, double yAcc) {
+
+        double predictedDx = dx - (xVel * xVelK) - (0.5 * xAcc * xAccK); // Negative bc dx = target - robot
+        double predictedDy = dy - (yVel * yVelK) - (0.5 * yAcc * yAccK);  // Negative bc dy = target - robot
+
+        double predictedDist = Math.sqrt(dx*dx + dy*dy);
+
+        return distToRPM(predictedDist);
+    }
+
+
+}