diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java index af8bbca..56126d3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java @@ -14,6 +14,7 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.constants.Color; import org.firstinspires.ftc.teamcode.pedroPathing.Constants; +import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes; import org.firstinspires.ftc.teamcode.utilsv2.*; import static org.firstinspires.ftc.teamcode.utilsv2.Turret.limelightUsed; @@ -31,6 +32,7 @@ public class TeleopV4 extends LinearOpMode { Flywheel flywheel; VelocityCommander commander; ParkTilter parkTilter; + MeasuringLoopTimes loopTimes; public static Pose relocalizePose = new Pose(128, 83, 0); @@ -57,6 +59,9 @@ public class TeleopV4 extends LinearOpMode { parkTilter = new ParkTilter(robot); parkTilter.unpark(); + loopTimes = new MeasuringLoopTimes(); + loopTimes.init(); + shooter = new Shooter(robot, TELE, follower, Color.redAlliance, turret, flywheel, commander); shooter.setState(Shooter.ShooterState.TRACK_GOAL); shooter.setRedAlliance(Color.redAlliance); @@ -142,6 +147,11 @@ public class TeleopV4 extends LinearOpMode { parkTilter.unpark(); } + loopTimes.loop(); + TELE.addData("Average Loop Time", loopTimes.getAvgLoopTime()); + TELE.addData("Max Loop Time", loopTimes.getMaxLoopTimeOneMin()); + TELE.addData("Min Loop Time", loopTimes.getMinLoopTimeOneMin()); + TELE.addData("Distance From Goal", commander.getDistance()); TELE.addData("Hood Position", commander.getHoodPredicted()); TELE.addData("Transfer Power", robot.transfer.getPower()); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/SortedSpindexerTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/SortedSpindexerTest.java index 69606da..f34b4fb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/SortedSpindexerTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/SortedSpindexerTest.java @@ -106,22 +106,6 @@ public class SortedSpindexerTest extends LinearOpMode { spindexerTransferIntake.startSortedShoot(); } - if (gamepad1.right_trigger > 0.5 && - (state == SpindexerTransferIntake.RapidMode.INTAKE || - state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF)) { - - spindexerTransferIntake.setRapidMode( - SpindexerTransferIntake.RapidMode.HOLD_BALLS - ); - } - if (gamepad1.rightBumperWasPressed() - && state == SpindexerTransferIntake.RapidMode.HOLD_BALLS) { - - spindexerTransferIntake.setRapidMode( - SpindexerTransferIntake.RapidMode.INTAKE - ); - } - if (gamepad1.dpad_down){ parkTilter.park(); } else if (gamepad1.dpad_up) { 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 index 1d1fe97..186e9f8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/MeasuringLoopTimes.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/MeasuringLoopTimes.java @@ -43,13 +43,12 @@ public class MeasuringLoopTimes { public void loop() { currentTime = getTimeSeconds(); - if ((MeasurementStart + 5.0) < currentTime) + if ((MeasurementStart + 60) < currentTime) { minLoopTime = 9999999.0; maxLoopTime = 0.0; MeasurementStart = currentTime; - avgLoopTime = avgLoopTimeSum / (double) avgLoopTimeTicker; avgLoopTimeSum = 0.0; avgLoopTimeTicker = 0; } @@ -59,6 +58,7 @@ public class MeasuringLoopTimes { avgLoopTimeSum += mainLoopTime; avgLoopTimeTicker++; + avgLoopTime = avgLoopTimeSum / (double) avgLoopTimeTicker; minLoopTime = Math.min(minLoopTime,mainLoopTime); maxLoopTime = Math.max(maxLoopTime,mainLoopTime); }