From 830c2b1481892f578ddaf50a8fafdd0c02859d94 Mon Sep 17 00:00:00 2001 From: Matt9150 Date: Tue, 10 Feb 2026 00:17:26 -0600 Subject: [PATCH] Further speed up loop times. We are now running 50% faster but need to retune the turret PID and update the shoot all speed everywhere. --- .../ftc/teamcode/utils/Flywheel.java | 24 ++++++++++--------- .../ftc/teamcode/utils/Servos.java | 2 +- .../ftc/teamcode/utils/Spindexer.java | 6 ++--- .../ftc/teamcode/utils/Turret.java | 23 ++++++++++++++---- 4 files changed, 35 insertions(+), 20 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..7cbd38a 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 @@ -12,6 +12,7 @@ public class Flywheel { public double velo1 = 0.0; public double velo2 = 0.0; double targetVelocity = 0.0; + double previousTargetVelocity = 0.0; double powPID = 0.0; boolean steady = false; public Flywheel (HardwareMap hardwareMap) { @@ -55,20 +56,21 @@ 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 + if (Math.abs(targetVelocity - commandedVelocity) > 0.0001) { + 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)); - 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)); - - // Record Current Velocity - velo1 = TPS_to_RPM(robot.shooter1.getVelocity()); - velo2 = TPS_to_RPM(robot.shooter2.getVelocity()); - velo = Math.max(velo1,velo2); + // Record Current Velocity + velo1 = TPS_to_RPM(robot.shooter1.getVelocity()); + velo2 = TPS_to_RPM(robot.shooter2.getVelocity()); + velo = Math.max(velo1, velo2); + } // really should be a running average of the last 5 steady = (Math.abs(targetVelocity - velo) < 200.0); 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 99944e3..e2f611d 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 @@ -44,7 +44,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 d4e0d66..5b14dcf 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; + double shootWaitMax = 4; public Types.Motif desiredMotif = Types.Motif.NONE; // For Use enum RotatedBallPositionNames { @@ -467,7 +468,6 @@ public class Spindexer { break; case SHOOTWAIT: - double shootWaitMax = 4; // Stopping when we get to the new position if (prevIntakeState != currentIntakeState) { if (commandedIntakePosition==2) { @@ -505,8 +505,8 @@ public class Spindexer { if (servos.getSpinPos() > spinEndPos){ currentIntakeState = IntakeState.FINDNEXT; } else { - double spinPos = servos.getSpinCmdPos() + shootAllSpindexerSpeedIncrease; - if (spinPos > spinEndPos + 0.03){ + double spinPos = servos.getSpinCmdPos() + 0.005; //shootAllSpindexerSpeedIncrease; + if (spinPos > spinEndPos + 0.03) { // 0.03 @ 48ms loops times spinPos = spinEndPos + 0.03; } servos.setSpinPos(spinPos); 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 61e05b7..b8b5416 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 @@ -38,7 +38,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; @@ -56,6 +57,9 @@ public class Turret { private double permanentOffset = 0.0; private PIDController bearingPID; + private double prevTurretPos = 0.0; + private boolean firstTurretPos = true; + public Turret(Robot rob, MultipleTelemetry tele, Limelight3A cam) { this.TELE = tele; this.robot = rob; @@ -74,8 +78,18 @@ public class Turret { } public void manualSetTurret(double pos) { - robot.turr1.setPosition(pos); - robot.turr2.setPosition(1 - pos); + setTurretPos(pos); + } + + public double setTurretPos(double pos) { + if (firstTurretPos || !Servos.servoPosEqual(pos, prevTurretPos)) { + robot.turr1.setPosition(pos); + robot.turr2.setPosition(1 - pos); + firstTurretPos = false; + } + + prevTurretPos = pos; + return pos; } public boolean turretEqual(double pos) { @@ -273,8 +287,7 @@ public class Turret { } // Set servo positions - robot.turr1.setPosition(turretPos + manualOffset); - robot.turr2.setPosition(1.0 - turretPos - manualOffset); + setTurretPos(turretPos + manualOffset); /* ---------------- TELEMETRY ---------------- */