From 90dfa2e49375bfa43b075d8d31c72da4f04c6481 Mon Sep 17 00:00:00 2001 From: Titan Robotics Club Date: Wed, 25 Dec 2024 02:27:00 -0800 Subject: [PATCH] Sync'd latest changes. --- TeamCode/src/main/java/teamcode/FtcAuto.java | 12 +-- .../src/main/java/teamcode/FtcTeleOp.java | 80 ++++++++++++------- TeamCode/src/main/java/teamcode/Robot.java | 19 ++--- .../src/main/java/teamcode/RobotParams.java | 2 +- .../main/java/teamcode/subsystems/Vision.java | 15 ++-- 5 files changed, 78 insertions(+), 50 deletions(-) diff --git a/TeamCode/src/main/java/teamcode/FtcAuto.java b/TeamCode/src/main/java/teamcode/FtcAuto.java index f48e6ef..084fc7a 100644 --- a/TeamCode/src/main/java/teamcode/FtcAuto.java +++ b/TeamCode/src/main/java/teamcode/FtcAuto.java @@ -162,10 +162,10 @@ public class FtcAuto extends FtcOpMode break; } - if (robot.vision != null) - { - // Enabling vision early so we can detect target before match starts if necessary. - // Only enable the necessary vision for that purpose. +// if (robot.vision != null) +// { +// // Enabling vision early so we can detect target before match starts if necessary. +// // Only enable the necessary vision for that purpose. // if (robot.vision.aprilTagVision != null) // { // robot.globalTracer.traceInfo(moduleName, "Enabling AprilTagVision."); @@ -183,7 +183,7 @@ public class FtcAuto extends FtcOpMode // robot.globalTracer.traceInfo(moduleName, "Enabling BlueBlobVision."); // robot.vision.setBlueBlobVisionEnabled(true); // } - } +// } } //robotInit // @@ -268,9 +268,9 @@ public class FtcAuto extends FtcOpMode robot.battery.setEnabled(false); } - printPerformanceMetrics(); robot.globalTracer.traceInfo( moduleName, "***** Stopping autonomous: " + TrcTimer.getCurrentTimeString() + " *****"); + printPerformanceMetrics(); if (TrcDbgTrace.isTraceLogOpened()) { diff --git a/TeamCode/src/main/java/teamcode/FtcTeleOp.java b/TeamCode/src/main/java/teamcode/FtcTeleOp.java index efe4b01..1954317 100644 --- a/TeamCode/src/main/java/teamcode/FtcTeleOp.java +++ b/TeamCode/src/main/java/teamcode/FtcTeleOp.java @@ -54,6 +54,7 @@ public class FtcTeleOp extends FtcOpMode private boolean statusUpdateOn = false; private boolean relocalizing = false; private TrcPose2D robotFieldPose = null; + private Integer savedLimelightPipeline = null; // // Implements FtcOpMode abstract method. @@ -136,16 +137,16 @@ public class FtcTeleOp extends FtcOpMode // if (robot.vision != null) { - if (robot.vision.limelightVision != null) - { - robot.globalTracer.traceInfo(moduleName, "Enabling Limelight AprilTagVision."); - robot.vision.setLimelightVisionEnabled(0, true); - } - else if (robot.vision.aprilTagVision != null) + if (robot.vision.aprilTagVision != null) { robot.globalTracer.traceInfo(moduleName, "Enabling WebCam AprilTagVision."); robot.vision.setAprilTagVisionEnabled(true); } + else if (robot.vision.limelightVision != null) + { + robot.globalTracer.traceInfo(moduleName, "Enabling Limelight AprilTagVision."); + robot.vision.setLimelightVisionEnabled(0, true); + } } } //startMode @@ -163,9 +164,9 @@ public class FtcTeleOp extends FtcOpMode // Tell robot object opmode is about to stop so it can do the necessary cleanup for the mode. // robot.stopMode(prevMode); - printPerformanceMetrics(); robot.globalTracer.traceInfo( moduleName, "***** Stopping TeleOp: " + TrcTimer.getCurrentTimeString() + " *****"); + printPerformanceMetrics(); if (TrcDbgTrace.isTraceLogOpened()) { @@ -187,6 +188,7 @@ public class FtcTeleOp extends FtcOpMode { if (slowPeriodicLoop) { + int lineNum = 1; // // DriveBase subsystem. // @@ -214,9 +216,13 @@ public class FtcTeleOp extends FtcOpMode { robot.robotDrive.driveBase.arcadeDrive(inputs[1], inputs[2]); } - robot.dashboard.displayPrintf( - 1, "RobotDrive: Power=(%.2f,y=%.2f,rot=%.2f),Mode:%s", - inputs[0], inputs[1], inputs[2], robot.robotDrive.driveBase.getDriveOrientation()); + + if (RobotParams.Preferences.doStatusUpdate || statusUpdateOn) + { + robot.dashboard.displayPrintf( + lineNum++, "RobotDrive: Power=(%.2f,y=%.2f,rot=%.2f),Mode:%s", + inputs[0], inputs[1], inputs[2], robot.robotDrive.driveBase.getDriveOrientation()); + } } // Check for EndGame warning. if (elapsedTime > RobotParams.Game.ENDGAME_DEADLINE) @@ -242,7 +248,7 @@ public class FtcTeleOp extends FtcOpMode // Display subsystem status. if (RobotParams.Preferences.doStatusUpdate || statusUpdateOn) { - robot.updateStatus(2); + robot.updateStatus(lineNum); } } } //periodic @@ -370,7 +376,7 @@ public class FtcTeleOp extends FtcOpMode { robot.globalTracer.traceInfo(moduleName, ">>>>> ZeroCalibrating."); robot.cancelAll(); - robot.zeroCalibrate(); + robot.zeroCalibrate(moduleName, null); if (robot.robotDrive != null && robot.robotDrive instanceof FtcSwerveDrive) { // Drive base is a Swerve Drive, align all steering wheels forward. @@ -381,27 +387,47 @@ public class FtcTeleOp extends FtcOpMode break; case Start: - if (robot.vision != null && - (robot.vision.isLimelightVisionEnabled() || robot.vision.isAprilTagVisionEnabled()) && - robot.robotDrive != null) + if (robot.vision != null && robot.robotDrive != null) { - // On press of the button, we will start looking for AprilTag for re-localization. - // On release of the button, we will set the robot's field location if we found the AprilTag. - relocalizing = pressed; - if (!pressed) + boolean hasAprilTagVision = robot.vision.isAprilTagVisionEnabled(); + + if (!hasAprilTagVision && robot.vision.limelightVision != null) { - if (robotFieldPose != null) + hasAprilTagVision = true; + if (pressed) { - // Vision found an AprilTag, set the new robot field location. - robot.globalTracer.traceInfo( - moduleName, ">>>>> Finish re-localizing: pose=" + robotFieldPose); - robot.robotDrive.driveBase.setFieldPosition(robotFieldPose, false); - robotFieldPose = null; + // Webcam AprilTag vision is not enable, enable Limelight AprilTag pipeline instead. + savedLimelightPipeline = robot.vision.limelightVision.getPipeline(); + robot.vision.setLimelightVisionEnabled(0, true); } } - else + + if (hasAprilTagVision) { - robot.globalTracer.traceInfo(moduleName, ">>>>> Start re-localizing ..."); + // On press of the button, we will start looking for AprilTag for re-localization. + // On release of the button, we will set the robot's field location if we found the AprilTag. + relocalizing = pressed; + if (!pressed) + { + if (robotFieldPose != null) + { + // Vision found an AprilTag, set the new robot field location. + robot.globalTracer.traceInfo( + moduleName, ">>>>> Finish re-localizing: pose=" + robotFieldPose); + robot.robotDrive.driveBase.setFieldPosition(robotFieldPose, false); + robotFieldPose = null; + if (savedLimelightPipeline != null) + { + // Done with AprilTag re-localization, restore previous Limelight pipeline. + robot.vision.limelightVision.setPipeline(savedLimelightPipeline); + savedLimelightPipeline = null; + } + } + } + else + { + robot.globalTracer.traceInfo(moduleName, ">>>>> Start re-localizing ..."); + } } } break; diff --git a/TeamCode/src/main/java/teamcode/Robot.java b/TeamCode/src/main/java/teamcode/Robot.java index b966a7c..69f654e 100644 --- a/TeamCode/src/main/java/teamcode/Robot.java +++ b/TeamCode/src/main/java/teamcode/Robot.java @@ -89,7 +89,7 @@ public class Robot // Create and initialize vision subsystems. if (RobotParams.Preferences.useVision && (RobotParams.Preferences.tuneColorBlobVision || - RobotParams.Preferences.useAprilTagVision || + RobotParams.Preferences.useWebcamAprilTagVision || RobotParams.Preferences.useColorBlobVision || RobotParams.Preferences.useLimelightVision)) { @@ -115,8 +115,13 @@ public class Robot if (RobotParams.Preferences.useSubsystems) { // Create subsystems. - // Zero calibrate all subsystems only at init time. - zeroCalibrate(); + + // Zero calibrate all subsystems only in Auto or if TeleOp is run standalone without prior Auto. + // There is no reason to zero calibrate again if Auto was run right before TeleOp. + if (runMode == TrcRobot.RunMode.AUTO_MODE || FtcAuto.autoChoices.alliance == null) + { + zeroCalibrate(null, null); + } // Create autotasks. } } @@ -312,14 +317,6 @@ public class Robot { } //zeroCalibrate - /** - * This method zero calibrates all subsystems. - */ - public void zeroCalibrate() - { - zeroCalibrate(null, null); - } //zeroCalibrate - /** * This method sets the robot's starting position according to the autonomous choices. * diff --git a/TeamCode/src/main/java/teamcode/RobotParams.java b/TeamCode/src/main/java/teamcode/RobotParams.java index 2937473..7e7f049 100644 --- a/TeamCode/src/main/java/teamcode/RobotParams.java +++ b/TeamCode/src/main/java/teamcode/RobotParams.java @@ -62,7 +62,7 @@ public class RobotParams public static final boolean tuneColorBlobVision = false; public static final boolean useLimelightVision = false; public static final boolean useCameraStreamProcessor = false; - public static final boolean useAprilTagVision = false; + public static final boolean useWebcamAprilTagVision = false; public static final boolean useColorBlobVision = false; public static final boolean showVisionView = !inCompetition; public static final boolean showVisionStat = false; diff --git a/TeamCode/src/main/java/teamcode/subsystems/Vision.java b/TeamCode/src/main/java/teamcode/subsystems/Vision.java index 77f4d8e..894a757 100644 --- a/TeamCode/src/main/java/teamcode/subsystems/Vision.java +++ b/TeamCode/src/main/java/teamcode/subsystems/Vision.java @@ -273,7 +273,7 @@ public class Vision // FtcDashboard.getInstance().startCameraStream(cameraStreamProcessor, 0); } - if (RobotParams.Preferences.useAprilTagVision) + if (RobotParams.Preferences.useWebcamAprilTagVision) { tracer.traceInfo(moduleName, "Starting Webcam AprilTagVision..."); FtcVisionAprilTag.Parameters aprilTagParams = new FtcVisionAprilTag.Parameters() @@ -292,14 +292,14 @@ public class Vision tracer.traceInfo(moduleName, "Starting Webcam ColorBlobVision..."); redBlobVision = new FtcVisionEocvColorBlob( - "RedBlob", colorConversion, redBlobColorThresholds, colorBlobFilterContourParams, true, - robot.robotInfo.webCam1.cameraRect, robot.robotInfo.webCam1.worldRect, true); + LEDIndicator.RED_BLOB, colorConversion, redBlobColorThresholds, colorBlobFilterContourParams, + true, robot.robotInfo.webCam1.cameraRect, robot.robotInfo.webCam1.worldRect, true); redBlobProcessor = redBlobVision.getVisionProcessor(); visionProcessorsList.add(redBlobProcessor); blueBlobVision = new FtcVisionEocvColorBlob( - "BlueBlob", colorConversion, blueBlobColorThresholds, colorBlobFilterContourParams, true, - robot.robotInfo.webCam1.cameraRect, robot.robotInfo.webCam1.worldRect, true); + LEDIndicator.BLUE_BLOB, colorConversion, blueBlobColorThresholds, colorBlobFilterContourParams, + true, robot.robotInfo.webCam1.cameraRect, robot.robotInfo.webCam1.worldRect, true); blueBlobProcessor = blueBlobVision.getVisionProcessor(); visionProcessorsList.add(blueBlobProcessor); } @@ -487,6 +487,11 @@ public class Vision colorBlobInfo.detectedObj.label, colorBlobInfo.detectedObj.getRotatedRectVertices()); } + if (colorBlobInfo != null && robot.ledIndicator != null) + { + robot.ledIndicator.setDetectedPattern(colorBlobInfo.detectedObj.label); + } + if (lineNum != -1) { robot.dashboard.displayPrintf(