From 9f5dcb4343dc20bb2cfc997562d375f45ca12495 Mon Sep 17 00:00:00 2001 From: Matt9150 Date: Sat, 7 Feb 2026 16:06:03 -0600 Subject: [PATCH] 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;