todo

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

@@ -5,7 +5,7 @@ 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;
 
@@ -178,8 +178,8 @@ public class Blue_V2 extends LinearOpMode {
                 TELE.update();
 
                 if (gpp || pgp || ppg){
-                    robot.turr1.setPosition(turret_blue);
-                    robot.turr2.setPosition(1 - turret_blue);
+                    robot.turr1.setPower(turret_blue);
+                    robot.turr2.setPower(1 - turret_blue);
                     return false;
                 } else {
                     return true;
@@ -203,8 +203,8 @@ public class Blue_V2 extends LinearOpMode {
                     position = spindexer_intakePos1 - 0.02;
                 }
 
-                robot.spin1.setPosition(position);
-                robot.spin2.setPosition(1 - position);
+                robot.spin1.setPower(position);
+                robot.spin2.setPower(1 - position);
 
                 if (ticker == 0) {
                     stamp = getRuntime();
@@ -264,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.spin2.setPosition(1-spindexer);
+                robot.spin1.setPower(spindexer);
+                robot.spin2.setPower(1-spindexer);
                 TELE.addData("Velocity", velo);
                 TELE.addLine("spindex");
                 TELE.update();
@@ -380,8 +380,8 @@ public class Blue_V2 extends LinearOpMode {
                 } else {
                     position = spindexer_intakePos1 - 0.02;
                 }
-                robot.spin1.setPosition(position);
-                robot.spin2.setPosition(1 - position);
+                robot.spin1.setPower(position);
+                robot.spin2.setPower(1 - position);
 
                 TELE.addData("Velocity", velo);
                 TELE.addLine("Intaking");
@@ -418,8 +418,8 @@ public class Blue_V2 extends LinearOpMode {
                 } else {
                     position = spindexer_intakePos1 - 0.02;
                 }
-                robot.spin1.setPosition(position);
-                robot.spin2.setPosition(1 - position);
+                robot.spin1.setPower(position);
+                robot.spin2.setPower(1 - position);
 
                 double s1D = robot.color1.getDistance(DistanceUnit.MM);
                 double s2D = robot.color2.getDistance(DistanceUnit.MM);
@@ -541,13 +541,13 @@ public class Blue_V2 extends LinearOpMode {
 
             robot.hood.setPosition(hoodAuto);
 
-            robot.turr1.setPosition(turret_detectBlue);
-            robot.turr2.setPosition(1 - turret_detectBlue);
+            robot.turr1.setPower(turret_detectBlue);
+            robot.turr2.setPower(1 - turret_detectBlue);
 
             robot.transferServo.setPosition(transferServo_out);
 
-            robot.spin1.setPosition(spindexer_intakePos1);
-            robot.spin2.setPosition(1 - spindexer_intakePos1);
+            robot.spin1.setPower(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);
-                    robot.turr2.setPosition(1 - turret_red);
-                    return false;
+                    double turretPID = servo.setTurrPos(turret_red);
+                    robot.turr1.setPower(turretPID);
+                    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 = 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());
+                powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition());
                 velo = flywheel.getVelo();
                 robot.shooter1.setPower(powPID);
                 robot.shooter2.setPower(powPID);
-                robot.spin1.setPosition(spindexer);
-                robot.spin2.setPosition(1-spindexer);
+                robot.spin1.setPower(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) {
-                    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());
+                powPID = flywheel.manageFlywheel(vel, 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.spin2.setPosition(1 - position);
+                robot.spin1.setPower(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.spin2.setPosition(1 - position);
+                robot.spin1.setPower(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.spin2.setPosition(1 - spindexer_intakePos1);
+            robot.spin1.setPower(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/ShooterVars.java

@@ -19,5 +19,5 @@ public class ShooterVars {
     public static double maxStep = 0.06;         // prevents sudden jumps
 
     // VELOCITY CONSTANTS
-    public static int AUTO_CLOSE_VEL = 3050; //3300;
+    public static int AUTO_CLOSE_VEL = 3025; //3300;
 }

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.ServoPositions.spindexer_intakePos1;
-import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1;
-import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2;
-import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3;
-import static org.firstinspires.ftc.teamcode.constants.ServoPositions.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 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.tests.PIDServoTest.*;
 
 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;
-                robot.spin1.setPosition(position);
-                robot.spin2.setPosition(1 - position);
+                spindexPos = spindexer_intakePos1;
             } else {
                 robot.intake.setPower(0);
             }
@@ -280,46 +286,7 @@ public class TeleopV2 extends LinearOpMode {
 
             //SHOOTER:
 
-            double penguin = 0;
-
-            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);
+            double powPID = flywheel.manageFlywheel((int) vel, robot.shooter1.getCurrentPosition());
 
             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,38 @@ 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);
+                    if (camTicker < 8){
+                        error += bearing/1300; //adds error in first 8 frames of seeing the tag to see how much to adjust the default pos
+                    }
+                    camTicker++;
+                }
+
+
+            } else {
+                camTicker = 0;
+            }
+
             if (manualTurret) {
                 pos = turrDefault + (manualOffset / 100);
             }
 
             if (!overrideTurr) {
-
-                robot.turr1.setPosition(pos);
-                robot.turr2.setPosition(1 - pos);
+                turretPos = pos;
             }
 
             if (gamepad2.dpad_right) {
@@ -455,12 +447,9 @@ public class TeleopV2 extends LinearOpMode {
                 reject = true;
 
                 if (getRuntime() % 3 > 1.5) {
-                    robot.spin1.setPosition(0);
-                    robot.spin2.setPosition(1);
+                    spindexPos = 1;
                 } else {
-
-                    robot.spin1.setPosition(1);
-                    robot.spin2.setPosition(0);
+                    spindexPos = 0;
                 }
 
                 robot.transferServo.setPosition(transferServo_out);
@@ -478,39 +467,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 +517,7 @@ public class TeleopV2 extends LinearOpMode {
 
                 } else {
                     // Finished shooting all balls
-                    robot.spin1.setPosition(spindexer_intakePos1);
-                    robot.spin2.setPosition(1 - spindexer_intakePos1);
+                    spindexPos = spindexer_intakePos1;
                     shootA = true;
                     shootB = true;
                     shootC = true;
@@ -767,15 +730,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);
-        robot.spin2.setPosition(1 - spindexer);
+        spindexPos = spindexer;
 
         // Check if spindexer has reached the target position
         if (spinOk || getRuntime() - stamp > 1.5) {
@@ -930,9 +887,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/IntakeTest.java

@@ -0,0 +1,167 @@
+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.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;
+
+@Config
+@TeleOp
+public class IntakeTest extends LinearOpMode {
+    Robot robot;
+    MultipleTelemetry TELE;
+    Servos servo;
+
+    public static int mode = 0; // 0 for teleop, 1 for auto
+    public static double manualPow = 1.0;
+    double stamp = 0;
+    int ticker = 0;
+    boolean b1 = false;
+    boolean b2 = false;
+    boolean b3 = false;
+    boolean steadySpin = false;
+    double powPID = 0.0;
+    double spindexerPos = spindexer_intakePos1;
+    @Override
+    public void runOpMode() throws InterruptedException {
+        robot = new Robot(hardwareMap);
+        servo = new Servos(hardwareMap);
+        TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        while (opModeIsActive()) {
+            //TODO: test tele intake with new spindexer
+            if (mode == 0) {
+                if (gamepad1.cross) {
+                    ticker = 0;
+                    robot.spin1.setPower(manualPow);
+                    robot.spin2.setPower(-manualPow);
+                    robot.intake.setPower(1);
+                } else {
+                    robot.spin1.setPower(0);
+                    robot.spin2.setPower(0);
+                    if (ticker == 0) {
+                        stamp = getRuntime();
+                    }
+                    ticker++;
+                    if (getRuntime() - stamp < 0.5) {
+                        robot.intake.setPower(-1);
+                    } else {
+                        robot.intake.setPower(0);
+                    }
+                }
+            //TODO: test this monstrosity
+            } else if (mode == 1) {
+
+                if (gamepad1.cross){
+                    robot.intake.setPower(1);
+                } else if (gamepad1.circle){
+                    robot.intake.setPower(-1);
+                } else {
+                    robot.intake.setPower(0);
+                }
+
+                colorDetect();
+                spindexer();
+
+                if (b1 && steadySpin && getRuntime() - stamp > 0.5){
+                    if (!b2){
+                        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 (!b3){
+                        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;
+                        }
+                    }
+                }
+
+                powPID = servo.setSpinPos(spindexerPos);
+
+            } else if (mode == 2){ // switch to this mode before switching modes
+                powPID = 0;
+                spindexerPos = spindexer_intakePos1;
+                stamp = getRuntime();
+                ticker = 0;
+            }
+            TELE.addData("Manual Power", manualPow);
+            TELE.addData("PID Power", powPID);
+            TELE.addData("B1", b1);
+            TELE.addData("B2", b2);
+            TELE.addData("B3", b3);
+            TELE.addData("Spindex Pos", servo.getSpinPos());
+        }
+    }
+
+    public void colorDetect() {
+        // ----- COLOR 1 -----
+        double green1 = robot.color1.getNormalizedColors().green;
+        double blue1 = robot.color1.getNormalizedColors().blue;
+        double red1 = robot.color1.getNormalizedColors().red;
+
+        b1 = robot.color1.getDistance(DistanceUnit.MM) < 40;
+
+        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;
+
+        b2 = robot.color2.getDistance(DistanceUnit.MM) < 40;
+
+        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;
+
+        b3 = robot.color3.getDistance(DistanceUnit.MM) < 30;
+
+        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();
+    }
+
+    public void spindexer(){
+        if (!servo.spinEqual(spindexerPos)){
+            robot.spin1.setPower(powPID);
+            robot.spin2.setPower(-powPID);
+            steadySpin = false;
+            ticker = 0;
+        } else{
+            robot.spin1.setPower(0);
+            robot.spin2.setPower(0);
+            steadySpin = true;
+            if (ticker == 0){
+                stamp = getRuntime();
+            }
+            ticker++;
+        }
+    }
+}

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

@@ -0,0 +1,73 @@
+package org.firstinspires.ftc.teamcode.tests;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.hardware.limelightvision.LLResult;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.utils.Robot;
+//TODO: fix to get the apriltag that it is reading
+public class LimelightTest extends LinearOpMode {
+    Robot robot;
+    MultipleTelemetry TELE;
+    public static int pipeline = 0; //0 is for test; 1, 2, and 3 are for obelisk; 4 is for blue track; 5 is for red track
+    public static int mode = 0; //0 for bare testing, 1 for obelisk, 2 for blue track, 3 for red track
+    int obeliskPipe = 1;
+    @Override
+    public void runOpMode() throws InterruptedException {
+        robot = new Robot(hardwareMap);
+        TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+        robot.limelight.pipelineSwitch(pipeline);
+        waitForStart();
+        if (isStopRequested()) return;
+        robot.limelight.start();
+        while (opModeIsActive()){
+            if (mode == 0){
+                robot.limelight.pipelineSwitch(pipeline);
+                LLResult result = robot.limelight.getLatestResult();
+                if (result != null) {
+                    if (result.isValid()) {
+                        TELE.addData("tx", result.getTx());
+                        TELE.addData("ty", result.getTy());
+                        TELE.update();
+                    }
+                }
+            } else if (mode == 1){
+                robot.limelight.pipelineSwitch(obeliskPipe);
+                LLResult result = robot.limelight.getLatestResult();
+                if (result != null && result.isValid()){
+                    TELE.addData("ID", obeliskPipe+20);
+                    TELE.update();
+                } else {
+                    if (obeliskPipe >= 3){
+                        obeliskPipe = 1;
+                    } else {
+                        obeliskPipe++;
+                    }
+                }
+            } else if (mode == 2){
+                robot.limelight.pipelineSwitch(4);
+                LLResult result = robot.limelight.getLatestResult();
+                if (result != null) {
+                    if (result.isValid()) {
+                        TELE.addData("tx", result.getTx());
+                        TELE.addData("ty", result.getTy());
+                        TELE.update();
+                    }
+                }
+            } else if (mode == 3){
+                robot.limelight.pipelineSwitch(5);
+                LLResult result = robot.limelight.getLatestResult();
+                if (result != null) {
+                    if (result.isValid()) {
+                        TELE.addData("tx", result.getTx());
+                        TELE.addData("ty", result.getTy());
+                        TELE.update();
+                    }
+                }
+            } else {
+                robot.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;
-    private double lastTimeStamp = 0.0;
+    Flywheel flywheel;
 
     @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) {
-
-                // --- FEEDFORWARD BASE POWER ---
-                double feed = kF * parameter;        // Example: vel=2500 → feed=0.5
-
-                // --- 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);
-
+                double powPID = flywheel.manageFlywheel((int) parameter, robot.shooter1.getCurrentPosition());
+                rightShooter.setPower(powPID);
+                leftShooter.setPower(powPID);
+                TELE.addData("PIDPower", powPID);
             }
 
-            lastTimeStamp = getRuntime();
-            lastEncoderRevolutions = (double) encoder.getCurrentPosition() / 2048;
-
             if (hoodPos != 0.501) {
                 robot.hood.setPosition(hoodPos);
             }
 
-            if (turretPos!=0.501){
-                robot.turr1.setPosition(turretPos);
-                robot.turr2.setPosition(turretPos);
+            if (turretPos != 0.501) {
+                robot.turr1.setPower(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 = true;
-        }
-        else
-        {
-            steady = false;
-        }
+        steady = (Math.abs(targetVelocity - velo) < 100.0);
 
         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){
-                robot.spin1.setPosition(spindexPos);
-                robot.spin2.setPosition(1-spindexPos);
+            if (spindexPos != 0.501 && !servo.spinEqual(spindexPos) && mode == 0){
+                double pos = servo.setSpinPos(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){
-                robot.turr1.setPosition(turretPos);
-                robot.turr2.setPosition(1-turretPos);
+            if (turretPos != 0.501 && !servo.turretEqual(turretPos)){
+                double pos = servo.setTurrPos(turretPos);
+                robot.turr1.setPower(pos);
+                robot.turr2.setPower(-pos);
+            } else if (mode == 0){
+                robot.turr1.setPower(turrHoldPow);
+                robot.turr2.setPower(turrHoldPow);
+            } else {
+                robot.spin1.setPower(turretPow);
+                robot.spin2.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("hood", 1-scalar*((robot.hoodPos.getVoltage() - restPos) / 3.3));
-            TELE.addData("transferServo", scalar*((robot.transferServoPos.getVoltage() - restPos) / 3.3));
-            TELE.addData("turret", scalar*((robot.turr1Pos.getVoltage() - restPos) / 3.3));
-            TELE.addData("spindexerA", robot.spin1Pos.getVoltage());
-            TELE.addData("hoodA", robot.hoodPos.getVoltage());
-            TELE.addData("transferServoA", robot.transferServoPos.getVoltage());
-            TELE.addData("turretA", robot.turr1Pos.getVoltage());
+            TELE.addData("spindexer", servo.getSpinPos());
+            TELE.addData("turret", servo.getTurrPos());
+            TELE.addData("spindexer voltage", robot.spin1Pos.getVoltage());
+            TELE.addData("hood voltage", robot.hoodPos.getVoltage());
+            TELE.addData("transferServo voltage", robot.transferServoPos.getVoltage());
+            TELE.addData("turret voltage", robot.turr1Pos.getVoltage());
+            TELE.addData("Spin Equal", servo.spinEqual(spindexPos));
             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;
@@ -35,13 +37,13 @@ public class Robot {
 
     public Servo rejecter;
 
-    public Servo turr1;
+    public CRServo turr1;
 
-    public Servo turr2;
+    public CRServo turr2;
 
-    public Servo spin1;
+    public CRServo spin1;
 
-    public Servo spin2;
+    public CRServo spin2;
 
     public DigitalChannel pin0;
 
@@ -80,6 +82,8 @@ public class Robot {
 
     public RevColorSensorV3 color3;
 
+    public Limelight3A limelight;
+
     public Robot(HardwareMap hardwareMap) {
 
         //Define components w/ hardware map
@@ -104,6 +108,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,19 +117,19 @@ 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");
 
@@ -160,5 +166,7 @@ public class Robot {
         color2 = hardwareMap.get(RevColorSensorV3.class, "c2");
 
         color3 = hardwareMap.get(RevColorSensorV3.class, "c3");
+
+        limelight = hardwareMap.get(Limelight3A.class,"Limelight");
     }
 }

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

@@ -0,0 +1,62 @@
+package org.firstinspires.ftc.teamcode.utils;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.arcrobotics.ftclib.controller.PIDFController;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.teamcode.utils.Robot;
+
+@Config
+public class Servos {
+    Robot robot;
+
+    PIDFController spinPID;
+
+    PIDFController turretPID;
+
+    //PID constants
+    // TODO: get PIDF constants
+    public static double spinP = 2.85, spinI = 0.015, spinD = 0.09, spinF = 0.03;
+    public static double turrP = 4.0, turrI = 0.0, turrD = 0.0, turrF = 0.0;
+
+    public static double spin_scalar = 1.011;
+    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.01;
+    }
+
+    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;
+    }
+}