From 9f5dcb4343dc20bb2cfc997562d375f45ca12495 Mon Sep 17 00:00:00 2001 From: Matt9150 Date: Sat, 7 Feb 2026 16:06:03 -0600 Subject: [PATCH 1/3] Reduce calls to the spindexer, transfer and color sensors. Add new MeasuringLoopTimes class to measure min, max and avg loop times. --- .../ftc/teamcode/teleop/TeleopV3.java | 24 +++++-- .../teamcode/utils/MeasuringLoopTimes.java | 66 +++++++++++++++++++ .../ftc/teamcode/utils/Servos.java | 34 +++++++++- .../ftc/teamcode/utils/Spindexer.java | 53 +++++++-------- .../ftc/teamcode/utils/Turret.java | 1 + 5 files changed, 142 insertions(+), 36 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/MeasuringLoopTimes.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java index d406445..7057e24 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java @@ -19,6 +19,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; import org.firstinspires.ftc.teamcode.utils.Drivetrain; import org.firstinspires.ftc.teamcode.utils.Flywheel; +import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes; import org.firstinspires.ftc.teamcode.utils.Robot; import org.firstinspires.ftc.teamcode.utils.Servos; import org.firstinspires.ftc.teamcode.utils.Spindexer; @@ -74,6 +75,8 @@ public class TeleopV3 extends LinearOpMode { boolean turretInterpolate = false; private boolean shootAll = false; + public MeasuringLoopTimes measuringLoopTimes; + @Override public void runOpMode() throws InterruptedException { robot = new Robot(hardwareMap); @@ -94,6 +97,9 @@ public class TeleopV3 extends LinearOpMode { drivetrain = new Drivetrain(robot, drive); + measuringLoopTimes = new MeasuringLoopTimes(); + measuringLoopTimes.init(); + PIDFController tController = new PIDFController(tp, ti, td, tf); tController.setTolerance(0.001); @@ -115,7 +121,7 @@ public class TeleopV3 extends LinearOpMode { waitForStart(); if (isStopRequested()) return; - robot.transferServo.setPosition(transferServo_out); + servo.setTransferPos(transferServo_out); while (opModeIsActive()) { @@ -135,7 +141,7 @@ public class TeleopV3 extends LinearOpMode { if (gamepad1.right_bumper) { shootAll = false; - robot.transferServo.setPosition(transferServo_out); + servo.setTransferPos(transferServo_out); } @@ -263,14 +269,14 @@ public class TeleopV3 extends LinearOpMode { spindexer.prepareShootAllContinous(); //TELE.addLine("preparing to shoot"); // } else if (shooterTicker == 2) { -// //robot.transferServo.setPosition(transferServo_in); +// //servo.setTransferPos(transferServo_in); // spindexer.shootAll(); // TELE.addLine("starting to shoot"); } else if (!spindexer.shootAllComplete()) { - robot.transferServo.setPosition(transferServo_in); + servo.setTransferPos(transferServo_in); //TELE.addLine("shoot"); } else { - robot.transferServo.setPosition(transferServo_out); + servo.setTransferPos(transferServo_out); //spindexPos = spindexer_intakePos1; shootAll = false; spindexer.resetSpindexer(); @@ -282,7 +288,7 @@ public class TeleopV3 extends LinearOpMode { } if (gamepad1.left_stick_button) { - robot.transferServo.setPosition(transferServo_out); + servo.setTransferPos(transferServo_out); //spindexPos = spindexer_intakePos1; shootAll = false; @@ -304,6 +310,7 @@ public class TeleopV3 extends LinearOpMode { for (LynxModule hub : allHubs) { hub.clearBulkCache(); } + measuringLoopTimes.loop(); // // TELE.addData("Spin1Green", green1 + ": " + ballIn(1)); // TELE.addData("Spin2Green", green2 + ": " + ballIn(2)); @@ -340,6 +347,11 @@ public class TeleopV3 extends LinearOpMode { TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle); TELE.addData("timeSinceStamp", getRuntime() - shootStamp); + TELE.addData("Avg Loop Time", measuringLoopTimes.getAvgLoopTime()); + TELE.addData("Min Loop Time", measuringLoopTimes.getMinLoopTimeOneMin()); + TELE.addData("Max Loop Time", measuringLoopTimes.getMaxLoopTimeOneMin()); + + TELE.update(); ticker++; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/MeasuringLoopTimes.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/MeasuringLoopTimes.java new file mode 100644 index 0000000..d7d0d1e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/MeasuringLoopTimes.java @@ -0,0 +1,66 @@ +package org.firstinspires.ftc.teamcode.utils; + +import com.qualcomm.robotcore.util.ElapsedTime; + +public class MeasuringLoopTimes { + private ElapsedTime elapsedtime; + private double minLoopTime = 999999999999.0; + + private double maxLoopTime = 0.0; + private double mainLoopTime = 0.0; + + private double MeasurementStart = 0.0; + private double currentTime = 0.0; + + private double avgLoopTime = 0.0; + private int avgLoopTimeTicker = 0; + private double avgLoopTimeSum = 0; + + private double getTimeSeconds () + { + return (double) System.currentTimeMillis()/1000.0; + } + + public void init() { + elapsedtime = new ElapsedTime(); + elapsedtime.reset(); + + MeasurementStart = getTimeSeconds(); + } + + + public double getAvgLoopTime() { + return avgLoopTime; + } + + public double getMaxLoopTimeOneMin() { + return maxLoopTime; + } + + public double getMinLoopTimeOneMin() { + return minLoopTime; + } + + public void loop() { + currentTime = getTimeSeconds(); + if ((MeasurementStart + 15.0) < currentTime) + { + minLoopTime = 9999999.0; + maxLoopTime = 0.0; + MeasurementStart = currentTime; + + avgLoopTime = avgLoopTimeSum / (double) avgLoopTimeTicker; + avgLoopTimeSum = 0.0; + avgLoopTimeTicker = 0; + } + + mainLoopTime = elapsedtime.milliseconds(); + elapsedtime.reset(); + + avgLoopTimeSum += mainLoopTime; + avgLoopTimeTicker++; + minLoopTime = Math.min(minLoopTime,mainLoopTime); + maxLoopTime = Math.max(maxLoopTime,mainLoopTime); + } + +} 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 324a36f..99944e3 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 @@ -18,6 +18,14 @@ public class Servos { PIDFController spinPID; PIDFController turretPID; + private double prevSpinPos = 0.0; + private boolean firstSpinPos = true; + + private double prevTransferPos = 0.0; + private boolean firstTransferPos = true; + + private double spinPos = 0.0; + public Servos(HardwareMap hardwareMap) { robot = new Robot(hardwareMap); spinPID = new PIDFController(spinP, spinI, spinD, spinF); @@ -32,8 +40,32 @@ public class Servos { return spin_scalar * ((robot.spin1Pos.getVoltage() - spin_restPos) / 3.3); } - public double setSpinPos(double pos) { + public double getSpinCmdPos() { + return prevSpinPos; + } + public boolean servoPosEqual(double pos1, double pos2) { + return (Math.abs(pos1 - pos2) < 0.005); + } + + public double setTransferPos(double pos) { + if (firstTransferPos || !servoPosEqual(pos, prevTransferPos)) { + robot.transferServo.setPosition(pos); + firstTransferPos = false; + } + + prevTransferPos = pos; + return pos; + } + + public double setSpinPos(double pos) { + if (firstSpinPos || !servoPosEqual(pos, prevSpinPos)) { + robot.spin1.setPosition(pos); + robot.spin2.setPosition(1-pos); + firstSpinPos = false; + } + + prevSpinPos = pos; return pos; } 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 ea5aa6e..d4e0d66 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 @@ -3,6 +3,8 @@ package org.firstinspires.ftc.teamcode.utils; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.arcrobotics.ftclib.controller.PIDFController; import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.NormalizedColorSensor; +import com.qualcomm.robotcore.hardware.NormalizedRGBA; import static org.firstinspires.ftc.teamcode.constants.Color.*; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.shootAllSpindexerSpeedIncrease; @@ -184,11 +186,9 @@ public class Spindexer { if (detectRearColor) { // Detect which color - double green = robot.color1.getNormalizedColors().green; - double red = robot.color1.getNormalizedColors().red; - double blue = robot.color1.getNormalizedColors().blue; + NormalizedRGBA color1RGBA = robot.color1.getNormalizedColors(); - double gP = green / (green + red + blue); + double gP = color1RGBA.green / (color1RGBA.green + color1RGBA.red + color1RGBA.blue); // FIXIT - Add filtering to improve accuracy. if (gP >= 0.38) { @@ -205,11 +205,9 @@ public class Spindexer { // reset FoundEmpty because looking for 3 in a row before reset ballPositions[spindexerBallPos].foundEmpty = 0; if (detectFrontColor) { - double green = robot.color2.getNormalizedColors().green; - double red = robot.color2.getNormalizedColors().red; - double blue = robot.color2.getNormalizedColors().blue; + NormalizedRGBA color2RGBA = robot.color2.getNormalizedColors(); - double gP = green / (green + red + blue); + double gP = color2RGBA.green / (color2RGBA.green + color2RGBA.red + color2RGBA.blue); if (gP >= 0.4) { ballPositions[spindexerBallPos].ballColor = BallColor.GREEN; // green @@ -234,11 +232,9 @@ public class Spindexer { // reset FoundEmpty because looking for 3 in a row before reset ballPositions[spindexerBallPos].foundEmpty = 0; if (detectFrontColor) { - double green = robot.color3.getNormalizedColors().green; - double red = robot.color3.getNormalizedColors().red; - double blue = robot.color3.getNormalizedColors().blue; + NormalizedRGBA color3RGBA = robot.color3.getNormalizedColors(); - double gP = green / (green + red + blue); + double gP = color3RGBA.green / (color3RGBA.green + color3RGBA.red + color3RGBA.blue); if (gP >= 0.42) { ballPositions[spindexerBallPos].ballColor = BallColor.GREEN; // green @@ -269,8 +265,7 @@ public class Spindexer { } // Has code to unjam spindexer private void moveSpindexerToPos(double pos) { - robot.spin1.setPosition(pos); - robot.spin2.setPosition(1-pos); + servos.setSpinPos(pos); // double currentPos = servos.getSpinPos(); // if (!servos.spinEqual(pos) && Math.abs(prevPos - currentPos) <= 0){ // if (currentPos > pos){ @@ -281,7 +276,7 @@ public class Spindexer { // robot.spin2.setPosition(1 - servos.getSpinPos() + 0.05); // } // } -// prevPos = currentPos; +// prevPos = pos; } public void stopSpindexer() { @@ -325,7 +320,7 @@ public class Spindexer { case UNKNOWN_START: // For now just set position ONE if UNKNOWN commandedIntakePosition = 0; - moveSpindexerToPos(intakePositions[0]); + servos.setSpinPos(intakePositions[0]); currentIntakeState = Spindexer.IntakeState.UNKNOWN_MOVE; break; case UNKNOWN_MOVE: @@ -336,7 +331,7 @@ public class Spindexer { unknownColorDetect = 0; } else { // Keep moving the spindexer - moveSpindexerToPos(intakePositions[commandedIntakePosition]); + servos.setSpinPos(intakePositions[commandedIntakePosition]); } break; case UNKNOWN_DETECT: @@ -355,7 +350,7 @@ public class Spindexer { } else { // Maintain Position spindexerWiggle *= -1.0; - moveSpindexerToPos(intakePositions[commandedIntakePosition]+spindexerWiggle); + servos.setSpinPos(intakePositions[commandedIntakePosition]+spindexerWiggle); } break; case FINDNEXT: @@ -387,7 +382,7 @@ public class Spindexer { //commandedIntakePosition = bestFitMotif(); currentIntakeState = Spindexer.IntakeState.FULL; } - moveSpindexerToPos(intakePositions[commandedIntakePosition]); + servos.setSpinPos(intakePositions[commandedIntakePosition]); break; case MOVING: @@ -403,7 +398,7 @@ public class Spindexer { //detectBalls(false, false); } else { // Keep moving the spindexer - moveSpindexerToPos(intakePositions[commandedIntakePosition]); + servos.setSpinPos(intakePositions[commandedIntakePosition]); } break; @@ -416,7 +411,7 @@ public class Spindexer { } // Maintain Position spindexerWiggle *= -1.0; - moveSpindexerToPos(intakePositions[commandedIntakePosition]+spindexerWiggle); + servos.setSpinPos(intakePositions[commandedIntakePosition]+spindexerWiggle); break; case SHOOT_ALL_PREP: @@ -425,7 +420,7 @@ public class Spindexer { commandedIntakePosition = 0; if (!servos.spinEqual(outakePositions[commandedIntakePosition])) { // Keep moving the spindexer - moveSpindexerToPos(outakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions" + servos.setSpinPos(outakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions" } break; @@ -437,7 +432,7 @@ public class Spindexer { currentIntakeState = Spindexer.IntakeState.SHOOTNEXT; } // Maintain Position - moveSpindexerToPos(outakePositions[commandedIntakePosition]); + servos.setSpinPos(outakePositions[commandedIntakePosition]); break; case SHOOTNEXT: @@ -458,7 +453,7 @@ public class Spindexer { // Empty return to intake state currentIntakeState = IntakeState.FINDNEXT; } - moveSpindexerToPos(outakePositions[commandedIntakePosition]); + servos.setSpinPos(outakePositions[commandedIntakePosition]); break; case SHOOTMOVING: @@ -467,7 +462,7 @@ public class Spindexer { currentIntakeState = Spindexer.IntakeState.SHOOTWAIT; } else { // Keep moving the spindexer - moveSpindexerToPos(outakePositions[commandedIntakePosition]); + servos.setSpinPos(outakePositions[commandedIntakePosition]); } break; @@ -492,14 +487,14 @@ public class Spindexer { } // Keep moving the spindexer spindexerOuttakeWiggle *= -1.0; - moveSpindexerToPos(outakePositions[commandedIntakePosition]+spindexerOuttakeWiggle); + servos.setSpinPos(outakePositions[commandedIntakePosition]+spindexerOuttakeWiggle); break; case SHOOT_PREP_CONTINOUS: if (servos.spinEqual(spinStartPos)){ currentIntakeState = Spindexer.IntakeState.SHOOT_CONTINOUS; } else { - moveSpindexerToPos(spinStartPos); + servos.setSpinPos(spinStartPos); } break; @@ -510,11 +505,11 @@ public class Spindexer { if (servos.getSpinPos() > spinEndPos){ currentIntakeState = IntakeState.FINDNEXT; } else { - double spinPos = robot.spin1.getPosition() + shootAllSpindexerSpeedIncrease; + double spinPos = servos.getSpinCmdPos() + shootAllSpindexerSpeedIncrease; if (spinPos > spinEndPos + 0.03){ spinPos = spinEndPos + 0.03; } - moveSpindexerToPos(spinPos); + servos.setSpinPos(spinPos); } break; 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 9f01c43..61e05b7 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 @@ -11,6 +11,7 @@ import com.qualcomm.hardware.limelightvision.LLResultTypes; import com.qualcomm.hardware.limelightvision.Limelight3A; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.ElapsedTime; import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; From 830c2b1481892f578ddaf50a8fafdd0c02859d94 Mon Sep 17 00:00:00 2001 From: Matt9150 Date: Tue, 10 Feb 2026 00:17:26 -0600 Subject: [PATCH 2/3] 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 ---------------- */ From f5b8b2fd8999672e457c7dbc880471d3bc2c0e8a Mon Sep 17 00:00:00 2001 From: Matt9150 Date: Tue, 10 Feb 2026 20:29:33 -0600 Subject: [PATCH 3/3] Tweak PID for Limelight to improve turret Aiming. --- .../main/java/org/firstinspires/ftc/teamcode/utils/Turret.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 b8b5416..cab0f48 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 @@ -39,7 +39,7 @@ public class Turret { 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.095, B_PID_I = 0.0, B_PID_D = 0.0090; + public static double B_PID_P = 0.066, B_PID_I = 0.0, B_PID_D = 0.007; Robot robot; MultipleTelemetry TELE; Limelight3A webcam;