diff --git a/TeamCode/src/main/java/ftclib b/TeamCode/src/main/java/ftclib index 2c52995..fbd6673 160000 --- a/TeamCode/src/main/java/ftclib +++ b/TeamCode/src/main/java/ftclib @@ -1 +1 @@ -Subproject commit 2c529954d6224dc55586b437b37d4ccf738184bd +Subproject commit fbd66737d87286ad0c32b1e1b5fddd8b4a5fc5b5 diff --git a/TeamCode/src/main/java/teamcode/FtcAuto.java b/TeamCode/src/main/java/teamcode/FtcAuto.java index 99a66d9..f48e6ef 100644 --- a/TeamCode/src/main/java/teamcode/FtcAuto.java +++ b/TeamCode/src/main/java/teamcode/FtcAuto.java @@ -43,7 +43,7 @@ import trclib.timer.TrcTimer; /** * This class contains the Autonomous Mode program. */ -@Autonomous(name="FtcAutonomous", group="Ftcxxxx") +@Autonomous(name="FtcAutonomous", group="FtcTeam") public class FtcAuto extends FtcOpMode { private final String moduleName = getClass().getSimpleName(); @@ -73,9 +73,9 @@ public class FtcAuto extends FtcOpMode public static class AutoChoices { public double delay = 0.0; - public Alliance alliance = Alliance.RED_ALLIANCE; - public StartPos startPos = StartPos.LEFT; - public AutoStrategy strategy = AutoStrategy.DO_NOTHING; + public Alliance alliance = null; + public StartPos startPos = null; + public AutoStrategy strategy = null; public double xTarget = 0.0; public double yTarget = 0.0; public double turnTarget = 0.0; @@ -192,7 +192,8 @@ public class FtcAuto extends FtcOpMode /** * This method is called periodically after robotInit() is called but before competition starts. For example, - * we can put vision code here to detect target before autonomous starts. + * we can put vision code here to detect target before autonomous starts, or code to move the subsystems to a + * particular configuration to be within the 18-inch starting restriction. */ @Override public void initPeriodic() diff --git a/TeamCode/src/main/java/teamcode/FtcTeleOp.java b/TeamCode/src/main/java/teamcode/FtcTeleOp.java index 5394299..efe4b01 100644 --- a/TeamCode/src/main/java/teamcode/FtcTeleOp.java +++ b/TeamCode/src/main/java/teamcode/FtcTeleOp.java @@ -29,6 +29,7 @@ import java.util.Locale; import ftclib.drivebase.FtcSwerveDrive; import ftclib.driverio.FtcGamepad; import ftclib.robotcore.FtcOpMode; +import teamcode.subsystems.RumbleIndicator; import trclib.drivebase.TrcDriveBase; import trclib.pathdrive.TrcPose2D; import trclib.robotcore.TrcDbgTrace; @@ -38,7 +39,7 @@ import trclib.timer.TrcTimer; /** * This class contains the TeleOp Mode program. */ -@TeleOp(name="FtcTeleOp", group="Ftcxxxx") +@TeleOp(name="FtcTeleOp", group="FtcTeam") public class FtcTeleOp extends FtcOpMode { private final String moduleName = getClass().getSimpleName(); @@ -50,6 +51,7 @@ public class FtcTeleOp extends FtcOpMode private double turnPowerScale; private boolean driverAltFunc = false; private boolean operatorAltFunc = false; + private boolean statusUpdateOn = false; private boolean relocalizing = false; private TrcPose2D robotFieldPose = null; @@ -85,11 +87,19 @@ public class FtcTeleOp extends FtcOpMode driverGamepad = new FtcGamepad("DriverGamepad", gamepad1); driverGamepad.setButtonEventHandler(this::driverButtonEvent); driverGamepad.setLeftStickInverted(false, true); + driverGamepad.setRightStickInverted(false, true); operatorGamepad = new FtcGamepad("OperatorGamepad", gamepad2); operatorGamepad.setButtonEventHandler(this::operatorButtonEvent); + operatorGamepad.setLeftStickInverted(false, true); operatorGamepad.setRightStickInverted(false, true); + if (RobotParams.Preferences.useRumble) + { + robot.driverRumble = new RumbleIndicator("DriverRumble", driverGamepad); + robot.operatorRumble = new RumbleIndicator("OperatorRumble", operatorGamepad); + } + drivePowerScale = RobotParams.Robot.DRIVE_NORMAL_SCALE; turnPowerScale = RobotParams.Robot.TURN_NORMAL_SCALE; setDriveOrientation(RobotParams.Robot.DRIVE_ORIENTATION); @@ -208,6 +218,19 @@ public class FtcTeleOp extends FtcOpMode 1, "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) + { + if (robot.driverRumble != null) + { + robot.driverRumble.setRumblePattern(RumbleIndicator.ENDGAME_DEADLINE); + } + + if (robot.operatorRumble != null) + { + robot.operatorRumble.setRumblePattern(RumbleIndicator.ENDGAME_DEADLINE); + } + } } // // Other subsystems. @@ -217,7 +240,7 @@ public class FtcTeleOp extends FtcOpMode // Analog control of subsystems. } // Display subsystem status. - if (RobotParams.Preferences.doStatusUpdate) + if (RobotParams.Preferences.doStatusUpdate || statusUpdateOn) { robot.updateStatus(2); } @@ -294,7 +317,14 @@ public class FtcTeleOp extends FtcOpMode case B: case X: + break; + case Y: + if (driverAltFunc && pressed) + { + robot.globalTracer.traceInfo(moduleName, ">>>>> Cancel all."); + robot.cancelAll(); + } break; case LeftBumper: @@ -303,18 +333,29 @@ public class FtcTeleOp extends FtcOpMode break; case RightBumper: - // Press and hold for slow drive. - if (pressed) + if (driverAltFunc) { - robot.globalTracer.traceInfo(moduleName, ">>>>> DrivePower slow."); - drivePowerScale = RobotParams.Robot.DRIVE_SLOW_SCALE; - turnPowerScale = RobotParams.Robot.TURN_SLOW_SCALE; + if (!RobotParams.Preferences.doStatusUpdate) + { + // Toggle status update ON/OFF. + statusUpdateOn = !statusUpdateOn; + } } else { - robot.globalTracer.traceInfo(moduleName, ">>>>> DrivePower normal."); - drivePowerScale = RobotParams.Robot.DRIVE_NORMAL_SCALE; - turnPowerScale = RobotParams.Robot.TURN_NORMAL_SCALE; + // Press and hold for slow drive. + if (pressed) + { + robot.globalTracer.traceInfo(moduleName, ">>>>> DrivePower slow."); + drivePowerScale = RobotParams.Robot.DRIVE_SLOW_SCALE; + turnPowerScale = RobotParams.Robot.TURN_SLOW_SCALE; + } + else + { + robot.globalTracer.traceInfo(moduleName, ">>>>> DrivePower normal."); + drivePowerScale = RobotParams.Robot.DRIVE_NORMAL_SCALE; + turnPowerScale = RobotParams.Robot.TURN_NORMAL_SCALE; + } } break; @@ -401,9 +442,9 @@ public class FtcTeleOp extends FtcOpMode if (pressed) { // Zero calibrate all subsystems (arm, elevator and turret). - robot.globalTracer.traceInfo(moduleName, ">>>>> ZeroCalibrate pressed."); + robot.globalTracer.traceInfo(moduleName, ">>>>> ZeroCalibrating."); robot.cancelAll(); - robot.zeroCalibrate(moduleName); + robot.zeroCalibrate(moduleName, null); } break; diff --git a/TeamCode/src/main/java/teamcode/FtcTest.java b/TeamCode/src/main/java/teamcode/FtcTest.java index 4ec75c9..8728e0d 100644 --- a/TeamCode/src/main/java/teamcode/FtcTest.java +++ b/TeamCode/src/main/java/teamcode/FtcTest.java @@ -54,7 +54,7 @@ import trclib.timer.TrcTimer; * testing purposes. It provides numerous tests for diagnosing problems with the robot. It also provides tools * for tuning and calibration. */ -@TeleOp(name="FtcTest", group="Ftcxxxx") +@TeleOp(name="FtcTest", group="FtcTeam") public class FtcTest extends FtcTeleOp { private final String moduleName = getClass().getSimpleName(); @@ -232,10 +232,16 @@ public class FtcTest extends FtcTeleOp // Vision generally will impact performance, so we only enable it if it's needed. if (robot.vision.aprilTagVision != null) { - robot.globalTracer.traceInfo(moduleName, "Enabling AprilTagVision."); + robot.globalTracer.traceInfo(moduleName, "Enabling AprilTagVision for Webcam."); robot.vision.setAprilTagVisionEnabled(true); } + if (robot.vision.limelightVision != null) + { + robot.globalTracer.traceInfo(moduleName, "Enabling AprilTagVision for Limelight."); + robot.vision.setLimelightVisionEnabled(0, true); + } + if (robot.vision.redBlobVision != null) { robot.globalTracer.traceInfo(moduleName, "Enabling RedBlobVision."); @@ -247,12 +253,6 @@ public class FtcTest extends FtcTeleOp robot.globalTracer.traceInfo(moduleName, "Enabling BlueBlobVision."); robot.vision.setColorBlobVisionEnabled(Vision.ColorBlobType.BlueBlob, true); } - - if (robot.vision.limelightVision != null) - { - robot.globalTracer.traceInfo(moduleName, "Enabling LimelightVision."); - robot.vision.setLimelightVisionEnabled(0, true); - } } break; @@ -436,12 +436,12 @@ public class FtcTest extends FtcTeleOp { case SENSORS_TEST: case SUBSYSTEMS_TEST: - doSensorsTest(); + doSensorsTest(lineNum); break; case VISION_TEST: case TUNE_COLORBLOB_VISION: - doVisionTest(); + doVisionTest(lineNum); break; case X_TIMED_DRIVE: @@ -617,6 +617,17 @@ public class FtcTest extends FtcTeleOp } passToTeleOp = false; } + else if (testChoices.test == Test.VISION_TEST && + robot.vision != null && robot.vision.isLimelightVisionEnabled()) + { + if (pressed) + { + int pipelineIndex = (robot.vision.limelightVision.getPipeline() + 1) % + Vision.LimelightParams.NUM_PIPELINES; + robot.vision.limelightVision.setPipeline(pipelineIndex); + robot.globalTracer.traceInfo(moduleName, "Switch Limelight pipeline to " + pipelineIndex); + } + } break; case LeftBumper: @@ -1015,10 +1026,11 @@ public class FtcTest extends FtcTeleOp * This method reads all sensors and prints out their values. This is a very useful diagnostic tool to check * if all sensors are working properly. For encoders, since test sensor mode is also teleop mode, you can * operate the gamepads to turn the motors and check the corresponding encoder counts. + * + * @param lineNum specifies the starting line number on the dashboard to display sensor states. */ - private void doSensorsTest() + private void doSensorsTest(int lineNum) { - int lineNum = 9; // // Read all sensors and display on the dashboard. // Drive the robot around to sample different locations of the field. @@ -1062,17 +1074,19 @@ public class FtcTest extends FtcTeleOp /** * This method calls vision code to detect target objects and display their info. + * + * @param lineNum specifies the starting line number on the dashboard to display vision info. */ - private void doVisionTest() + private void doVisionTest(int lineNum) { if (robot.vision != null) { - int lineNum = 9; - - if (robot.vision.vision != null) + if (robot.vision.limelightVision != null) { - // displayExposureSettings is only available for VisionPortal. - robot.vision.displayExposureSettings(lineNum++); + robot.vision.getLimelightDetectedObject( + robot.vision.limelightVision.getPipeline() == 0? + FtcLimelightVision.ResultType.Fiducial: FtcLimelightVision.ResultType.Python, + null, lineNum++); } if (robot.vision.rawColorBlobVision != null) @@ -1087,17 +1101,18 @@ public class FtcTest extends FtcTeleOp if (robot.vision.redBlobVision != null) { - robot.vision.getDetectedColorBlob(Vision.ColorBlobType.RedBlob, lineNum++); + robot.vision.getDetectedColorBlob(Vision.ColorBlobType.RedBlob, 0.0, lineNum++); } if (robot.vision.blueBlobVision != null) { - robot.vision.getDetectedColorBlob(Vision.ColorBlobType.BlueBlob, lineNum++); + robot.vision.getDetectedColorBlob(Vision.ColorBlobType.BlueBlob, 0.0, lineNum++); } - if (robot.vision.limelightVision != null) + if (robot.vision.vision != null) { - robot.vision.getLimelightDetectedObject(FtcLimelightVision.ResultType.Fiducial, null, lineNum++); + // displayExposureSettings is only available for VisionPortal. + robot.vision.displayExposureSettings(lineNum++); } } } //doVisionTest diff --git a/TeamCode/src/main/java/teamcode/Robot.java b/TeamCode/src/main/java/teamcode/Robot.java index 7026c16..b966a7c 100644 --- a/TeamCode/src/main/java/teamcode/Robot.java +++ b/TeamCode/src/main/java/teamcode/Robot.java @@ -31,11 +31,13 @@ import ftclib.robotcore.FtcOpMode; import ftclib.sensor.FtcRobotBattery; import teamcode.subsystems.LEDIndicator; import teamcode.subsystems.RobotBase; +import teamcode.subsystems.RumbleIndicator; import teamcode.subsystems.Vision; import trclib.motor.TrcMotor; import trclib.motor.TrcServo; import trclib.pathdrive.TrcPose2D; import trclib.robotcore.TrcDbgTrace; +import trclib.robotcore.TrcEvent; import trclib.robotcore.TrcRobot; import trclib.sensor.TrcDigitalInput; import trclib.timer.TrcTimer; @@ -60,6 +62,8 @@ public class Robot public Vision vision; // Sensors and indicators. public LEDIndicator ledIndicator; + public RumbleIndicator driverRumble; + public RumbleIndicator operatorRumble; public FtcRobotBattery battery; // Subsystems. // Autotasks. @@ -201,15 +205,21 @@ public class Robot if (vision != null) { vision.setCameraStreamEnabled(false); - if (vision.rawColorBlobVision != null) + if (vision.isRawColorBlobVisionEnabled()) { globalTracer.traceInfo(moduleName, "Disabling RawColorBlobVision."); vision.setRawColorBlobVisionEnabled(false); } - if (vision.aprilTagVision != null) + if (vision.isLimelightVisionEnabled()) { - globalTracer.traceInfo(moduleName, "Disabling AprilTagVision."); + globalTracer.traceInfo(moduleName, "Disabling LimelightVision."); + vision.setLimelightVisionEnabled(0, false); + } + + if (vision.isAprilTagVisionEnabled()) + { + globalTracer.traceInfo(moduleName, "Disabling Webcam AprilTagVision."); vision.setAprilTagVisionEnabled(false); } @@ -296,8 +306,9 @@ public class Robot * This method zero calibrates all subsystems. * * @param owner specifies the owner ID to check if the caller has ownership of the motor. + * @param event specifies the event to signal when the zero calibration is done. */ - public void zeroCalibrate(String owner) + public void zeroCalibrate(String owner, TrcEvent event) { } //zeroCalibrate @@ -306,7 +317,7 @@ public class Robot */ public void zeroCalibrate() { - zeroCalibrate(null); + zeroCalibrate(null, null); } //zeroCalibrate /** @@ -372,7 +383,7 @@ public class Robot */ public TrcPose2D adjustPoseByAlliance(double x, double y, double heading, FtcAuto.Alliance alliance) { - return adjustPoseByAlliance(x, y, heading, alliance, true); + return adjustPoseByAlliance(x, y, heading, alliance, false); } //adjustPoseByAlliance /** @@ -391,13 +402,13 @@ public class Robot /** * This method adjusts the given pose in the red alliance to be the specified alliance. * - * @param pose specifies pose in the blue alliance in tile unit. + * @param pose specifies pose in the red alliance in tile unit. * @param alliance specifies the alliance to be converted to. * @return pose adjusted to be in the specified alliance in inches. */ public TrcPose2D adjustPoseByAlliance(TrcPose2D pose, FtcAuto.Alliance alliance) { - return adjustPoseByAlliance(pose, alliance, true); + return adjustPoseByAlliance(pose, alliance, false); } //adjustPoseByAlliance /** diff --git a/TeamCode/src/main/java/teamcode/RobotParams.java b/TeamCode/src/main/java/teamcode/RobotParams.java index 16e46cc..2937473 100644 --- a/TeamCode/src/main/java/teamcode/RobotParams.java +++ b/TeamCode/src/main/java/teamcode/RobotParams.java @@ -30,7 +30,7 @@ import trclib.driverio.TrcGameController.DriveMode; import trclib.pathdrive.TrcPose2D; /** - * This class contains robot and subsystem constants and parameters. + * This class contains robot constants and parameters. */ public class RobotParams { @@ -54,6 +54,7 @@ public class RobotParams public static final boolean showSubsystems = true; public static final boolean useBlinkinLED = false; public static final boolean useGobildaLED = false; + public static final boolean useRumble = false; // Vision public static final boolean useVision = false; public static final boolean useWebCam = false; // false to use Android phone camera. @@ -63,7 +64,6 @@ public class RobotParams public static final boolean useCameraStreamProcessor = false; public static final boolean useAprilTagVision = false; public static final boolean useColorBlobVision = false; - public static final boolean doWatershed = false; public static final boolean showVisionView = !inCompetition; public static final boolean showVisionStat = false; // Drive Base @@ -110,6 +110,11 @@ public class RobotParams new TrcPose2D(0.0, 0.0, 0.0) // TagId 4 }; // Robot start locations. + // Game elapsed times. + public static final double AUTO_PERIOD = 30.0; // 30 seconds auto period + public static final double TELEOP_PERIOD = 120.0; // 2 minutes teleop period + public static final double PARKING_TIME = 10.0; + public static final double ENDGAME_DEADLINE = TELEOP_PERIOD - PARKING_TIME; } //class Game /** diff --git a/TeamCode/src/main/java/teamcode/autocommands/CmdAuto.java b/TeamCode/src/main/java/teamcode/autocommands/CmdAuto.java index 8b1d628..28d0aef 100644 --- a/TeamCode/src/main/java/teamcode/autocommands/CmdAuto.java +++ b/TeamCode/src/main/java/teamcode/autocommands/CmdAuto.java @@ -112,6 +112,8 @@ public class CmdAuto implements TrcRobot.RobotCommand switch (state) { case START: + // Set robot location according to auto choices. + robot.setRobotStartPosition(autoChoices); if (autoChoices.delay > 0.0) { robot.globalTracer.traceInfo(moduleName, "***** Do delay " + autoChoices.delay + "s."); diff --git a/TeamCode/src/main/java/teamcode/subsystems/RumbleIndicator.java b/TeamCode/src/main/java/teamcode/subsystems/RumbleIndicator.java new file mode 100644 index 0000000..d6cb02d --- /dev/null +++ b/TeamCode/src/main/java/teamcode/subsystems/RumbleIndicator.java @@ -0,0 +1,68 @@ +/* + * Copyright (c) 2024 Titan Robotics Club (http://www.titanrobotics.com) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package teamcode.subsystems; + +import ftclib.driverio.FtcGamepad; +import ftclib.driverio.FtcGamepadRumble; +import trclib.driverio.TrcPriorityIndicator; + +/** + * This class encapsulates the gamepad rumble to provide a priority indicator showing the status of the robot. + */ +public class RumbleIndicator +{ + // Rumble pattern names. + public static final String ENDGAME_DEADLINE = "EndGameDeadline"; + private final TrcPriorityIndicator indicator; + + /** + * Constructor: Create an instance of the object. + * + * @param indicatorName specifies the indicator hardware name. + * @param gamepad specifies the gamepad to use for the rumble. + */ + public RumbleIndicator(String indicatorName, FtcGamepad gamepad) + { + final FtcGamepadRumble.Pattern[] rumblePatternPriorities = { + // Highest priority. + new FtcGamepadRumble.Pattern(ENDGAME_DEADLINE, 3) + // Lowest priority. + }; + indicator = new FtcGamepadRumble(indicatorName, gamepad.gamepad); + ((FtcGamepadRumble) indicator).setPatternPriorities(rumblePatternPriorities); + } //RumbleIndicator + + /** + * This method sets the rumble pattern ON for a period of time and turns off automatically afterwards. + * + * @param patternName specifies the name of the rumble pattern to turn on. + */ + public void setRumblePattern(String patternName) + { + if (indicator != null) + { + indicator.setPatternState(patternName, true, 1.0); + } + } //setRumblePattern + +} //class RumbleIndicator diff --git a/TeamCode/src/main/java/teamcode/subsystems/Vision.java b/TeamCode/src/main/java/teamcode/subsystems/Vision.java index 6d79fe2..77f4d8e 100644 --- a/TeamCode/src/main/java/teamcode/subsystems/Vision.java +++ b/TeamCode/src/main/java/teamcode/subsystems/Vision.java @@ -47,6 +47,7 @@ import ftclib.vision.FtcVisionAprilTag; import ftclib.vision.FtcVisionEocvColorBlob; import teamcode.Robot; import teamcode.RobotParams; +import trclib.dataprocessor.TrcUtil; import trclib.pathdrive.TrcPose2D; import trclib.pathdrive.TrcPose3D; import trclib.robotcore.TrcDbgTrace; @@ -74,25 +75,25 @@ public class Vision camName = "Webcam 1"; camImageWidth = 640; camImageHeight = 480; - camXOffset = 0.0; // Inches to the right from robot center - camYOffset = 2.0; // Inches forward from robot center - camZOffset = 9.75; // Inches up from the floor - camPitch = 15.0; // degrees down from horizontal - camYaw = 0.0; // degrees clockwise from robot front + camXOffset = -4.25; // Inches to the right from robot center + camYOffset = 5.5; // Inches forward from robot center + camZOffset = 10.608; // Inches up from the floor + camYaw = -2.0; // degrees clockwise from robot forward + camPitch = -32.346629699; // degrees up from horizontal camRoll = 0.0; camPose = new TrcPose3D(camXOffset, camYOffset, camZOffset, camYaw, camPitch, camRoll); camOrientation = OpenCvCameraRotation.UPRIGHT; // Homography: cameraRect in pixels, worldRect in inches cameraRect = new TrcHomographyMapper.Rectangle( - 0.0, 120.0, // Camera Top Left - camImageWidth -1, 120.0, // Camera Top Right - 0.0, camImageHeight - 1, // Camera Bottom Left - camImageWidth - 1, camImageHeight - 1); // Camera Bottom Right + 14.0, 28.0, // Camera Top Left + 612.0, 33.0, // Camera Top Right + 56.0, 448.0, // Camera Bottom Left + 581.0, 430.5); // Camera Bottom Right worldRect = new TrcHomographyMapper.Rectangle( - -12.5626, 48.0 - RobotParams.Robot.ROBOT_LENGTH/2.0 - camYOffset, // World Top Left - 11.4375, 44.75 - RobotParams.Robot.ROBOT_LENGTH/2.0 - camYOffset, // World Top Right - -2.5625, 21.0 - RobotParams.Robot.ROBOT_LENGTH/2.0 - camYOffset, // World Bottom Left - 2.5626, 21.0 - RobotParams.Robot.ROBOT_LENGTH/2.0 - camYOffset); // World Bottom Right + -19.0, 37.5, // World Top Left + 24.0, 37.5, // World Top Right + -4.75, 9.0, // World Bottom Left + 6.25, 9.0); // World Bottom Right } //FrontCamParams } //class FrontCamParams @@ -109,8 +110,8 @@ public class Vision camXOffset = 0.0; // Inches to the right from robot center camYOffset = 2.0; // Inches forward from robot center camZOffset = 9.75; // Inches up from the floor - camPitch = 15.0; // degrees down from horizontal camYaw = 0.0; // degrees clockwise from robot front + camPitch = 15.0; // degrees down from horizontal camRoll = 0.0; camPose = new TrcPose3D(camXOffset, camYOffset, camZOffset, camYaw, camPitch, camRoll); camOrientation = OpenCvCameraRotation.UPRIGHT; @@ -133,16 +134,20 @@ public class Vision */ public static class LimelightParams extends FtcRobotDrive.VisionInfo { + public static final int NUM_PIPELINES = 4; + public LimelightParams() { camName = "Limelight3a"; camImageWidth = 640; camImageHeight = 480; - camXOffset = 0.0; // Inches to the right from robot center - camYOffset = 2.0; // Inches forward from robot center - camZOffset = 9.75; // Inches up from the floor - camPitch = 15.0; // degrees down from horizontal - camYaw = 0.0; // degrees clockwise from robot front + camHFov = 80.0; // in degrees + camVFov = 56.0; // in degrees + camXOffset = 135.47*TrcUtil.INCHES_PER_MM; // Inches to the right from robot center + camYOffset = 2.073; // Inches forward from robot center + camZOffset = 10.758; // Inches up from the floor + camYaw = -3.438; // degrees clockwise from robot front + camPitch = 0.0; // degrees down from horizontal camRoll = 0.0; camPose = new TrcPose3D(camXOffset, camYOffset, camZOffset, camYaw, camPitch, camRoll); } //LimelightParams @@ -160,8 +165,8 @@ public class Vision // // YCrCb Color Space. private static final int colorConversion = Imgproc.COLOR_RGB2YCrCb; - private static final double[] redBlobColorThresholds = {20.0, 120.0, 170.0, 220.0, 80.0, 120.0}; - private static final double[] blueBlobColorThresholds = {20.0, 60.0, 100.0, 150.0, 150.0, 180.0}; + private static final double[] redBlobColorThresholds = {10.0, 180.0, 170.0, 240.0, 80.0, 120.0}; + private static final double[] blueBlobColorThresholds = {0.0, 180.0, 80.0, 150.0, 150.0, 200.0}; private static final TrcOpenCvColorBlobPipeline.FilterContourParams colorBlobFilterContourParams = new TrcOpenCvColorBlobPipeline.FilterContourParams() .setMinArea(500.0) @@ -171,6 +176,15 @@ public class Vision .setSolidityRange(0.0, 100.0) .setVerticesRange(0.0, 1000.0) .setAspectRatioRange(0.5, 2.5); + private static final TrcOpenCvColorBlobPipeline.FilterContourParams tuneFilterContourParams = + new TrcOpenCvColorBlobPipeline.FilterContourParams() + .setMinArea(10.0) + .setMinPerimeter(12.0) + .setWidthRange(0.0, 1000.0) + .setHeightRange(0.0, 1000.0) + .setSolidityRange(0.0, 100.0) + .setVerticesRange(0.0, 1000.0) + .setAspectRatioRange(0.5, 2.5); private final TrcDbgTrace tracer; private final Robot robot; @@ -178,7 +192,7 @@ public class Vision private FtcRawEocvColorBlobPipeline rawColorBlobPipeline; public FtcRawEocvVision rawColorBlobVision; public FtcLimelightVision limelightVision; - public FtcCameraStreamProcessor cameraStreamProcessor; + private FtcCameraStreamProcessor cameraStreamProcessor; public FtcVisionAprilTag aprilTagVision; private AprilTagProcessor aprilTagProcessor; public FtcVisionEocvColorBlob redBlobVision; @@ -204,9 +218,9 @@ public class Vision this.tracer = new TrcDbgTrace(); this.robot = robot; - this.webcam1 = robot.robotInfo.webCam1 != null? + webcam1 = robot.robotInfo.webCam1 != null? opMode.hardwareMap.get(WebcamName.class, robot.robotInfo.webCam1.camName): null; - this.webcam2 = robot.robotInfo.webCam2 != null? + webcam2 = robot.robotInfo.webCam2 != null? opMode.hardwareMap.get(WebcamName.class, robot.robotInfo.webCam2.camName): null; if (RobotParams.Preferences.tuneColorBlobVision && webcam1 != null) { @@ -230,8 +244,7 @@ public class Vision tracer.traceInfo(moduleName, "Starting RawEocvColorBlobVision..."); rawColorBlobPipeline = new FtcRawEocvColorBlobPipeline( - "rawColorBlobPipeline", colorConversion, redBlobColorThresholds, colorBlobFilterContourParams, - true, false); + "rawColorBlobPipeline", colorConversion, redBlobColorThresholds, tuneFilterContourParams, true); // By default, display original Mat. rawColorBlobPipeline.setVideoOutput(0); rawColorBlobPipeline.setAnnotateEnabled(true); @@ -262,7 +275,7 @@ public class Vision if (RobotParams.Preferences.useAprilTagVision) { - tracer.traceInfo(moduleName, "Starting AprilTagVision..."); + tracer.traceInfo(moduleName, "Starting Webcam AprilTagVision..."); FtcVisionAprilTag.Parameters aprilTagParams = new FtcVisionAprilTag.Parameters() .setDrawTagIdEnabled(true) .setDrawTagOutlineEnabled(true) @@ -276,16 +289,16 @@ public class Vision if (RobotParams.Preferences.useColorBlobVision && robot.robotInfo.webCam1 != null) { - tracer.traceInfo(moduleName, "Starting ColorBlobVision..."); + tracer.traceInfo(moduleName, "Starting Webcam ColorBlobVision..."); redBlobVision = new FtcVisionEocvColorBlob( - "RedBlob", colorConversion, redBlobColorThresholds, colorBlobFilterContourParams, true, false, + "RedBlob", 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, false, + "BlueBlob", colorConversion, blueBlobColorThresholds, colorBlobFilterContourParams, true, robot.robotInfo.webCam1.cameraRect, robot.robotInfo.webCam1.worldRect, true); blueBlobProcessor = blueBlobVision.getVisionProcessor(); visionProcessorsList.add(blueBlobProcessor); @@ -300,7 +313,8 @@ public class Vision // Use USB webcams. vision = new FtcVision( webcam1, webcam2, robot.robotInfo.webCam1.camImageWidth, robot.robotInfo.webCam1.camImageHeight, - RobotParams.Preferences.showVisionView, RobotParams.Preferences.showVisionStat, visionProcessors); + RobotParams.Preferences.showVisionView, RobotParams.Preferences.showVisionStat, + visionProcessors); } else { @@ -309,7 +323,8 @@ public class Vision RobotParams.Preferences.useBuiltinCamBack? BuiltinCameraDirection.BACK: BuiltinCameraDirection.FRONT, robot.robotInfo.webCam1.camImageWidth, robot.robotInfo.webCam1.camImageHeight, - RobotParams.Preferences.showVisionView, RobotParams.Preferences.showVisionStat, visionProcessors); + RobotParams.Preferences.showVisionView, RobotParams.Preferences.showVisionStat, + visionProcessors); } // Disable all vision until they are needed. @@ -498,6 +513,8 @@ public class Vision limelightVision.setPipeline(pipelineIndex); } limelightVision.setVisionEnabled(enabled); + tracer.traceInfo(moduleName, "Pipeline %d is %s: running=%s", + pipelineIndex, enabled? "enabled": "disabled", limelightVision.limelight.isRunning()); } } //setLimelightVisionEnabled @@ -523,26 +540,47 @@ public class Vision FtcLimelightVision.ResultType resultType, String label, int lineNum) { TrcVisionTargetInfo limelightInfo = null; - String objectName = null; if (limelightVision != null) { - limelightInfo = limelightVision.getBestDetectedTargetInfo(resultType, label, null); + String objectName = null; + int pipelineIndex = -1; + Double robotHeading = robot.robotDrive != null? robot.robotDrive.driveBase.getHeading(): null; + + limelightInfo = limelightVision.getBestDetectedTargetInfo(resultType, label, robotHeading, null); if (limelightInfo != null) { - objectName = limelightInfo.detectedObj.label; + pipelineIndex = limelightVision.getPipeline(); + switch (pipelineIndex) + { + case 0: + objectName = LEDIndicator.APRIL_TAG; + break; + + case 1: + objectName = LEDIndicator.RED_BLOB; + break; + + case 2: + objectName = LEDIndicator.BLUE_BLOB; + break; + + default: + break; + } } - } - if (objectName != null && robot.ledIndicator != null) - { - robot.ledIndicator.setDetectedPattern(objectName); - } + if (objectName != null && robot.ledIndicator != null) + { + robot.ledIndicator.setDetectedPattern(objectName); + } - if (lineNum != -1) - { - robot.dashboard.displayPrintf( - lineNum, "%s: %s", objectName, limelightInfo != null? limelightInfo: "Not found."); + if (lineNum != -1) + { + robot.dashboard.displayPrintf( + lineNum, "%s(%d): %s", + objectName, pipelineIndex, limelightInfo != null? limelightInfo: "Not found."); + } } return limelightInfo; @@ -770,11 +808,12 @@ public class Vision * This method calls ColorBlob vision to detect the specified color blob object. * * @param colorBlobType specifies the color blob type to be detected. + * @param groundOffset specifies the ground offset of the detected sample. * @param lineNum specifies the dashboard line number to display the detected object info, -1 to disable printing. * @return detected color blob object info. */ public TrcVisionTargetInfo getDetectedColorBlob( - ColorBlobType colorBlobType, int lineNum) + ColorBlobType colorBlobType, double groundOffset, int lineNum) { TrcVisionTargetInfo colorBlobInfo = null; @@ -782,12 +821,12 @@ public class Vision { case RedBlob: colorBlobInfo = redBlobVision != null? redBlobVision.getBestDetectedTargetInfo( - null, this::compareDistance, 0.0, 0.0): null; + null, this::compareDistance, groundOffset, robot.robotInfo.webCam1.camZOffset): null; break; case BlueBlob: colorBlobInfo = blueBlobVision != null? blueBlobVision.getBestDetectedTargetInfo( - null, this::compareDistance, 0.0, 0.0): null; + null, this::compareDistance, groundOffset, robot.robotInfo.webCam1.camZOffset): null; break; case AnyColorBlob: @@ -795,14 +834,14 @@ public class Vision new ArrayList<>(); colorBlobInfo = redBlobVision != null ? redBlobVision.getBestDetectedTargetInfo( - null, this::compareDistance, 0.0, 0.0) : null; + null, this::compareDistance, groundOffset, robot.robotInfo.webCam1.camZOffset) : null; if (colorBlobInfo != null) { colorBlobList.add(colorBlobInfo); } colorBlobInfo = blueBlobVision != null ? blueBlobVision.getBestDetectedTargetInfo( - null, this::compareDistance, 0.0, 0.0) : null; + null, this::compareDistance, groundOffset, robot.robotInfo.webCam1.camZOffset) : null; if (colorBlobInfo != null) { colorBlobList.add(colorBlobInfo); @@ -835,7 +874,7 @@ public class Vision if (colorBlobInfo != null) { robot.dashboard.displayPrintf( - lineNum, "%s: %s (rotatedAngle=%.1f", + lineNum, "%s: %s (rotatedAngle=%.1f)", colorBlobInfo.detectedObj.label, colorBlobInfo, colorBlobInfo.detectedObj.rotatedRect.angle); } else @@ -861,6 +900,10 @@ public class Vision { offset = 5.75; } + else if ( resultType == FtcLimelightVision.ResultType.Python) + { + offset = 10.0; + } return offset; } //getTargetGroundOffset diff --git a/TeamCode/src/main/java/trclib b/TeamCode/src/main/java/trclib index 077f132..75535f0 160000 --- a/TeamCode/src/main/java/trclib +++ b/TeamCode/src/main/java/trclib @@ -1 +1 @@ -Subproject commit 077f1325b73a057493c1b76c4de571fc83c0cbc3 +Subproject commit 75535f0b03a866e1d1c2658b6ffbfb33770795a9