From 05412940e8de2738f763e4fc8f756ea565506676 Mon Sep 17 00:00:00 2001
From: DanTheMan-byte <danibarrf@gmail.com>
Date: Thu, 1 Jan 2026 22:39:02 -0600
Subject: [PATCH] edited Flywheel.java

---
 .../ftc/teamcode/teleop/TeleopV2.java         | 16 ++-------
 .../ftc/teamcode/tests/ShooterTest.java       |  4 +--
 .../ftc/teamcode/utils/Flywheel.java          | 35 ++++++++++---------
 3 files changed, 23 insertions(+), 32 deletions(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java
index b3f3aa0..2d22a20 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java
+++ b/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.ServoPositions.*;
+import static org.firstinspires.ftc.teamcode.constants.ShooterVars.*;
+import static org.firstinspires.ftc.teamcode.utils.PositionalServoProgrammer.*;
 
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java
index fed29bb..2db3dd3 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java
+++ b/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.ShooterVars.*;
+
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
@@ -17,8 +19,6 @@ public class ShooterTest extends LinearOpMode {
     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;
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 71797d9..7a9ac11 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
@@ -13,15 +13,16 @@ public class Flywheel {
     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()
     {
-    };
-};
\ No newline at end of file
+    }
+}
\ No newline at end of file