Merge branch 'danielv2'

# Conflicts:
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue_V2.java
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V2.java
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java

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

@@ -3,8 +3,9 @@ package org.firstinspires.ftc.teamcode.autonomous;
 import static org.firstinspires.ftc.teamcode.constants.Poses.*;
 
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodAuto;
 import static org.firstinspires.ftc.teamcode.constants.ShooterVars.*;
-import static org.firstinspires.ftc.teamcode.utils.PositionalServoProgrammer.*;
+import static org.firstinspires.ftc.teamcode.tests.PIDServoTest.*;
 
 import androidx.annotation.NonNull;
 
@@ -177,8 +178,8 @@ public class Blue_V2 extends LinearOpMode {
                 TELE.update();
 
                 if (gpp || pgp || ppg){
-                    robot.turr1.setPosition(turret_blue);
+                    robot.turr1.setPower(turret_blue);
-                    robot.turr2.setPosition(1 - turret_blue);
+                    robot.turr2.setPower(1 - turret_blue);
                     return false;
                 } else {
                     return true;
@@ -202,8 +203,8 @@ public class Blue_V2 extends LinearOpMode {
                     position = spindexer_intakePos1 - 0.02;
                 }
 
-                robot.spin1.setPosition(position);
+                robot.spin1.setPower(position);
-                robot.spin2.setPosition(1 - position);
+                robot.spin2.setPower(1 - position);
 
                 if (ticker == 0) {
                     stamp = getRuntime();
@@ -263,8 +264,8 @@ public class Blue_V2 extends LinearOpMode {
                 velo = flywheel.getVelo();
                 robot.shooter1.setPower(powPID);
                 robot.shooter2.setPower(powPID);
-                robot.spin1.setPosition(spindexer);
+                robot.spin1.setPower(spindexer);
-                robot.spin2.setPosition(1-spindexer);
+                robot.spin2.setPower(1-spindexer);
                 TELE.addData("Velocity", velo);
                 TELE.addLine("spindex");
                 TELE.update();
@@ -379,8 +380,8 @@ public class Blue_V2 extends LinearOpMode {
                 } else {
                     position = spindexer_intakePos1 - 0.02;
                 }
-                robot.spin1.setPosition(position);
+                robot.spin1.setPower(position);
-                robot.spin2.setPosition(1 - position);
+                robot.spin2.setPower(1 - position);
 
                 TELE.addData("Velocity", velo);
                 TELE.addLine("Intaking");
@@ -417,8 +418,8 @@ public class Blue_V2 extends LinearOpMode {
                 } else {
                     position = spindexer_intakePos1 - 0.02;
                 }
-                robot.spin1.setPosition(position);
+                robot.spin1.setPower(position);
-                robot.spin2.setPosition(1 - position);
+                robot.spin2.setPower(1 - position);
 
                 double s1D = robot.color1.getDistance(DistanceUnit.MM);
                 double s2D = robot.color2.getDistance(DistanceUnit.MM);
@@ -540,13 +541,13 @@ public class Blue_V2 extends LinearOpMode {
 
             robot.hood.setPosition(hoodAuto);
 
-            robot.turr1.setPosition(turret_detectBlue);
+            robot.turr1.setPower(turret_detectBlue);
-            robot.turr2.setPosition(1 - turret_detectBlue);
+            robot.turr2.setPower(1 - turret_detectBlue);
 
             robot.transferServo.setPosition(transferServo_out);
 
-            robot.spin1.setPosition(spindexer_intakePos1);
+            robot.spin1.setPower(spindexer_intakePos1);
-            robot.spin2.setPosition(1 - spindexer_intakePos1);
+            robot.spin2.setPower(1 - spindexer_intakePos1);
 
             aprilTag.update();
             TELE.addData("Velocity", velo);

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

@@ -3,7 +3,6 @@ package org.firstinspires.ftc.teamcode.autonomous;
 import static org.firstinspires.ftc.teamcode.constants.Poses.*;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
 import static org.firstinspires.ftc.teamcode.constants.ShooterVars.*;
-import static org.firstinspires.ftc.teamcode.utils.PositionalServoProgrammer.*;
 
 import androidx.annotation.NonNull;
 
@@ -26,6 +25,8 @@ import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
 import org.firstinspires.ftc.teamcode.utils.Flywheel;
 import org.firstinspires.ftc.teamcode.utils.AprilTagWebcam;
 import org.firstinspires.ftc.teamcode.utils.Robot;
+import org.firstinspires.ftc.teamcode.utils.Servos;
+import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
 
 @Config
 @Autonomous(preselectTeleOp = "TeleopV2")
@@ -41,8 +42,9 @@ public class Red_V2 extends LinearOpMode {
 
     Flywheel flywheel;
 
+    Servos servo;
+
     double velo = 0.0;
-    double targetVelocity = 0.0;
     public static double intake1Time = 2.9;
 
     public static double intake2Time = 2.9;
@@ -57,42 +59,33 @@ public class Red_V2 extends LinearOpMode {
 
     double powPID = 0.0;
 
+    double bearing = 0.0;
+
     int b1 = 0; // 0 = no ball, 1 = green, 2 = purple
 
     int b2 = 0;// 0 = no ball, 1 = green, 2 = purple
 
     int b3 = 0;// 0 = no ball, 1 = green, 2 = purple
 
-    boolean spindexPosEqual(double spindexer) {
-        TELE.addData("Velocity", velo);
-        TELE.addLine("spindex equal");
-        TELE.update();
-        return (scalar * ((robot.spin1Pos.getVoltage() - restPos) / 3.3) > spindexer - 0.01 &&
-                scalar * ((robot.spin1Pos.getVoltage() - restPos) / 3.3) < spindexer + 0.01);
-    }
-
     public Action initShooter(int vel) {
         return new Action() {
-            double initPos = 0.0;
             double stamp = 0.0;
             double stamp1 = 0.0;
             double ticker = 0.0;
             double stamp2 = 0.0;
-            double currentPos = 0.0;
             boolean steady = false;
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
                 if (ticker == 0) {
                     stamp2 = getRuntime();
                 }
 
-                targetVelocity = (double) vel;
                 ticker++;
                 if (ticker % 16 == 0) {
                     stamp = getRuntime();
                     stamp1 = stamp;
                 }
 
-                powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
+                powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition());
                 velo = flywheel.getVelo();
                 robot.shooter1.setPower(powPID);
                 robot.shooter2.setPower(powPID);
@@ -121,7 +114,7 @@ public class Red_V2 extends LinearOpMode {
             double stamp = 0.0;
             boolean steady = false;
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
-                powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
+                powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition());
                 velo = flywheel.getVelo();
                 steady = flywheel.getSteady();
                 robot.shooter1.setPower(powPID);
@@ -130,7 +123,7 @@ public class Red_V2 extends LinearOpMode {
 
                 TELE.addData("Velocity", velo);
                 TELE.update();
-
+                detectTag();
 
                 if (last && !steady){
                     stamp = getRuntime();
@@ -176,9 +169,10 @@ public class Red_V2 extends LinearOpMode {
                 TELE.update();
 
                 if (gpp || pgp || ppg){
-                    robot.turr1.setPosition(turret_red);
+                    double turretPID = servo.setTurrPos(turret_red);
-                    robot.turr2.setPosition(1 - turret_red);
+                    robot.turr1.setPower(turretPID);
-                    return false;
+                    robot.turr2.setPower(-turretPID);
+                    return !servo.turretEqual(turret_red);
                 } else {
                     return true;
                 }
@@ -186,51 +180,17 @@ public class Red_V2 extends LinearOpMode {
         };
     }
 
-    public Action spindex (double spindexer, double vel){
+    public Action spindex (double spindexer, int vel){
         return new Action() {
-            double currentPos = 0.0;
-            double stamp = 0.0;
-            double initPos = 0.0;
-            double stamp1 = 0.0;
-            int ticker = 0;
             @Override
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
-                ticker++;
-                if (ticker % 8 == 0) {
-                    currentPos = (double) robot.shooter1.getCurrentPosition() / 2048;
-                    stamp = getRuntime();
-                    velo = -60 * ((currentPos - initPos) / (stamp - stamp1));
-                    initPos = currentPos;
-                    stamp1 = stamp;
-                }
 
-                if (vel - velo > 500 && ticker > 16) {
+                powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition());
-                    powPID = 1.0;
-                } else if (velo - vel > 500 && ticker > 16){
-                    powPID = 0.0;
-                } else if (Math.abs(vel - velo) < 100 && ticker > 16){
-                    double feed = Math.log((668.39 / (vel + 591.96)) - 0.116) / -4.18;
-
-                    // --- PROPORTIONAL CORRECTION ---
-                    double error = vel - velo;
-                    double correction = kP * error;
-
-                    // limit how fast power changes (prevents oscillation)
-                    correction = Math.max(-maxStep, Math.min(maxStep, correction));
-
-                    // --- FINAL MOTOR POWER ---
-                    powPID = feed + correction;
-
-                    // clamp to allowed range
-                    powPID = Math.max(0, Math.min(1, powPID));
-                }
-
-                powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
                 velo = flywheel.getVelo();
                 robot.shooter1.setPower(powPID);
                 robot.shooter2.setPower(powPID);
-                robot.spin1.setPosition(spindexer);
+                robot.spin1.setPower(spindexer);
-                robot.spin2.setPosition(1-spindexer);
+                robot.spin2.setPower(1-spindexer);
                 TELE.addData("Velocity", velo);
                 TELE.addLine("spindex");
                 TELE.update();
@@ -239,65 +199,32 @@ public class Red_V2 extends LinearOpMode {
 
                 teleStart = drive.localizer.getPose();
 
-                return !spindexPosEqual(spindexer);
+                return !servo.spinEqual(spindexer);
             }
         };
     }
 
-    public Action Shoot(double vel) {
+    public Action Shoot(int vel) {
         return new Action() {
             double transferStamp = 0.0;
             int ticker = 1;
             boolean transferIn = false;
-            double currentPos = 0.0;
-            double stamp = 0.0;
-            double initPos = 0.0;
-            double stamp1 = 0.0;
             @Override
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
                 TELE.addData("Velocity", velo);
                 TELE.addLine("shooting");
                 TELE.update();
 
-                if (ticker % 8 == 0) {
+                powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition());
-                    currentPos = (double) robot.shooter1.getCurrentPosition() / 2048;
-                    stamp = getRuntime();
-                    velo = -60 * ((currentPos - initPos) / (stamp - stamp1));
-                    initPos = currentPos;
-                    stamp1 = stamp;
-                }
-
-                if (vel - velo > 500 && ticker > 16) {
-                    powPID = 1.0;
-                } else if (velo - vel > 500 && ticker > 16){
-                    powPID = 0.0;
-                } else if (Math.abs(vel - velo) < 100 && ticker > 16){
-                    double feed = Math.log((668.39 / (vel + 591.96)) - 0.116) / -4.18;
-
-                    // --- PROPORTIONAL CORRECTION ---
-                    double error = vel - velo;
-                    double correction = kP * error;
-
-                    // limit how fast power changes (prevents oscillation)
-                    correction = Math.max(-maxStep, Math.min(maxStep, correction));
-
-                    // --- FINAL MOTOR POWER ---
-                    powPID = feed + correction;
-
-                    // clamp to allowed range
-                    powPID = Math.max(0, Math.min(1, powPID));
-                }
-
-                powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
                 velo = flywheel.getVelo();
                 robot.shooter1.setPower(powPID);
                 robot.shooter2.setPower(powPID);
 
                 drive.updatePoseEstimate();
+                detectTag();
 
                 teleStart = drive.localizer.getPose();
 
-
                 if (ticker == 1) {
                     transferStamp = getRuntime();
                     ticker++;
@@ -345,8 +272,8 @@ public class Red_V2 extends LinearOpMode {
                 } else {
                     position = spindexer_intakePos1 - 0.02;
                 }
-                robot.spin1.setPosition(position);
+                robot.spin1.setPower(position);
-                robot.spin2.setPosition(1 - position);
+                robot.spin2.setPower(1 - position);
 
                 TELE.addData("Velocity", velo);
                 TELE.addLine("Intaking");
@@ -404,8 +331,8 @@ public class Red_V2 extends LinearOpMode {
                 } else {
                     position = spindexer_intakePos1 - 0.02;
                 }
-                robot.spin1.setPosition(position);
+                robot.spin1.setPower(position);
-                robot.spin2.setPosition(1 - position);
+                robot.spin2.setPower(1 - position);
 
                 double s1D = robot.color1.getDistance(DistanceUnit.MM);
                 double s2D = robot.color2.getDistance(DistanceUnit.MM);
@@ -527,16 +454,14 @@ public class Red_V2 extends LinearOpMode {
 
             robot.hood.setPosition(hoodAuto);
 
-            robot.turr1.setPosition(turret_detectRed);
-            robot.turr2.setPosition(1 - turret_detectRed);
-
             robot.transferServo.setPosition(transferServo_out);
 
-            robot.spin1.setPosition(spindexer_intakePos1);
+            robot.spin1.setPower(spindexer_intakePos1);
-            robot.spin2.setPosition(1 - spindexer_intakePos1);
+            robot.spin2.setPower(1 - spindexer_intakePos1);
 
             aprilTag.update();
             TELE.addData("Velocity", velo);
+            TELE.addData("Turret Pos", servo.getTurrPos());
             TELE.update();
         }
 
@@ -560,7 +485,7 @@ public class Red_V2 extends LinearOpMode {
 
             teleStart = drive.localizer.getPose();
 
-            powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
+            powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, robot.shooter1.getCurrentPosition());
             velo = flywheel.getVelo();
             robot.shooter1.setPower(powPID);
             robot.shooter2.setPower(powPID);
@@ -596,7 +521,7 @@ public class Red_V2 extends LinearOpMode {
 
             teleStart = drive.localizer.getPose();
 
-            powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
+            powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, robot.shooter1.getCurrentPosition());
             velo = flywheel.getVelo();
             robot.shooter1.setPower(powPID);
             robot.shooter2.setPower(powPID);
@@ -626,7 +551,7 @@ public class Red_V2 extends LinearOpMode {
                     )
             );
 
-            powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
+            powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, robot.shooter1.getCurrentPosition());
             velo = flywheel.getVelo();
             robot.shooter1.setPower(powPID);
             robot.shooter2.setPower(powPID);
@@ -647,6 +572,25 @@ public class Red_V2 extends LinearOpMode {
 
     }
 
+    public void detectTag(){
+        AprilTagDetection d20 = aprilTag.getTagById(20);
+        AprilTagDetection d24 = aprilTag.getTagById(24);
+
+        if (d20 != null) {
+            bearing = d20.ftcPose.bearing;
+            TELE.addData("Bear", bearing);
+        }
+
+        if (d24 != null) {
+            bearing = d24.ftcPose.bearing;
+            TELE.addData("Bear", bearing);
+        }
+        double turretPos = servo.getTurrPos() - (bearing / 1300);
+        double turretPID = servo.setTurrPos(turretPos);
+        robot.turr1.setPower(turretPID);
+        robot.turr2.setPower(-turretPID);
+    }
+
     public void shootingSequence() {
         TELE.addData("Velocity", velo);
         if (gpp) {

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

@@ -5,16 +5,16 @@ import com.acmerobotics.dashboard.config.Config;
 @Config
 public class ServoPositions {
 
-    public static double spindexer_intakePos1 = 0.665;
+    public static double spindexer_intakePos1 = 0.34;
 
-    public static double spindexer_intakePos3 = 0.29;
+    public static double spindexer_intakePos2 = 0.5;
 
-    public static double spindexer_intakePos2 = 0.99;
+    public static double spindexer_intakePos3 = 0.66;
 
-    public static double spindexer_outtakeBall3 = 0.845;
+    public static double spindexer_outtakeBall3 = 0.42;
 
-    public static double spindexer_outtakeBall2 = 0.48;
+    public static double spindexer_outtakeBall2 = 0.74;
-    public static double spindexer_outtakeBall1 = 0.1;
+    public static double spindexer_outtakeBall1 = 0.58;
 
     public static double transferServo_out = 0.15;
 

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

@@ -1,19 +1,9 @@
 package org.firstinspires.ftc.teamcode.teleop;
 
-import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
+import static org.firstinspires.ftc.teamcode.constants.Poses.*;
-import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1;
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
-import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1;
+import static org.firstinspires.ftc.teamcode.constants.ShooterVars.*;
-import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2;
+import static org.firstinspires.ftc.teamcode.tests.PIDServoTest.*;
-import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3;
-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.constants.ServoPositions.turrDefault;
-import static org.firstinspires.ftc.teamcode.constants.ShooterVars.waitTransfer;
-import static org.firstinspires.ftc.teamcode.constants.ShooterVars.waitTransferOut;
-import static org.firstinspires.ftc.teamcode.tests.ShooterTest.kP;
-import static org.firstinspires.ftc.teamcode.tests.ShooterTest.maxStep;
-import static org.firstinspires.ftc.teamcode.utils.PositionalServoProgrammer.restPos;
-import static org.firstinspires.ftc.teamcode.utils.PositionalServoProgrammer.scalar;
 
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
@@ -26,7 +16,9 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
 import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
 import org.firstinspires.ftc.teamcode.utils.AprilTagWebcam;
+import org.firstinspires.ftc.teamcode.utils.Flywheel;
 import org.firstinspires.ftc.teamcode.utils.Robot;
+import org.firstinspires.ftc.teamcode.utils.Servos;
 import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
 
 import java.util.ArrayList;
@@ -35,7 +27,8 @@ import java.util.List;
 @TeleOp
 @Config
 public class TeleopV2 extends LinearOpMode {
-
+    Servos servo;
+    Flywheel flywheel;
     public static double manualVel = 3000;
     public static double hood = 0.5;
     public static double hoodDefaultPos = 0.5;
@@ -63,6 +56,7 @@ public class TeleopV2 extends LinearOpMode {
     double yOffset = 0.0;
     double headingOffset = 0.0;
     int ticker = 0;
+    int camTicker = 0;
     List<Double> s1G = new ArrayList<>();
     List<Double> s2G = new ArrayList<>();
     List<Double> s3G = new ArrayList<>();
@@ -98,6 +92,11 @@ public class TeleopV2 extends LinearOpMode {
     private double transferStamp = 0.0;
     private int tickerA = 1;
     private boolean transferIn = false;
+    double turretPID = 0.0;
+    double turretPos = 0.5;
+    double spindexPID = 0.0;
+    double spindexPos = spindexer_intakePos1;
+    double error = 0.0;
 
     public static double velPrediction(double distance) {
 
@@ -129,6 +128,8 @@ public class TeleopV2 extends LinearOpMode {
         TELE = new MultipleTelemetry(
                 telemetry, FtcDashboard.getInstance().getTelemetry()
         );
+        servo = new Servos(hardwareMap);
+        flywheel = new Flywheel();
 
         drive = new MecanumDrive(hardwareMap, teleStart);
 
@@ -136,9 +137,6 @@ public class TeleopV2 extends LinearOpMode {
 
         aprilTagWebcam.init(new Robot(hardwareMap), TELE);
 
-        robot.turr1.setPosition(0.4);
-        robot.turr2.setPosition(1 - 0.4);
-
         waitForStart();
         if (isStopRequested()) return;
         while (opModeIsActive()) {
@@ -160,6 +158,21 @@ public class TeleopV2 extends LinearOpMode {
             robot.frontRight.setPower(frontRightPower);
             robot.backRight.setPower(backRightPower);
 
+            // PID SERVOS
+            turretPID = servo.setTurrPos(turretPos);
+            robot.turr1.setPower(turretPID);
+            robot.turr2.setPower(-turretPID);
+
+            //TODO: make sure changing position works throughout opmode
+            if (!servo.spinEqual(spindexPos)){
+                spindexPID = servo.setSpinPos(spindexPos);
+                robot.spin1.setPower(spindexPID);
+                robot.spin2.setPower(-spindexPID);
+            } else{
+                robot.spin1.setPower(0);
+                robot.spin2.setPower(0);
+            }
+
             //INTAKE:
 
             if (gamepad1.rightBumperWasPressed()) {
@@ -183,22 +196,15 @@ public class TeleopV2 extends LinearOpMode {
 
                 robot.intake.setPower(1);
 
-                double position;
-
                 if ((getRuntime() % 0.3) > 0.15) {
-                    position = spindexer_intakePos1 + 0.015;
+                    spindexPos = spindexer_intakePos1 + 0.015;
                 } else {
-                    position = spindexer_intakePos1 - 0.015;
+                    spindexPos = spindexer_intakePos1 - 0.015;
                 }
 
-                robot.spin1.setPosition(position);
-                robot.spin2.setPosition(1 - position);
-
             } else if (reject) {
                 robot.intake.setPower(-1);
-                double position = spindexer_intakePos1;
+                spindexPos = spindexer_intakePos1;
-                robot.spin1.setPosition(position);
-                robot.spin2.setPosition(1 - position);
             } else {
                 robot.intake.setPower(0);
             }
@@ -280,46 +286,7 @@ public class TeleopV2 extends LinearOpMode {
 
             //SHOOTER:
 
-            double penguin = 0;
+            double powPID = flywheel.manageFlywheel((int) vel, robot.shooter1.getCurrentPosition());
-
-            if (ticker % 8 == 0) {
-                penguin = (double) robot.shooterEncoder.getCurrentPosition() / 2048;
-                double stamp = getRuntime();
-                velo1 = -60 * ((penguin - initPos) / (stamp - stamp1));
-                initPos = penguin;
-                stamp1 = stamp;
-            }
-
-            velo = velo1;
-
-            double feed = vel / 4500;
-
-            if (vel > 500) {
-                feed = Math.log((668.39 / (vel + 591.96)) - 0.116) / -4.18;
-            }
-
-            // --- PROPORTIONAL CORRECTION ---
-            double error = vel - velo1;
-            double correction = kP * error;
-
-            // limit how fast power changes (prevents oscillation)
-            correction = Math.max(-maxStep, Math.min(maxStep, correction));
-
-            // --- FINAL MOTOR POWER ---
-            double powPID = feed + correction;
-
-            // clamp to allowed range
-            powPID = Math.max(0, Math.min(1, powPID));
-
-            if (vel - velo > 1000) {
-                powPID = 1;
-            } else if (velo - vel > 1000) {
-                powPID = 0;
-            }
-
-            TELE.addData("PIDPower", powPID);
-
-            TELE.addData("vel", velo1);
 
             robot.shooter1.setPower(powPID);
             robot.shooter2.setPower(powPID);
@@ -355,7 +322,8 @@ public class TeleopV2 extends LinearOpMode {
                 offset -= 360;
             }
 
-            double pos = turrDefault;
+            //TODO: test the camera teleop code
+            double pos = turrDefault + (error/8); // adds the overall error to the default
 
             TELE.addData("offset", offset);
 
@@ -367,14 +335,51 @@ public class TeleopV2 extends LinearOpMode {
                 pos = 0.97;
             }
 
+            if (y < 0.1 && y > -0.1 && x < 0.1 && x > -0.1 && rx < 0.1 && rx > -0.1){ //not moving
+                AprilTagDetection d20 = aprilTagWebcam.getTagById(20);
+                AprilTagDetection d24 = aprilTagWebcam.getTagById(24);
+
+                double bearing = 0.0;
+                if (d20 != null || d24 != null){
+                    if (d20 != null) {
+                        bearing = d20.ftcPose.bearing;
+                    }
+                    if (d24 != null) {
+                        bearing = d24.ftcPose.bearing;
+                    }
+                    overrideTurr = true;
+                    turretPos = servo.getTurrPos() - (bearing/1300);
+                    TELE.addData("Bear", bearing);
+
+                    double bearingCorrection = bearing / 1300;
+
+
+                    // deadband: ignore tiny noise
+                    if (Math.abs(bearing) > 0.3 && camTicker < 8) {
+
+                        // only accumulate if bearing direction is consistent
+                        if (Math.signum(bearingCorrection) == Math.signum(error) || error == 0) {
+                            error += bearingCorrection;
+                        }
+                    }
+
+                    camTicker++;
+
+                }
+
+
+            } else {
+                camTicker = 0;
+                overrideTurr = false;
+
+            }
+
             if (manualTurret) {
                 pos = turrDefault + (manualOffset / 100);
             }
 
             if (!overrideTurr) {
-
+                turretPos = pos;
-                robot.turr1.setPosition(pos);
-                robot.turr2.setPosition(1 - pos);
             }
 
             if (gamepad2.dpad_right) {
@@ -455,12 +460,9 @@ public class TeleopV2 extends LinearOpMode {
                 reject = true;
 
                 if (getRuntime() % 3 > 1.5) {
-                    robot.spin1.setPosition(0);
+                    spindexPos = 1;
-                    robot.spin2.setPosition(1);
                 } else {
-
+                    spindexPos = 0;
-                    robot.spin1.setPosition(1);
-                    robot.spin2.setPosition(0);
                 }
 
                 robot.transferServo.setPosition(transferServo_out);
@@ -478,39 +480,14 @@ public class TeleopV2 extends LinearOpMode {
                     int currentSlot = shootOrder.get(0); // Peek, do NOT remove yet
                     boolean shootingDone = false;
 
-                    AprilTagDetection d20 = aprilTagWebcam.getTagById(20);
-                    AprilTagDetection d24 = aprilTagWebcam.getTagById(24);
-
-                    if (d20 != null) {
-                        overrideTurr = true;
-                        double bearing = d20.ftcPose.bearing;
-
-                        double finalPos = robot.turr1.getPosition() - (bearing / 1300);
-                        robot.turr1.setPosition(finalPos);
-                        robot.turr2.setPosition(1 - finalPos);
-
-                        TELE.addData("Bear", bearing);
-
-                    }
-
-                    if (d24 != null) {
-                        overrideTurr = true;
-
-                        double bearing = d24.ftcPose.bearing;
-                        double finalPos = robot.turr1.getPosition() - (bearing / 1300);
-                        robot.turr1.setPosition(finalPos);
-                        robot.turr2.setPosition(1 - finalPos);
-
-                    }
-
                     if (!outtake1) {
-                        outtake1 = (spindexPosEqual(spindexer_outtakeBall1));
+                        outtake1 = (servo.spinEqual(spindexer_outtakeBall1));
                     }
                     if (!outtake2) {
-                        outtake2 = (spindexPosEqual(spindexer_outtakeBall2));
+                        outtake2 = (servo.spinEqual(spindexer_outtakeBall2));
                     }
                     if (!outtake3) {
-                        outtake3 = (spindexPosEqual(spindexer_outtakeBall3));
+                        outtake3 = (servo.spinEqual(spindexer_outtakeBall3));
                     }
 
                     switch (currentSlot) {
@@ -553,8 +530,7 @@ public class TeleopV2 extends LinearOpMode {
 
                 } else {
                     // Finished shooting all balls
-                    robot.spin1.setPosition(spindexer_intakePos1);
+                    spindexPos = spindexer_intakePos1;
-                    robot.spin2.setPosition(1 - spindexer_intakePos1);
                     shootA = true;
                     shootB = true;
                     shootC = true;
@@ -767,15 +743,9 @@ public class TeleopV2 extends LinearOpMode {
         return countTrue > countWindow / 2.0; // more than 50% true
     }
 
-    boolean spindexPosEqual(double spindexer) {
-        return (scalar * ((robot.spin1Pos.getVoltage() - restPos) / 3.3) > spindexer - 0.01 &&
-                scalar * ((robot.spin1Pos.getVoltage() - restPos) / 3.3) < spindexer + 0.01);
-    }
-
     public boolean shootTeleop(double spindexer, boolean spinOk, double stamp) {
         // Set spin positions
-        robot.spin1.setPosition(spindexer);
+        spindexPos = spindexer;
-        robot.spin2.setPosition(1 - spindexer);
 
         // Check if spindexer has reached the target position
         if (spinOk || getRuntime() - stamp > 1.5) {
@@ -930,9 +900,4 @@ public class TeleopV2 extends LinearOpMode {
         }
         return true; // default
     }
-
-    public double turretPos() {
-        return (scalar * ((robot.turr1Pos.getVoltage() - restPos) / 3.3));
-    }
-
 }

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

@@ -0,0 +1,55 @@
+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.robotcore.external.navigation.DistanceUnit;
+import org.firstinspires.ftc.teamcode.utils.Robot;
+
+@Config
+@TeleOp
+public class ColorTest extends LinearOpMode {
+    Robot robot;
+    MultipleTelemetry TELE;
+    @Override
+    public void runOpMode() throws InterruptedException {
+        robot = new Robot(hardwareMap);
+        TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+
+        waitForStart();
+        if (isStopRequested()) return;
+
+        while(opModeIsActive()){
+            double green1 = robot.color1.getNormalizedColors().green;
+            double blue1 = robot.color1.getNormalizedColors().blue;
+            double red1 = robot.color1.getNormalizedColors().red;
+
+            TELE.addData("Color1 toColor", robot.color1.getNormalizedColors().toColor());
+            TELE.addData("Color1 green", green1 / (green1 + blue1 + red1));
+            TELE.addData("Color1 distance (mm)", robot.color1.getDistance(DistanceUnit.MM));
+
+// ----- COLOR 2 -----
+            double green2 = robot.color2.getNormalizedColors().green;
+            double blue2 = robot.color2.getNormalizedColors().blue;
+            double red2 = robot.color2.getNormalizedColors().red;
+
+            TELE.addData("Color2 toColor", robot.color2.getNormalizedColors().toColor());
+            TELE.addData("Color2 green", green2 / (green2 + blue2 + red2));
+            TELE.addData("Color2 distance (mm)", robot.color2.getDistance(DistanceUnit.MM));
+
+// ----- COLOR 3 -----
+            double green3 = robot.color3.getNormalizedColors().green;
+            double blue3 = robot.color3.getNormalizedColors().blue;
+            double red3 = robot.color3.getNormalizedColors().red;
+
+            TELE.addData("Color3 toColor", robot.color3.getNormalizedColors().toColor());
+            TELE.addData("Color3 green", green3 / (green3 + blue3 + red3));
+            TELE.addData("Color3 distance (mm)", robot.color3.getDistance(DistanceUnit.MM));
+
+            TELE.update();
+        }
+    }
+}

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

@@ -0,0 +1,224 @@
+package org.firstinspires.ftc.teamcode.tests;
+
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.hardware.lynx.LynxModule;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+import org.firstinspires.ftc.teamcode.utils.Robot;
+import org.firstinspires.ftc.teamcode.utils.Servos;
+
+import java.util.ArrayList;
+import java.util.List;
+
+@Config
+@TeleOp
+public class IntakeTest extends LinearOpMode {
+    Robot robot;
+    MultipleTelemetry TELE;
+    Servos servo;
+    public boolean green1 = false;
+    public boolean green2 = false;
+    public boolean green3 = false;
+    List<Double> s1G = new ArrayList<>();
+    List<Double> s2G = new ArrayList<>();
+    List<Double> s3G = new ArrayList<>();
+    List<Double> s1T = new ArrayList<>();
+    List<Double> s2T = new ArrayList<>();
+    List<Double> s3T = new ArrayList<>();
+    List<Boolean> s1 = new ArrayList<>();
+    List<Boolean> s2 = new ArrayList<>();
+    List<Boolean> s3 = new ArrayList<>();
+    public static int mode = 0; // 0 for teleop, 1 for auto
+    public static double manualPow = 0.15;
+    double stamp = 0;
+    int ticker = 0;
+    boolean steadySpin = false;
+    double powPID = 0.0;
+    boolean intake = true;
+    double spindexerPos = spindexer_intakePos1;
+    boolean wasMoving = false;
+    double currentPos = 0.0;
+    double initPos = 0.0;
+    boolean reverse = false;
+    @Override
+    public void runOpMode() throws InterruptedException {
+        List<LynxModule> allHubs = hardwareMap.getAll(LynxModule.class);
+        for (LynxModule hub : allHubs) {
+            hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
+        }
+        robot = new Robot(hardwareMap);
+        servo = new Servos(hardwareMap);
+        TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        while (opModeIsActive()) {
+            if (mode == 0) {
+                if (gamepad1.right_bumper) {
+                    ticker++;
+                    if (ticker % 16 == 0){
+                        currentPos = servo.getSpinPos();
+                        if (Math.abs(currentPos - initPos) == 0.0){
+                            reverse = !reverse;
+                        }
+                        initPos = currentPos;
+                    }
+                    if (reverse){
+                        robot.spin1.setPower(manualPow);
+                        robot.spin2.setPower(-manualPow);
+                    } else {
+                        robot.spin1.setPower(-manualPow);
+                        robot.spin2.setPower(manualPow);
+                    }
+                    robot.intake.setPower(1);
+                    stamp = getRuntime();
+                    TELE.addData("Reverse?", reverse);
+                    TELE.update();
+                } else {
+                    robot.spin1.setPower(0);
+                    robot.spin2.setPower(0);
+
+                    if (getRuntime() - stamp < 0.5) {
+                        robot.intake.setPower(-1);
+                    } else {
+                        robot.intake.setPower(0);
+                    }
+
+                    ticker = 0;
+                }
+            } else if (mode == 1) {
+
+                if (gamepad1.right_bumper && intake){
+                    robot.intake.setPower(1);
+                } else if (gamepad1.left_bumper){
+                    robot.intake.setPower(-1);
+                } else {
+                    robot.intake.setPower(0);
+                }
+
+                colorDetect();
+                spindexer();
+
+                if (ballIn(1) && steadySpin && intake && getRuntime() - stamp > 0.5){
+                    if (!ballIn(2)){
+                        if (servo.spinEqual(spindexer_intakePos1)){
+                            spindexerPos = spindexer_intakePos2;
+                        } else if (servo.spinEqual(spindexer_intakePos2)){
+                            spindexerPos = spindexer_intakePos3;
+                        } else if (servo.spinEqual(spindexer_intakePos3)){
+                            spindexerPos = spindexer_intakePos1;
+                        }
+                    } else if (!ballIn(3)){
+                        if (servo.spinEqual(spindexer_intakePos1)){
+                            spindexerPos = spindexer_intakePos3;
+                        } else if (servo.spinEqual(spindexer_intakePos2)){
+                            spindexerPos = spindexer_intakePos1;
+                        } else if (servo.spinEqual(spindexer_intakePos3)){
+                            spindexerPos = spindexer_intakePos2;
+                        }
+                    }
+                }
+
+            } else if (mode == 2){ // switch to this mode before switching modes or to reset balls
+                powPID = 0;
+                spindexerPos = spindexer_intakePos1;
+                stamp = getRuntime();
+                ticker = 0;
+                spindexer();
+                intake = true;
+            }
+            for (LynxModule hub : allHubs) {
+                hub.clearBulkCache();
+            }
+            double y = -gamepad1.right_stick_y; // Remember, Y stick value is reversed
+            double x = gamepad1.right_stick_x * 1.1; // Counteract imperfect strafing
+            double rx = gamepad1.left_stick_x;
+
+            double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
+            double frontLeftPower = (y + x + rx) / denominator;
+            double backLeftPower = (y - x + rx) / denominator;
+            double frontRightPower = (y - x - rx) / denominator;
+            double backRightPower = (y + x - rx) / denominator;
+
+            robot.frontLeft.setPower(frontLeftPower);
+            robot.backLeft.setPower(backLeftPower);
+            robot.frontRight.setPower(frontRightPower);
+            robot.backRight.setPower(backRightPower);
+
+            TELE.addData("Manual Power", manualPow);
+            TELE.addData("PID Power", powPID);
+            TELE.addData("B1", ballIn(1));
+            TELE.addData("B2", ballIn(2));
+            TELE.addData("B3", ballIn(3));
+            TELE.addData("Spindex Pos", servo.getSpinPos());
+            TELE.update();
+        }
+    }
+
+    public void colorDetect() {
+        double s1D = robot.color1.getDistance(DistanceUnit.MM);
+        double s2D = robot.color2.getDistance(DistanceUnit.MM);
+        double s3D = robot.color3.getDistance(DistanceUnit.MM);
+
+        TELE.addData("Color 1 Distance", s1D);
+        TELE.addData("Color 2 Distance", s2D);
+        TELE.addData("Color 3 Distance", s3D);
+        TELE.update();
+
+        if (s1D < 43) {
+            s1T.add(getRuntime());
+        }
+
+        if (s2D < 60) {
+            s2T.add(getRuntime());
+        }
+
+        if (s3D < 33) {
+            s3T.add(getRuntime());
+        }
+    }
+
+    public void spindexer() {
+        boolean atTarget = servo.spinEqual(spindexerPos);
+
+        if (!atTarget) {
+            powPID = servo.setSpinPos(spindexerPos);
+            robot.spin1.setPower(powPID);
+            robot.spin2.setPower(-powPID);
+
+            steadySpin = false;
+            wasMoving = true;   // remember we were moving
+            stamp = getRuntime();
+        } else {
+            robot.spin1.setPower(0);
+            robot.spin2.setPower(0);
+            steadySpin = true;
+            wasMoving = false;
+        }
+    }
+
+
+    boolean ballIn(int slot) {
+        List<Double> times;
+
+        if (slot == 1) {times = s1T;}
+        else if (slot == 2) {times = s2T;}
+        else if (slot == 3) {times = s3T;}
+        else return false;
+
+        if (!times.isEmpty()){
+            return times.get(times.size() - 1) > getRuntime() - 2;
+        } else {
+            return false;
+        }
+    }
+
+}

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

@@ -0,0 +1,77 @@
+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.hardware.limelightvision.LLResult;
+import com.qualcomm.hardware.limelightvision.LLResultTypes;
+import com.qualcomm.hardware.limelightvision.Limelight3A;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import java.util.List;
+
+@Config
+@TeleOp
+//TODO: fix to get the apriltag that it is reading
+public class LimelightTest extends LinearOpMode {
+    MultipleTelemetry TELE;
+    public static int pipeline = 0; //0 is for test; 1 for obelisk; 2 is for blue track; 3 is for red track
+    public static int mode = 0; //0 for bare testing, 1 for obelisk, 2 for blue track, 3 for red track
+    @Override
+    public void runOpMode() throws InterruptedException {
+        Limelight3A limelight = hardwareMap.get(Limelight3A.class, "Limelight");
+        TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+        limelight.pipelineSwitch(pipeline);
+        waitForStart();
+        if (isStopRequested()) return;
+        limelight.start();
+        while (opModeIsActive()){
+            if (mode == 0){
+                limelight.pipelineSwitch(pipeline);
+                LLResult result = limelight.getLatestResult();
+                if (result != null) {
+                    if (result.isValid()) {
+                        TELE.addData("tx", result.getTx());
+                        TELE.addData("ty", result.getTy());
+                        TELE.update();
+                    }
+                }
+            } else if (mode == 1){
+                limelight.pipelineSwitch(1);
+                LLResult result = limelight.getLatestResult();
+                if (result != null && result.isValid()) {
+                    List<LLResultTypes.FiducialResult> fiducials = result.getFiducialResults();
+                    for (LLResultTypes.FiducialResult fiducial : fiducials) {
+                        int id = fiducial.getFiducialId();
+                        TELE.addData("ID", id);
+                        TELE.update();
+                    }
+
+                }
+            } else if (mode == 2){
+                limelight.pipelineSwitch(4);
+                LLResult result = limelight.getLatestResult();
+                if (result != null) {
+                    if (result.isValid()) {
+                        TELE.addData("tx", result.getTx());
+                        TELE.addData("ty", result.getTy());
+                        TELE.update();
+                    }
+                }
+            } else if (mode == 3){
+                limelight.pipelineSwitch(5);
+                LLResult result = limelight.getLatestResult();
+                if (result != null) {
+                    if (result.isValid()) {
+                        TELE.addData("tx", result.getTx());
+                        TELE.addData("ty", result.getTy());
+                        TELE.update();
+                    }
+                }
+            } else {
+                limelight.pipelineSwitch(0);
+            }
+        }
+    }
+}

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

@@ -0,0 +1,74 @@
+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.arcrobotics.ftclib.controller.PIDFController;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.CRServo;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+
+import org.firstinspires.ftc.teamcode.utils.Robot;
+
+@TeleOp
+@Config
+public class PIDServoTest extends LinearOpMode {
+
+    public static double p = 2, i = 0, d = 0, f = 0;
+
+    public static double target = 0.5;
+
+    public static int mode = 0; //0 is for turret, 1 is for spindexer
+
+    public static double scalar = 1.01;
+    public static double restPos = 0.0;
+
+    Robot robot;
+
+    double pos = 0.0;
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+
+        PIDFController controller = new PIDFController(p, i, d, f);
+
+        controller.setTolerance(0);
+        robot = new Robot(hardwareMap);
+
+        telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        while (opModeIsActive()) {
+            controller.setPIDF(p, i, d, f);
+
+            if (mode == 0) {
+                pos = scalar * ((robot.turr1Pos.getVoltage() - restPos) / 3.3);
+
+                double pid = controller.calculate(pos, target);
+
+                robot.turr1.setPower(pid);
+                robot.turr2.setPower(-pid);
+            } else if (mode == 1) {
+                pos = scalar * ((robot.spin1Pos.getVoltage() - restPos) / 3.3);
+
+                double pid = controller.calculate(pos, target);
+
+                robot.spin1.setPower(pid);
+                robot.spin2.setPower(-pid);
+            }
+
+            telemetry.addData("pos", pos);
+            telemetry.addData("Turret Voltage", robot.turr1Pos.getVoltage());
+            telemetry.addData("Spindex Voltage", robot.spin1Pos.getVoltage());
+            telemetry.addData("target", target);
+            telemetry.addData("Mode", mode);
+            telemetry.update();
+
+        }
+
+    }
+}

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

@@ -1,5 +1,7 @@
 package org.firstinspires.ftc.teamcode.tests;
 
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
+
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
@@ -7,6 +9,7 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
 
+import org.firstinspires.ftc.teamcode.utils.Flywheel;
 import org.firstinspires.ftc.teamcode.utils.Robot;
 
 @Config
@@ -16,18 +19,14 @@ public class ShooterTest extends LinearOpMode {
     public static int mode = 0;
     public static double parameter = 0.0;
     // --- CONSTANTS YOU TUNE ---
-    public static double MAX_RPM = 4500;      // your measured max RPM
-    public static double kP = 0.001;           // small proportional gain (tune this)
-    public static double maxStep = 0.06;         // prevents sudden jumps
 
     //TODO: @Daniel FIX THE BELOW CONSTANTS A LITTLE IF NEEDED
-    public static double transferPower = 0.0;
+    public static double transferPower = 1.0;
     public static double hoodPos = 0.501;
-
     public static double turretPos = 0.501;
+    public static boolean shoot = false;
     Robot robot;
-    private double lastEncoderRevolutions = 0.0;
+    Flywheel flywheel;
-    private double lastTimeStamp = 0.0;
 
     @Override
     public void runOpMode() throws InterruptedException {
@@ -35,7 +34,7 @@ public class ShooterTest extends LinearOpMode {
         robot = new Robot(hardwareMap);
         DcMotorEx leftShooter = robot.shooter1;
         DcMotorEx rightShooter = robot.shooter2;
-        DcMotorEx encoder = robot.shooter1;
+        flywheel = new Flywheel();
 
         MultipleTelemetry TELE = new MultipleTelemetry(
                 telemetry, FtcDashboard.getInstance().getTelemetry()
@@ -47,60 +46,35 @@ public class ShooterTest extends LinearOpMode {
 
         while (opModeIsActive()) {
 
-            double kF = 1.0 / MAX_RPM;     // baseline feedforward
-
-            double encoderRevolutions = (double) encoder.getCurrentPosition() / 2048;
-
-            double velocity = -60 * (encoderRevolutions - lastEncoderRevolutions) / (getRuntime() - lastTimeStamp);
-
-            TELE.addLine("Mode: 0 = Manual, 1 = Vel, 2 = Pos");
-            TELE.addLine("Parameter = pow, vel, or pos");
-            TELE.addData("leftShooterPower", leftShooter.getPower());
-            TELE.addData("rightShooterPower", rightShooter.getPower());
-            TELE.addData("shaftEncoderPos", encoderRevolutions);
-            TELE.addData("shaftEncoderVel", velocity);
-
-            double velPID;
-
             if (mode == 0) {
                 rightShooter.setPower(parameter);
                 leftShooter.setPower(parameter);
             } else if (mode == 1) {
-
+                double powPID = flywheel.manageFlywheel((int) parameter, robot.shooter1.getCurrentPosition());
-                // --- FEEDFORWARD BASE POWER ---
+                rightShooter.setPower(powPID);
-                double feed = kF * parameter;        // Example: vel=2500 → feed=0.5
+                leftShooter.setPower(powPID);
-
+                TELE.addData("PIDPower", powPID);
-                // --- PROPORTIONAL CORRECTION ---
-                double error = parameter - velocity;
-                double correction = kP * error;
-
-                // limit how fast power changes (prevents oscillation)
-                correction = Math.max(-maxStep, Math.min(maxStep, correction));
-
-                // --- FINAL MOTOR POWER ---
-                velPID = feed + correction;
-
-                // clamp to allowed range
-                velPID = Math.max(0, Math.min(1, velPID));
-
-                rightShooter.setPower(velPID);
-                leftShooter.setPower(velPID);
-
             }
 
-            lastTimeStamp = getRuntime();
-            lastEncoderRevolutions = (double) encoder.getCurrentPosition() / 2048;
-
             if (hoodPos != 0.501) {
                 robot.hood.setPosition(hoodPos);
             }
 
-            if (turretPos!=0.501){
+            if (turretPos != 0.501) {
-                robot.turr1.setPosition(turretPos);
+                robot.turr1.setPower(turretPos);
-                robot.turr2.setPosition(turretPos);
+                robot.turr2.setPower(turretPos);
             }
 
             robot.transfer.setPower(transferPower);
+            if (shoot) {
+                robot.transferServo.setPosition(transferServo_in);
+            } else {
+                robot.transferServo.setPosition(transferServo_out);
+            }
+            TELE.addData("Velocity", flywheel.getVelo());
+            TELE.addData("Power", robot.shooter1.getPower());
+            TELE.addData("Steady?", flywheel.getSteady());
+            TELE.addData("Position", robot.shooter1.getCurrentPosition());
 
             TELE.update();
 

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

@@ -7,21 +7,22 @@ import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 
 public class Flywheel {
     Robot robot;
-
     MultipleTelemetry TELE;
+
     double initPos = 0.0;
     double stamp = 0.0;
     double stamp1 = 0.0;
     double ticker = 0.0;
-    double stamp2 = 0.0;
     double currentPos = 0.0;
-    boolean prevSteady = false;
     double velo = 0.0;
-    double prevVelo = 0.0;
+    double velo1 = 0.0;
+    double velo2 = 0.0;
+    double velo3 = 0.0;
+    double velo4 = 0.0;
+    double velo5 = 0.0;
     double targetVelocity = 0.0;
     double powPID = 0.0;
     boolean steady = false;
-
     public Flywheel () {
         //robot = new Robot(hardwareMap);
     }
@@ -41,15 +42,22 @@ public class Flywheel {
 
 
     public double manageFlywheel(int commandedVelocity, double shooter1CurPos) {
-        targetVelocity = (double) commandedVelocity;
+        targetVelocity = commandedVelocity;
 
         ticker++;
-        if (ticker % 8 == 0) {
+        if (ticker % 2 == 0) {
+            velo5 = velo4;
+            velo4 = velo3;
+            velo3 = velo2;
+            velo2 = velo1;
+
             currentPos = shooter1CurPos / 2048;
             stamp = getTimeSeconds(); //getRuntime();
-            velo = -60 * ((currentPos - initPos) / (stamp - stamp1));
+            velo1 = -60 * ((currentPos - initPos) / (stamp - stamp1));
             initPos = currentPos;
             stamp1 = stamp;
+
+            velo = (velo1 + velo2 + velo3 + velo4 + velo5) / 5;
         }
         // Flywheel control code here
         if (targetVelocity - velo > 500) {
@@ -74,19 +82,12 @@ public class Flywheel {
         }
 
         // really should be a running average of the last 5
-        if ((Math.abs(targetVelocity - velo) < 150.0) && (Math.abs(targetVelocity - prevVelo) < 150.0))
+        steady = (Math.abs(targetVelocity - velo) < 100.0);
-        {
-            steady = true;
-        }
-        else
-        {
-            steady = false;
-        }
 
         return powPID;
     }
 
     public void update()
     {
-    };
+    }
-};
+}

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

@@ -1,5 +1,7 @@
 package org.firstinspires.ftc.teamcode.utils;
 
+import static org.firstinspires.ftc.teamcode.tests.PIDServoTest.*;
+
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
@@ -11,27 +13,47 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 public class PositionalServoProgrammer extends LinearOpMode {
     Robot robot;
     MultipleTelemetry TELE;
+    Servos servo;
+
     public static double spindexPos = 0.501;
+    public static double spindexPow = 0.0;
+    public static double spinHoldPow = 0.0;
     public static double turretPos = 0.501;
+    public static double turretPow = 0.0;
+    public static double turrHoldPow = 0.0;
     public static double transferPos = 0.501;
     public static double hoodPos = 0.501;
+    public static int mode = 0; //0 for positional, 1 for power
 
-    public static double scalar = 1.112;
-    public static double restPos = 0.15;
     @Override
     public void runOpMode() throws InterruptedException {
         robot = new Robot(hardwareMap);
         TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+        servo = new Servos(hardwareMap);
         waitForStart();
         if (isStopRequested()) return;
         while (opModeIsActive()){
-            if (spindexPos != 0.501){
+            if (spindexPos != 0.501 && !servo.spinEqual(spindexPos) && mode == 0){
-                robot.spin1.setPosition(spindexPos);
+                double pos = servo.setSpinPos(spindexPos);
-                robot.spin2.setPosition(1-spindexPos);
+                robot.spin1.setPower(pos);
+                robot.spin2.setPower(-pos);
+            } else if (mode == 0){
+                robot.spin1.setPower(spinHoldPow);
+                robot.spin2.setPower(spinHoldPow);
+            } else {
+                robot.spin1.setPower(spindexPow);
+                robot.spin2.setPower(-spindexPow);
             }
-            if (turretPos != 0.501){
+            if (turretPos != 0.501 && !servo.turretEqual(turretPos)){
-                robot.turr1.setPosition(turretPos);
+                double pos = servo.setTurrPos(turretPos);
-                robot.turr2.setPosition(1-turretPos);
+                robot.turr1.setPower(pos);
+                robot.turr2.setPower(-pos);
+            } else if (mode == 0){
+                robot.turr1.setPower(turrHoldPow);
+                robot.turr2.setPower(turrHoldPow);
+            } else {
+                robot.turr1.setPower(turretPow);
+                robot.turr2.setPower(-turretPow);
             }
             if (transferPos != 0.501){
                 robot.transferServo.setPosition(transferPos);
@@ -39,14 +61,13 @@ public class PositionalServoProgrammer extends LinearOpMode {
             if (hoodPos != 0.501){
                 robot.hood.setPosition(hoodPos);
             }
-            TELE.addData("spindexer", scalar*((robot.spin1Pos.getVoltage() - restPos) / 3.3));
+            TELE.addData("spindexer", servo.getSpinPos());
-            TELE.addData("hood", 1-scalar*((robot.hoodPos.getVoltage() - restPos) / 3.3));
+            TELE.addData("turret", servo.getTurrPos());
-            TELE.addData("transferServo", scalar*((robot.transferServoPos.getVoltage() - restPos) / 3.3));
+            TELE.addData("spindexer voltage", robot.spin1Pos.getVoltage());
-            TELE.addData("turret", scalar*((robot.turr1Pos.getVoltage() - restPos) / 3.3));
+            TELE.addData("hood voltage", robot.hoodPos.getVoltage());
-            TELE.addData("spindexerA", robot.spin1Pos.getVoltage());
+            TELE.addData("transferServo voltage", robot.transferServoPos.getVoltage());
-            TELE.addData("hoodA", robot.hoodPos.getVoltage());
+            TELE.addData("turret voltage", robot.turr1Pos.getVoltage());
-            TELE.addData("transferServoA", robot.transferServoPos.getVoltage());
+            TELE.addData("Spin Equal", servo.spinEqual(spindexPos));
-            TELE.addData("turretA", robot.turr1Pos.getVoltage());
             TELE.update();
         }
     }

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

@@ -1,7 +1,9 @@
 package org.firstinspires.ftc.teamcode.utils;
 
+import com.qualcomm.hardware.limelightvision.Limelight3A;
 import com.qualcomm.hardware.rev.RevColorSensorV3;
 import com.qualcomm.robotcore.hardware.AnalogInput;
+import com.qualcomm.robotcore.hardware.CRServo;
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
 import com.qualcomm.robotcore.hardware.DcMotorSimple;
@@ -18,67 +20,40 @@ public class Robot {
 
     public DcMotorEx frontLeft;
     public DcMotorEx frontRight;
-
     public DcMotorEx backLeft;
-
     public DcMotorEx backRight;
-
     public DcMotorEx intake;
-
     public DcMotorEx transfer;
-
     public DcMotorEx shooter1;
     public DcMotorEx shooter2;
     public Servo hood;
-
     public Servo transferServo;
-
     public Servo rejecter;
-
+    public CRServo turr1;
-    public Servo turr1;
+    public CRServo turr2;
-
+    public CRServo spin1;
-    public Servo turr2;
+    public CRServo spin2;
-
-    public Servo spin1;
-
-    public Servo spin2;
-
     public DigitalChannel pin0;
-
     public DigitalChannel pin1;
     public DigitalChannel pin2;
     public DigitalChannel pin3;
     public DigitalChannel pin4;
-
     public DigitalChannel pin5;
-
     public AnalogInput analogInput;
-
     public AnalogInput analogInput2;
-
     public AnalogInput spin1Pos;
-
     public AnalogInput spin2Pos;
-
     public AnalogInput hoodPos;
-
     public AnalogInput turr1Pos;
-
     public AnalogInput turr2Pos;
-
     public AnalogInput transferServoPos;
-
     public AprilTagProcessor aprilTagProcessor;
-
     public WebcamName webcam;
-
     public DcMotorEx shooterEncoder;
-
     public RevColorSensorV3 color1;
-
     public RevColorSensorV3 color2;
-
     public RevColorSensorV3 color3;
+    public Limelight3A limelight;
 
     public Robot(HardwareMap hardwareMap) {
 
@@ -104,6 +79,8 @@ public class Robot {
         shooter2 = hardwareMap.get(DcMotorEx.class, "shooter2");
 
         shooter1.setDirection(DcMotorSimple.Direction.REVERSE);
+        shooter1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        shooter2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
 
         shooterEncoder = shooter1;
 
@@ -111,22 +88,25 @@ public class Robot {
 
         hoodPos = hardwareMap.get(AnalogInput.class, "hoodPos");
 
-        turr1 = hardwareMap.get(Servo.class, "t1");
+        turr1 = hardwareMap.get(CRServo.class, "t1");
 
         turr1Pos = hardwareMap.get(AnalogInput.class, "t1Pos");
 
-        turr2 = hardwareMap.get(Servo.class, "t2");
+        turr2 = hardwareMap.get(CRServo.class, "t2");
 
         turr2Pos = hardwareMap.get(AnalogInput.class, "t2Pos");
 
-        spin1 = hardwareMap.get(Servo.class, "spin1");
+        spin1 = hardwareMap.get(CRServo.class, "spin1");
 
         spin1Pos = hardwareMap.get(AnalogInput.class, "spin1Pos");
 
-        spin2 = hardwareMap.get(Servo.class, "spin2");
+        spin2 = hardwareMap.get(CRServo.class, "spin2");
 
         spin2Pos = hardwareMap.get(AnalogInput.class, "spin2Pos");
 
+        spin1.setDirection(DcMotorSimple.Direction.REVERSE);
+        spin2.setDirection(DcMotorSimple.Direction.REVERSE);
+
         pin0 = hardwareMap.get(DigitalChannel.class, "pin0");
 
         pin1 = hardwareMap.get(DigitalChannel.class, "pin1");

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

@@ -0,0 +1,60 @@
+package org.firstinspires.ftc.teamcode.utils;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.arcrobotics.ftclib.controller.PIDFController;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+@Config
+public class Servos {
+    Robot robot;
+
+    PIDFController spinPID;
+
+    PIDFController turretPID;
+
+    //PID constants
+    // TODO: get PIDF constants
+    public static double spinP = 3.4, spinI = 0, spinD = 0.075, spinF = 0.02;
+    public static double turrP = 4.0, turrI = 0.0, turrD = 0.0, turrF = 0.0;
+
+    public static double spin_scalar = 1.0086;
+    public static double spin_restPos = 0.0;
+    public static double turret_scalar = 1.009;
+    public static double turret_restPos = 0.0;
+
+    public Servos(HardwareMap hardwareMap) {
+        robot = new Robot(hardwareMap);
+        spinPID = new PIDFController(spinP, spinI, spinD, spinF);
+        turretPID = new PIDFController(turrP, turrI, turrD, turrF);
+    }
+
+    // In the code below, encoder = robot.servo.getVoltage()
+
+    public double getSpinPos() {
+        return spin_scalar * ((robot.spin1Pos.getVoltage() - spin_restPos) / 3.3);
+    }
+    //TODO: PID warp so 0 and 1 are usable positions
+    public double setSpinPos(double pos) {
+        spinPID.setPIDF(spinP, spinI, spinD, spinF);
+
+        return spinPID.calculate(this.getSpinPos(), pos);
+    }
+
+    public boolean spinEqual(double pos) {
+        return Math.abs(pos - this.getSpinPos()) < 0.02;
+    }
+
+    public double getTurrPos() {
+        return turret_scalar * ((robot.turr1Pos.getVoltage() - turret_restPos) / 3.3);
+    }
+
+    public double setTurrPos(double pos) {
+        turretPID.setPIDF(turrP, turrI, turrD, turrF);
+
+        return spinPID.calculate(this.getTurrPos(), pos);
+    }
+
+    public boolean turretEqual(double pos) {
+        return Math.abs(pos - this.getTurrPos()) < 0.01;
+    }
+}