From 48b5925b15d892c85e585ffb140304215f7cf965 Mon Sep 17 00:00:00 2001
From: DanTheMan-byte <danibarrf@gmail.com>
Date: Tue, 10 Feb 2026 18:37:06 -0600
Subject: [PATCH] added loop time fixes

---
 .../firstinspires/ftc/teamcode/utils/Flywheel.java   | 12 ++++++------
 .../org/firstinspires/ftc/teamcode/utils/Servos.java |  2 +-
 .../firstinspires/ftc/teamcode/utils/Spindexer.java  |  2 +-
 .../org/firstinspires/ftc/teamcode/utils/Turret.java |  3 ++-
 4 files changed, 10 insertions(+), 9 deletions(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java
index b03db69..9a07420 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java
@@ -55,14 +55,14 @@ public class Flywheel {
     private double TPS_to_RPM (double TPS) { return (TPS*60.0)/28.0;}
 
     public double manageFlywheel(double commandedVelocity) {
-        targetVelocity = commandedVelocity;
-
         // Add code here to set PIDF based on desired RPM
 
-        robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1);
-        robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF2);
-        robot.shooter1.setVelocity(RPM_to_TPS(targetVelocity));
-        robot.shooter2.setVelocity(RPM_to_TPS(targetVelocity));
+        if (targetVelocity != commandedVelocity){
+            robot.shooter1.setVelocity(RPM_to_TPS(commandedVelocity));
+            robot.shooter2.setVelocity(RPM_to_TPS(commandedVelocity));
+        }
+
+        targetVelocity = commandedVelocity;
 
         // Record Current Velocity
         velo1 = TPS_to_RPM(robot.shooter1.getVelocity());
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java
index a8a2a00..e150405 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java
@@ -45,7 +45,7 @@ public class Servos {
         return prevSpinPos;
     }
 
-    public boolean servoPosEqual(double pos1, double pos2) {
+    public static boolean servoPosEqual(double pos1, double pos2) {
         return (Math.abs(pos1 - pos2) < 0.005);
     }
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java
index af42039..08a52e8 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java
@@ -49,6 +49,7 @@ public class Spindexer {
     public double spindexerOuttakeWiggle = 0.01;
     private double prevPos = 0.0;
     public double spindexerPosOffset = 0.00;
+    public static int shootWaitMax = 4;
     public Types.Motif desiredMotif = Types.Motif.NONE;
     // For Use
     enum RotatedBallPositionNames {
@@ -474,7 +475,6 @@ public class Spindexer {
                 break;
 
             case SHOOTWAIT:
-                double shootWaitMax = 4;
                 // Stopping when we get to the new position
                 if (prevIntakeState != currentIntakeState) {
                     if (commandedIntakePosition==2) {
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java
index 8ded7eb..a00b76b 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java
@@ -37,7 +37,8 @@ public class Turret {
     // TODO: tune these values for limelight
 
     public static double clampTolerance = 0.03;
-    public static double B_PID_P = 0.105, B_PID_I = 0.0, B_PID_D = 0.0125;
+    //public static double B_PID_P = 0.105, B_PID_I = 0.0, B_PID_D = 0.0125;
+    public static double B_PID_P = 0.095, B_PID_I = 0.0, B_PID_D = 0.0090;
     Robot robot;
     MultipleTelemetry TELE;
     Limelight3A webcam;