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 5078b0c..385086e 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 @@ -51,7 +51,7 @@ public class ShooterTest extends LinearOpMode { public static int spindexPos = 1; - public static int initTolerance = 1000; + public static int initTolerance = 800; public static boolean intake = true; @@ -138,6 +138,20 @@ public class ShooterTest extends LinearOpMode { if (powPID > 1.0){ powPID = 1.0; } + 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 --- + powPID = feed + correction; + + // clamp to allowed range + powPID = Math.max(0, Math.min(1, powPID)); shooter.setVelocity(powPID); if (shoot){