From ef1c7b0e6be4ed85cbf8bde50f77b807e75c770b Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Sat, 7 Feb 2026 19:02:41 -0600 Subject: [PATCH] implemented loop time efficiencies and added turret tracking to shooter test --- .../teamcode/autonomous/Auto_LT_Close.java | 58 +++++++-------- .../autonomous/Auto_LT_Close_12Ball.java | 4 +- .../autonomous/Auto_LT_Close_GateOpen.java | 4 +- .../ftc/teamcode/autonomous/Auto_LT_Far.java | 56 ++++++--------- .../ftc/teamcode/teleop/TeleopV3.java | 25 ++++--- .../ftc/teamcode/tests/LimelightTest.java | 2 +- .../ftc/teamcode/tests/ShooterTest.java | 53 +++++++++++++- .../teamcode/utils/MeasuringLoopTimes.java | 66 +++++++++++++++++ .../ftc/teamcode/utils/Servos.java | 49 +++++++++++-- .../ftc/teamcode/utils/Spindexer.java | 72 ++++++++++--------- .../ftc/teamcode/utils/Turret.java | 14 ++-- 11 files changed, 274 insertions(+), 129 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/autonomous/Auto_LT_Close.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java index d98acde..9f6f396 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java @@ -213,8 +213,7 @@ public class Auto_LT_Close extends LinearOpMode { stamp = System.currentTimeMillis(); } ticker++; - robot.transferServo.setPosition(transferServo_out); - + servos.setTransferPos(transferServo_out); drive.updatePoseEstimate(); teleStart = drive.localizer.getPose(); @@ -246,8 +245,7 @@ public class Auto_LT_Close extends LinearOpMode { spindexerWiggle *= -1.0; - robot.spin1.setPosition(spindexer_intakePos1 + spindexerWiggle); - robot.spin2.setPosition(1 - spindexer_intakePos1 - spindexerWiggle); + servos.setSpinPos(spindexer_intakePos1 + spindexerWiggle); spindexer.detectBalls(true, true); @@ -323,8 +321,7 @@ public class Auto_LT_Close extends LinearOpMode { // TELE.update(); robot.intake.setPower(-((System.currentTimeMillis() - stamp - colorSenseTime)) / 1000); - robot.spin1.setPosition(firstSpindexShootPos); - robot.spin2.setPosition(1 - firstSpindexShootPos); + servos.setSpinPos(firstSpindexShootPos); return true; } else { @@ -390,20 +387,18 @@ public class Auto_LT_Close extends LinearOpMode { if ((getRuntime() - stamp < shootTime && servos.getSpinPos() < spinEndPos) || shooterTicker == 0) { if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) { - robot.spin1.setPosition(autoSpinStartPos); - robot.spin2.setPosition(1 - autoSpinStartPos); + servos.setSpinPos(autoSpinStartPos); } else { - robot.transferServo.setPosition(transferServo_in); + servos.setTransferPos(transferServo_in); shooterTicker++; - double prevSpinPos = robot.spin1.getPosition(); - robot.spin1.setPosition(prevSpinPos + spindexSpeed); - robot.spin2.setPosition(1 - prevSpinPos - spindexSpeed); + double prevSpinPos = servos.getSpinCmdPos(); + servos.setSpinPos(prevSpinPos + spindexSpeed); } return true; } else { - robot.transferServo.setPosition(transferServo_out); + servos.setTransferPos(transferServo_out); spindexer.resetSpindexer(); spindexer.processIntake(); @@ -471,27 +466,24 @@ public class Auto_LT_Close extends LinearOpMode { if (getRuntime() - stamp < shootTime) { if (getRuntime() - stamp < firstShootTime) { - robot.transferServo.setPosition(transferServo_in); - robot.spin1.setPosition(firstSpindexShootPos); - robot.spin2.setPosition(1 - firstSpindexShootPos); + servos.setTransferPos(transferServo_out); + servos.setSpinPos(firstSpindexShootPos); } else { - robot.transferServo.setPosition(transferServo_in); + servos.setTransferPos(transferServo_in); shooterTicker++; - double prevSpinPos = robot.spin1.getPosition(); + double prevSpinPos = servos.getSpinCmdPos(); if (shootForward) { - robot.spin1.setPosition(prevSpinPos + spindexSpeed); - robot.spin2.setPosition(1 - prevSpinPos - spindexSpeed); + servos.setSpinPos(prevSpinPos + spindexSpeed); } else { - robot.spin1.setPosition(prevSpinPos - spindexSpeed); - robot.spin2.setPosition(1 - prevSpinPos + spindexSpeed); + servos.setSpinPos(prevSpinPos - spindexSpeed); } } return true; } else { - robot.transferServo.setPosition(transferServo_out); + servos.setTransferPos(transferServo_out); spindexer.resetSpindexer(); spindexer.processIntake(); @@ -563,8 +555,7 @@ public class Auto_LT_Close extends LinearOpMode { ticker++; motif = turret.detectObelisk(); - robot.turr1.setPosition(turrPos); - robot.turr2.setPosition(1 - turrPos); + turret.setTurret(turrPos); boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance; @@ -627,7 +618,7 @@ public class Auto_LT_Close extends LinearOpMode { double voltage = robot.voltage.getVoltage(); flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); flywheel.manageFlywheel(vel); - robot.hood.setPosition(hoodPos); + servos.setHoodPos(hoodPos); boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance; @@ -695,7 +686,7 @@ public class Auto_LT_Close extends LinearOpMode { turret.trackGoal(deltaPose); - robot.hood.setPosition(targetingSettings.hoodAngle); + servos.setHoodPos(targetingSettings.hoodAngle); double voltage = robot.voltage.getVoltage(); flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); @@ -766,7 +757,7 @@ public class Auto_LT_Close extends LinearOpMode { targetingSettings = targeting.calculateSettings (robotX, robotY, robotHeading, 0.0, false); - robot.hood.setPosition(targetingSettings.hoodAngle); + servos.setHoodPos(targetingSettings.hoodAngle); double voltage = robot.voltage.getVoltage(); flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); @@ -819,14 +810,13 @@ public class Auto_LT_Close extends LinearOpMode { turret = new Turret(robot, TELE, robot.limelight); - turret.manualSetTurret(turrDefault); + turret.setTurret(turrDefault); drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); - robot.spin1.setPosition(autoSpinStartPos); - robot.spin2.setPosition(1 - autoSpinStartPos); + servos.setSpinPos(autoSpinStartPos); - robot.transferServo.setPosition(transferServo_out); + servos.setTransferPos(transferServo_out); limelightUsed = false; @@ -834,8 +824,8 @@ public class Auto_LT_Close extends LinearOpMode { while (opModeInInit()) { - robot.hood.setPosition(shoot0Hood); - turret.manualSetTurret(turrDefault); + servos.setHoodPos(shoot0Hood); + turret.setTurret(turrDefault); if (gamepad2.crossWasPressed()) { redAlliance = !redAlliance; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball.java index ad11f86..b0fd2fa 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball.java @@ -169,7 +169,7 @@ public class Auto_LT_Close_12Ball extends LinearOpMode { robot.transferServo.setPosition(transferServo_out); - turret.manualSetTurret(turretShootPos); + turret.setTurret(turretShootPos); robot.intake.setPower(-((System.currentTimeMillis() - stamp)) / 1000); drive.updatePoseEstimate(); @@ -611,7 +611,7 @@ public class Auto_LT_Close_12Ball extends LinearOpMode { turret = new Turret(robot, TELE, robot.limelight); - turret.manualSetTurret(0.4); + turret.setTurret(0.4); drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_GateOpen.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_GateOpen.java index 86cd3b9..bd45591 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_GateOpen.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_GateOpen.java @@ -207,7 +207,7 @@ public class Auto_LT_Close_GateOpen extends LinearOpMode { ticker++; robot.transferServo.setPosition(transferServo_out); - turret.manualSetTurret(turretShootPos); + turret.setTurret(turretShootPos); drive.updatePoseEstimate(); @@ -752,7 +752,7 @@ public class Auto_LT_Close_GateOpen extends LinearOpMode { turret = new Turret(robot, TELE, robot.limelight); - turret.manualSetTurret(0.4); + turret.setTurret(0.4); drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java index 61dcfb8..8f1c859 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java @@ -97,8 +97,7 @@ public class Auto_LT_Far extends LinearOpMode { stamp = System.currentTimeMillis(); } ticker++; - robot.transferServo.setPosition(transferServo_out); - + servos.setTransferPos(transferServo_out); drive.updatePoseEstimate(); teleStart = drive.localizer.getPose(); @@ -130,8 +129,7 @@ public class Auto_LT_Far extends LinearOpMode { spindexerWiggle *= -1.0; - robot.spin1.setPosition(spindexer_intakePos1 + spindexerWiggle); - robot.spin2.setPosition(1 - spindexer_intakePos1 - spindexerWiggle); + servos.setSpinPos(spindexer_intakePos1 + spindexerWiggle); spindexer.detectBalls(true, true); @@ -207,8 +205,7 @@ public class Auto_LT_Far extends LinearOpMode { // TELE.update(); robot.intake.setPower(-((System.currentTimeMillis() - stamp - colorSenseTime)) / 1000); - robot.spin1.setPosition(firstSpindexShootPos); - robot.spin2.setPosition(1 - firstSpindexShootPos); + servos.setSpinPos(firstSpindexShootPos); return true; } else { @@ -274,20 +271,18 @@ public class Auto_LT_Far extends LinearOpMode { if ((getRuntime() - stamp < shootTime && servos.getSpinPos() < spinEndPos) || shooterTicker == 0) { if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) { - robot.spin1.setPosition(autoSpinStartPos); - robot.spin2.setPosition(1 - autoSpinStartPos); + servos.setSpinPos(autoSpinStartPos); } else { - robot.transferServo.setPosition(transferServo_in); + servos.setTransferPos(transferServo_in); shooterTicker++; - double prevSpinPos = robot.spin1.getPosition(); - robot.spin1.setPosition(prevSpinPos + spindexSpeed); - robot.spin2.setPosition(1 - prevSpinPos - spindexSpeed); + double prevSpinPos = servos.getSpinCmdPos(); + servos.setSpinPos(prevSpinPos + spindexSpeed); } return true; } else { - robot.transferServo.setPosition(transferServo_out); + servos.setTransferPos(transferServo_out); spindexer.resetSpindexer(); spindexer.processIntake(); @@ -355,27 +350,24 @@ public class Auto_LT_Far extends LinearOpMode { if (getRuntime() - stamp < shootTime) { if (getRuntime() - stamp < firstShootTime) { - robot.transferServo.setPosition(transferServo_in); - robot.spin1.setPosition(firstSpindexShootPos); - robot.spin2.setPosition(1 - firstSpindexShootPos); + servos.setTransferPos(transferServo_out); + servos.setSpinPos(firstSpindexShootPos); } else { - robot.transferServo.setPosition(transferServo_in); + servos.setTransferPos(transferServo_in); shooterTicker++; - double prevSpinPos = robot.spin1.getPosition(); + double prevSpinPos = servos.getSpinCmdPos(); if (shootForward) { - robot.spin1.setPosition(prevSpinPos + spindexSpeed); - robot.spin2.setPosition(1 - prevSpinPos - spindexSpeed); + servos.setSpinPos(prevSpinPos + spindexSpeed); } else { - robot.spin1.setPosition(prevSpinPos - spindexSpeed); - robot.spin2.setPosition(1 - prevSpinPos + spindexSpeed); + servos.setSpinPos(prevSpinPos - spindexSpeed); } } return true; } else { - robot.transferServo.setPosition(transferServo_out); + servos.setTransferPos(transferServo_out); spindexer.resetSpindexer(); spindexer.processIntake(); @@ -447,8 +439,7 @@ public class Auto_LT_Far extends LinearOpMode { ticker++; motif = turret.detectObelisk(); - robot.turr1.setPosition(turrPos); - robot.turr2.setPosition(1 - turrPos); + turret.setTurret(turrPos); boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance; @@ -511,7 +502,7 @@ public class Auto_LT_Far extends LinearOpMode { double voltage = robot.voltage.getVoltage(); flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); flywheel.manageFlywheel(vel); - robot.hood.setPosition(hoodPos); + servos.setHoodPos(hoodPos); boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance; @@ -579,7 +570,7 @@ public class Auto_LT_Far extends LinearOpMode { turret.trackGoal(deltaPose); - robot.hood.setPosition(targetingSettings.hoodAngle); + servos.setHoodPos(targetingSettings.hoodAngle); double voltage = robot.voltage.getVoltage(); flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); @@ -650,7 +641,7 @@ public class Auto_LT_Far extends LinearOpMode { targetingSettings = targeting.calculateSettings (robotX, robotY, robotHeading, 0.0, false); - robot.hood.setPosition(targetingSettings.hoodAngle); + servos.setHoodPos(targetingSettings.hoodAngle); double voltage = robot.voltage.getVoltage(); flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); @@ -701,14 +692,13 @@ public class Auto_LT_Far extends LinearOpMode { turret = new Turret(robot, TELE, robot.limelight); - turret.manualSetTurret(turrDefault); + turret.setTurret(turrDefault); drive = new MecanumDrive(hardwareMap, new Pose2d(0,0,0)); - robot.spin1.setPosition(autoSpinStartPos); - robot.spin2.setPosition(1 - autoSpinStartPos); + servos.setSpinPos(autoSpinStartPos); - robot.transferServo.setPosition(transferServo_out); + servos.setTransferPos(transferServo_out); robot.light.setPosition(1); @@ -721,7 +711,7 @@ public class Auto_LT_Far extends LinearOpMode { gamepad2.rumble(1000); } - turret.manualSetTurret(turretShootPos); + turret.setTurret(turretShootPos); robot.hood.setPosition(shoot0Hood); 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 c09cafd..ab30899 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; @@ -54,6 +55,7 @@ public class TeleopV3 extends LinearOpMode { Targeting targeting; Targeting.Settings targetingSettings; Drivetrain drivetrain; + MeasuringLoopTimes loopTimes; double autoHoodOffset = 0.0; int shooterTicker = 0; boolean intake = false; @@ -92,6 +94,9 @@ public class TeleopV3 extends LinearOpMode { drivetrain = new Drivetrain(robot, drive); + loopTimes = new MeasuringLoopTimes(); + loopTimes.init(); + PIDFController tController = new PIDFController(tp, ti, td, tf); tController.setTolerance(0.001); @@ -113,7 +118,7 @@ public class TeleopV3 extends LinearOpMode { waitForStart(); if (isStopRequested()) return; - robot.transferServo.setPosition(transferServo_out); + servo.setTransferPos(transferServo_out); while (opModeIsActive()) { @@ -133,7 +138,7 @@ public class TeleopV3 extends LinearOpMode { if (gamepad1.right_bumper) { shootAll = false; - robot.transferServo.setPosition(transferServo_out); + servo.setTransferPos(transferServo_out); } @@ -193,9 +198,9 @@ public class TeleopV3 extends LinearOpMode { //HOOD: if (targetingHood) { - robot.hood.setPosition(targetingSettings.hoodAngle + autoHoodOffset); + servo.setHoodPos(targetingSettings.hoodAngle + autoHoodOffset); } else { - robot.hood.setPosition(hoodDefaultPos); + servo.setHoodPos(hoodDefaultPos); } if (gamepad2.dpadUpWasPressed()) { @@ -265,10 +270,10 @@ public class TeleopV3 extends LinearOpMode { // 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(); @@ -280,7 +285,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; spindexer.resetSpindexer(); @@ -293,6 +298,7 @@ public class TeleopV3 extends LinearOpMode { for (LynxModule hub : allHubs) { hub.clearBulkCache(); } + loopTimes.loop(); // // TELE.addData("Spin1Green", green1 + ": " + ballIn(1)); // TELE.addData("Spin2Green", green2 + ": " + ballIn(2)); @@ -328,7 +334,10 @@ public class TeleopV3 extends LinearOpMode { // TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM); // TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle); // TELE.addData("timeSinceStamp", getRuntime() - shootStamp); - TELE.addData("Voltage", robot.voltage.getVoltage()); // returns alleged recorded voltage (not same as driver hub) + TELE.addData("Voltage", voltage); // returns alleged recorded voltage (not same as driver hub) + TELE.addData("Avg Loop Time", loopTimes.getAvgLoopTime()); + TELE.addData("Min Loop Time", loopTimes.getMinLoopTimeOneMin()); + TELE.addData("Max Loop Time", loopTimes.getMaxLoopTimeOneMin()); TELE.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/LimelightTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/LimelightTest.java index 6cbbbc4..3880604 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/LimelightTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/LimelightTest.java @@ -60,7 +60,7 @@ public class LimelightTest extends LinearOpMode { if (turretMode){ if (turretPos != 0.501){ - turret.manualSetTurret(turretPos); + turret.setTurret(turretPos); } } 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 63e85bc..dcb2d73 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 @@ -1,23 +1,30 @@ package org.firstinspires.ftc.teamcode.tests; +import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; import static org.firstinspires.ftc.teamcode.teleop.TeleopV3.spinSpeedIncrease; +import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.Pose2d; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotorEx; +import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; import org.firstinspires.ftc.teamcode.utils.Flywheel; import org.firstinspires.ftc.teamcode.utils.Robot; import org.firstinspires.ftc.teamcode.utils.Servos; import org.firstinspires.ftc.teamcode.utils.Spindexer; +import org.firstinspires.ftc.teamcode.utils.Targeting; +import org.firstinspires.ftc.teamcode.utils.Turret; @Config @TeleOp @@ -37,10 +44,12 @@ public class ShooterTest extends LinearOpMode { public static boolean shoot = false; public static boolean intake = false; + public static boolean turretTrack = true; Robot robot; Flywheel flywheel; Servos servo; - + MecanumDrive drive; + Turret turret; double shootStamp = 0.0; boolean shootAll = false; @@ -50,6 +59,7 @@ public class ShooterTest extends LinearOpMode { public double hoodAdjust = 0.0; public static double hoodAdjustFactor = 1.0; private int shooterTicker = 0; + public static double spinSpeed = 0.02; Spindexer spindexer ; @Override @@ -61,17 +71,51 @@ public class ShooterTest extends LinearOpMode { flywheel = new Flywheel(hardwareMap); spindexer = new Spindexer(hardwareMap); servo = new Servos(hardwareMap); + drive = new MecanumDrive(hardwareMap, new Pose2d(0,0,0)); MultipleTelemetry TELE = new MultipleTelemetry( telemetry, FtcDashboard.getInstance().getTelemetry() ); + turret = new Turret(robot, TELE, robot.limelight); + Turret.limelightUsed = true; waitForStart(); + robot.limelight.start(); + if (isStopRequested()) return; while (opModeIsActive()) { + if (redAlliance){ + robot.limelight.pipelineSwitch(4); + } else { + robot.limelight.pipelineSwitch(2); + } + + //TURRET TRACKING + drive.updatePoseEstimate(); + + double robX = drive.localizer.getPose().position.x; + double robY = drive.localizer.getPose().position.y; + + double robotHeading = drive.localizer.getPose().heading.toDouble(); + + double goalX = -15; + double goalY = 0; + + double dx = robX - goalX; // delta x from robot to goal + double dy = robY - goalY; // delta y from robot to goal + Pose2d deltaPose = new Pose2d(dx, dy, robotHeading); + + double distanceToGoal = Math.sqrt(dx * dx + dy * dy); + + if (turretTrack){ + turret.trackGoal(deltaPose); + } else if (turretPos != 0.501){ + turret.setTurret(turretPos); + } + double voltage = robot.voltage.getVoltage(); if (mode == 0) { @@ -120,8 +164,11 @@ public class ShooterTest extends LinearOpMode { robot.transferServo.setPosition(transferServo_in); shooterTicker++; double prevSpinPos = robot.spin1.getPosition(); - robot.spin1.setPosition(prevSpinPos + spinSpeedIncrease); - robot.spin2.setPosition(1 - prevSpinPos - spinSpeedIncrease); + + if (prevSpinPos < 0.9){ + robot.spin1.setPosition(prevSpinPos + spinSpeed); + robot.spin2.setPosition(1 - prevSpinPos - spinSpeed); + } } 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..a8a2a00 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,15 @@ 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 prevHoodPos = 0.0; + private boolean firstHoodPos = true; + public Servos(HardwareMap hardwareMap) { robot = new Robot(hardwareMap); spinPID = new PIDFController(spinP, spinI, spinD, spinF); @@ -32,8 +41,42 @@ 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; + } + + public double setHoodPos(double pos){ + if (firstHoodPos || !servoPosEqual(pos, prevHoodPos)) { + robot.hood.setPosition(pos); + firstHoodPos = false; + } + + prevHoodPos = pos; return pos; } @@ -48,8 +91,4 @@ public class Servos { public double setTurrPos(double pos) { return 1.0; } - - public boolean turretEqual(double pos) { - return true; - } } 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..af42039 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) { @@ -196,7 +196,7 @@ public class Spindexer { } else { ballPositions[commandedIntakePosition].ballColor = BallColor.PURPLE; // purple } - } + } } // Position 2 // Find which ball position this is in the spindexer @@ -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,13 +276,14 @@ public class Spindexer { // robot.spin2.setPosition(1 - servos.getSpinPos() + 0.05); // } // } -// prevPos = currentPos; +// prevPos = pos; } public void stopSpindexer() { } + private double prevLight = 0.0; public void ballCounterLight(){ int counter = 0; if (!ballPositions[0].isEmpty){ @@ -299,15 +295,21 @@ public class Spindexer { if (!ballPositions[2].isEmpty){ counter++; } + + double light; if (counter == 3){ - robot.light.setPosition(Light3); + light = Light3; } else if (counter == 2){ - robot.light.setPosition(Light2); + light = Light2; } else if (counter == 1){ - robot.light.setPosition(Light1); + light = Light1; } else { - robot.light.setPosition(Light0); + light = Light0; } + if (light != prevLight){ + robot.light.setPosition(light); + } + prevLight = light; } public boolean slotIsEmpty(int slot){ @@ -325,7 +327,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 +338,7 @@ public class Spindexer { unknownColorDetect = 0; } else { // Keep moving the spindexer - moveSpindexerToPos(intakePositions[commandedIntakePosition]); + servos.setSpinPos(intakePositions[commandedIntakePosition]); } break; case UNKNOWN_DETECT: @@ -355,7 +357,7 @@ public class Spindexer { } else { // Maintain Position spindexerWiggle *= -1.0; - moveSpindexerToPos(intakePositions[commandedIntakePosition]+spindexerWiggle); + servos.setSpinPos(intakePositions[commandedIntakePosition]+spindexerWiggle); } break; case FINDNEXT: @@ -387,7 +389,7 @@ public class Spindexer { //commandedIntakePosition = bestFitMotif(); currentIntakeState = Spindexer.IntakeState.FULL; } - moveSpindexerToPos(intakePositions[commandedIntakePosition]); + servos.setSpinPos(intakePositions[commandedIntakePosition]); break; case MOVING: @@ -403,7 +405,7 @@ public class Spindexer { //detectBalls(false, false); } else { // Keep moving the spindexer - moveSpindexerToPos(intakePositions[commandedIntakePosition]); + servos.setSpinPos(intakePositions[commandedIntakePosition]); } break; @@ -416,7 +418,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 +427,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 +439,7 @@ public class Spindexer { currentIntakeState = Spindexer.IntakeState.SHOOTNEXT; } // Maintain Position - moveSpindexerToPos(outakePositions[commandedIntakePosition]); + servos.setSpinPos(outakePositions[commandedIntakePosition]); break; case SHOOTNEXT: @@ -458,7 +460,7 @@ public class Spindexer { // Empty return to intake state currentIntakeState = IntakeState.FINDNEXT; } - moveSpindexerToPos(outakePositions[commandedIntakePosition]); + servos.setSpinPos(outakePositions[commandedIntakePosition]); break; case SHOOTMOVING: @@ -467,7 +469,7 @@ public class Spindexer { currentIntakeState = Spindexer.IntakeState.SHOOTWAIT; } else { // Keep moving the spindexer - moveSpindexerToPos(outakePositions[commandedIntakePosition]); + servos.setSpinPos(outakePositions[commandedIntakePosition]); } break; @@ -492,14 +494,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 +512,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; @@ -565,7 +567,7 @@ public class Spindexer { //break; case NONE: return 0; - //break; + //break; } return 0; } 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..8ded7eb 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 @@ -71,10 +71,13 @@ public class Turret { return turrPosScalar * (robot.turr1Pos.getVoltage() / 3.3) + turrDefault; } - - public void manualSetTurret(double pos) { - robot.turr1.setPosition(pos); - robot.turr2.setPosition(1 - pos); + private double prevTurrPos = 0.501; + public void setTurret(double pos) { + if (prevTurrPos != 0.501 && prevTurrPos != pos){ + robot.turr1.setPosition(pos); + robot.turr2.setPosition(1-pos); + } + prevTurrPos = pos; } public boolean turretEqual(double pos) { @@ -272,8 +275,7 @@ public class Turret { } // Set servo positions - robot.turr1.setPosition(turretPos + manualOffset); - robot.turr2.setPosition(1.0 - turretPos - manualOffset); + setTurret(turretPos + manualOffset); /* ---------------- TELEMETRY ---------------- */