diff --git a/TeamCode/src/main/java/ftclib b/TeamCode/src/main/java/ftclib index 2114117..6358a7e 160000 --- a/TeamCode/src/main/java/ftclib +++ b/TeamCode/src/main/java/ftclib @@ -1 +1 @@ -Subproject commit 2114117db4ca02599de16a981f207ca67f1cc501 +Subproject commit 6358a7e46b7a73ab78680f80093231f4db1de1a3 diff --git a/TeamCode/src/main/java/teamcode/Dashboard.java b/TeamCode/src/main/java/teamcode/Dashboard.java new file mode 100644 index 0000000..eb073fe --- /dev/null +++ b/TeamCode/src/main/java/teamcode/Dashboard.java @@ -0,0 +1,112 @@ +/* + * Copyright (c) 2025 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; + +import com.acmerobotics.dashboard.config.Config; + +import trclib.controller.TrcPidController; +import trclib.subsystem.TrcSubsystem; +import trclib.timer.TrcTimer; +import trclib.vision.TrcOpenCvColorBlobPipeline; + +/** + * This class creates the robot object that consists of sensors, indicators, drive base and all the subsystems. + */ +@Config +public class Dashboard +{ + private static Double nextDashboardUpdateTime = null; + + /** + * This method is called periodically to update various hardware/subsystem status of the robot to the dashboard + * and trace log. In order to lower the potential impact these updates, this method will only update the dashboard + * at DASHBOARD_UPDATE_INTERVAL. + * + * @param robot specifies the robot object. + * @param lineNum specifies the first Dashboard line for printing status. + * @return next available dashboard line. + */ + public static int updateDashboard(Robot robot, int lineNum) + { + double currTime = TrcTimer.getCurrentTime(); + + if (nextDashboardUpdateTime == null || currTime >= nextDashboardUpdateTime) + { + nextDashboardUpdateTime = currTime + RobotParams.Robot.DASHBOARD_UPDATE_INTERVAL; + if (RobotParams.Preferences.showDriveBase) + { + lineNum = robot.robotBase.updateStatus(lineNum); + } + + if (RobotParams.Preferences.showVision && robot.vision != null) + { + lineNum = robot.vision.updateStatus(lineNum); + } + + if (RobotParams.Preferences.showSubsystems) + { + lineNum = TrcSubsystem.updateStatusAll(lineNum); + } + } + + return lineNum; + } //updateDashboard + + @Config + public static class Vision + { + public static double[] colorThresholds = null; + public static TrcOpenCvColorBlobPipeline.FilterContourParams filterContourParams = null; + public static long exposure = 0; + } //class Vision + + @Config + public static class Drive + { + public static TrcPidController.PidCoefficients pidCoeffs = + new TrcPidController.PidCoefficients(0.0, 0.0, 0.0, 0.0, 0.0); + public static double maxVelocity = 0.0; + public static double maxAcceleration = 0.0; + public static double maxDeclearion = 0.0; + public static double xTarget = 0.0; + public static double yTarget = 0.0; + public static double turnTarget = 0.0; + public static double drivePower = 1.0; + public static double driveTime = 0.0; + } //class Drive + + @Config + public static class Subsystem + { + public static TrcPidController.PidCoefficients pidCoeffs = + new TrcPidController.PidCoefficients(0.0, 0.0, 0.0, 0.0, 0.0); + public static TrcPidController.FFCoefficients ffCoeffs = + new TrcPidController.FFCoefficients(0.0, 0.0, 0.0); + public static double pidTolerance = 1.0; + public static boolean softwarePid = true; + public static boolean enableSquid = true; + public static double powerLimit = 1.0; + public static double maxGarvityCompPower = 0.0; + } //class Subsystem + +} //class Dashboard diff --git a/TeamCode/src/main/java/teamcode/FtcAuto.java b/TeamCode/src/main/java/teamcode/FtcAuto.java index 084fc7a..ac4bed1 100644 --- a/TeamCode/src/main/java/teamcode/FtcAuto.java +++ b/TeamCode/src/main/java/teamcode/FtcAuto.java @@ -297,6 +297,11 @@ public class FtcAuto extends FtcOpMode // autoCommand.cmdPeriodic(elapsedTime); } + + if (RobotParams.Preferences.updateDashboard) + { + Dashboard.updateDashboard(robot, 1); + } } //periodic /** diff --git a/TeamCode/src/main/java/teamcode/FtcTeleOp.java b/TeamCode/src/main/java/teamcode/FtcTeleOp.java index 1954317..63093d2 100644 --- a/TeamCode/src/main/java/teamcode/FtcTeleOp.java +++ b/TeamCode/src/main/java/teamcode/FtcTeleOp.java @@ -49,8 +49,8 @@ public class FtcTeleOp extends FtcOpMode protected FtcGamepad operatorGamepad; private double drivePowerScale; private double turnPowerScale; - private boolean driverAltFunc = false; - private boolean operatorAltFunc = false; + protected boolean driverAltFunc = false; + protected boolean operatorAltFunc = false; private boolean statusUpdateOn = false; private boolean relocalizing = false; private TrcPose2D robotFieldPose = null; @@ -217,7 +217,7 @@ public class FtcTeleOp extends FtcOpMode robot.robotDrive.driveBase.arcadeDrive(inputs[1], inputs[2]); } - if (RobotParams.Preferences.doStatusUpdate || statusUpdateOn) + if (RobotParams.Preferences.updateDashboard || statusUpdateOn) { robot.dashboard.displayPrintf( lineNum++, "RobotDrive: Power=(%.2f,y=%.2f,rot=%.2f),Mode:%s", @@ -246,9 +246,9 @@ public class FtcTeleOp extends FtcOpMode // Analog control of subsystems. } // Display subsystem status. - if (RobotParams.Preferences.doStatusUpdate || statusUpdateOn) + if (RobotParams.Preferences.updateDashboard || statusUpdateOn) { - robot.updateStatus(lineNum); + Dashboard.updateDashboard(robot, lineNum); } } } //periodic @@ -341,7 +341,7 @@ public class FtcTeleOp extends FtcOpMode case RightBumper: if (driverAltFunc) { - if (!RobotParams.Preferences.doStatusUpdate) + if (!RobotParams.Preferences.updateDashboard) { // Toggle status update ON/OFF. statusUpdateOn = !statusUpdateOn; diff --git a/TeamCode/src/main/java/teamcode/FtcTest.java b/TeamCode/src/main/java/teamcode/FtcTest.java index 730c8a1..caf8a49 100644 --- a/TeamCode/src/main/java/teamcode/FtcTest.java +++ b/TeamCode/src/main/java/teamcode/FtcTest.java @@ -26,25 +26,20 @@ import androidx.annotation.NonNull; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import java.util.Arrays; -import java.util.Locale; - import ftclib.drivebase.FtcRobotDrive; import ftclib.drivebase.FtcSwerveDrive; import ftclib.driverio.FtcChoiceMenu; import ftclib.driverio.FtcGamepad; import ftclib.driverio.FtcMenu; -import ftclib.driverio.FtcValueMenu; -import ftclib.robotcore.FtcPidCoeffCache; import ftclib.vision.FtcLimelightVision; -import teamcode.subsystems.Vision; +import teamcode.vision.Vision; import trclib.command.CmdDriveMotorsTest; import trclib.command.CmdPidDrive; import trclib.command.CmdTimedDrive; +import trclib.controller.TrcPidController; import trclib.dataprocessor.TrcUtil; import trclib.pathdrive.TrcPose2D; import trclib.robotcore.TrcDbgTrace; -import trclib.robotcore.TrcPidController; import trclib.robotcore.TrcRobot; import trclib.timer.TrcElapsedTimer; import trclib.timer.TrcTimer; @@ -58,8 +53,8 @@ import trclib.timer.TrcTimer; public class FtcTest extends FtcTeleOp { private final String moduleName = getClass().getSimpleName(); - private static final boolean logEvents = true; - private static final boolean debugPid = true; + private static final boolean logEvents = false; + private static final boolean debugPid = false; private enum Test { @@ -72,10 +67,10 @@ public class FtcTest extends FtcTeleOp X_TIMED_DRIVE, Y_TIMED_DRIVE, PID_DRIVE, + PP_DRIVE, TUNE_X_PID, TUNE_Y_PID, TUNE_TURN_PID, - PURE_PURSUIT_DRIVE, CALIBRATE_SWERVE_STEERING } //enum Test @@ -85,61 +80,32 @@ public class FtcTest extends FtcTeleOp private static class TestChoices { Test test = Test.SENSORS_TEST; - double xTarget = 0.0; - double yTarget = 0.0; - double turnTarget = 0.0; - double driveTime = 0.0; - double drivePower = 0.0; - TrcPidController.PidCoefficients tunePidCoeffs = null; - double tuneDistance = 0.0; - double tuneHeading = 0.0; - double tuneDrivePower = 0.0; @NonNull @Override public String toString() { - return String.format( - Locale.US, - "test=\"%s\" " + - "xTarget=%.1f " + - "yTarget=%.1f " + - "turnTarget=%1f " + - "driveTime=%.1f " + - "drivePower=%.1f " + - "tunePidCoeffs=%s " + - "tuneDistance=%.1f " + - "tuneHeading=%.1f " + - "tuneDrivePower=%.1f", - test, xTarget, yTarget, turnTarget, driveTime, drivePower, tunePidCoeffs, tuneDistance, tuneHeading, - tuneDrivePower); + return "test=\"" + test + "\""; } //toString } //class TestChoices - private final FtcPidCoeffCache pidCoeffCache = new FtcPidCoeffCache(RobotParams.Robot.TEAM_FOLDER_PATH); private final TestChoices testChoices = new TestChoices(); private TrcElapsedTimer elapsedTimer = null; - private FtcChoiceMenu testMenu = null; private TrcRobot.RobotCommand testCommand = null; // Drive Speed Test. private double maxDriveVelocity = 0.0; private double maxDriveAcceleration = 0.0; + private double maxDriveDeceleration = 0.0; private double maxTurnVelocity = 0.0; private double prevTime = 0.0; private double prevVelocity = 0.0; // Swerve Steering Calibration. private boolean steerCalibrating = false; - // Color Blob Vision Turning. - private static final double[] COLOR_THRESHOLD_LOW_RANGES = {0.0, 0.0, 0.0}; - private static final double[] COLOR_THRESHOLD_HIGH_RANGES = {255.0, 255.0, 255.0}; - private double[] colorThresholds = null; - private int colorThresholdIndex = 0; - private double colorThresholdMultiplier = 1.0; private boolean teleOpControlEnabled = true; - private long exposure; private boolean fpsMeterEnabled = false; + // // Overrides FtcOpMode abstract method. // @@ -171,7 +137,8 @@ public class FtcTest extends FtcTeleOp case DRIVE_MOTORS_TEST: if (robot.robotDrive != null) { - testCommand = new CmdDriveMotorsTest(robot.robotDrive.driveMotors, 5.0, 0.5); + testCommand = new CmdDriveMotorsTest( + robot.robotDrive.driveBase, robot.robotDrive.driveMotors, 5.0, 0.5); } break; @@ -179,7 +146,8 @@ public class FtcTest extends FtcTeleOp if (robot.robotDrive != null) { testCommand = new CmdTimedDrive( - robot.robotDrive.driveBase, 0.0, testChoices.driveTime, testChoices.drivePower, 0.0, 0.0); + robot.robotDrive.driveBase, 0.0, Dashboard.Drive.driveTime, + Dashboard.Drive.drivePower, 0.0, 0.0); } break; @@ -187,16 +155,13 @@ public class FtcTest extends FtcTeleOp if (robot.robotDrive != null) { testCommand = new CmdTimedDrive( - robot.robotDrive.driveBase, 0.0, testChoices.driveTime, 0.0, testChoices.drivePower, 0.0); + robot.robotDrive.driveBase, 0.0, Dashboard.Drive.driveTime, + 0.0, Dashboard.Drive.drivePower, 0.0); } break; case PID_DRIVE: - case TUNE_X_PID: - case TUNE_Y_PID: - case TUNE_TURN_PID: - if (robot.robotDrive != null && - (testChoices.test != Test.TUNE_X_PID || robot.robotDrive.driveBase.supportsHolonomicDrive())) + if (robot.robotDrive != null) { testCommand = new CmdPidDrive(robot.robotDrive.driveBase, robot.robotDrive.pidDrive); } @@ -225,10 +190,6 @@ public class FtcTest extends FtcTeleOp if (robot.vision != null) { robot.vision.setCameraStreamEnabled(true); - if (robot.vision.vision != null) - { - exposure = robot.vision.vision.getCurrentExposure(); - } // Vision generally will impact performance, so we only enable it if it's needed. if (robot.vision.aprilTagVision != null) { @@ -262,10 +223,10 @@ public class FtcTest extends FtcTeleOp robot.globalTracer.traceInfo(moduleName, "Enabling FtcRawEocvVision."); robot.vision.setCameraStreamEnabled(true); robot.vision.setRawColorBlobVisionEnabled(true); - colorThresholds = robot.vision.getRawColorBlobThresholds(); - colorThresholdIndex = 0; - colorThresholdMultiplier = 1.0; - updateColorThresholds(); + Dashboard.Vision.colorThresholds = robot.vision.getRawColorBlobThresholds(); + Dashboard.Vision.filterContourParams = Vision.colorBlobFilterContourParams.clone(); + Dashboard.Vision.exposure = + robot.vision.vision != null? robot.vision.vision.getCurrentExposure(): 0; } break; @@ -273,56 +234,23 @@ public class FtcTest extends FtcTeleOp if (testCommand != null) { ((CmdPidDrive) testCommand).start( - 0.0, testChoices.drivePower, null, - new TrcPose2D(testChoices.xTarget*12.0, testChoices.yTarget*12.0, testChoices.turnTarget)); + 0.0, Dashboard.Drive.drivePower, null, + new TrcPose2D(Dashboard.Drive.xTarget*12.0, Dashboard.Drive.yTarget*12.0, + Dashboard.Drive.turnTarget)); robot.robotDrive.pidDrive.setTraceLevel(TrcDbgTrace.MsgLevel.INFO, logEvents, debugPid, false); } break; - case TUNE_X_PID: - if (testCommand != null) - { - ((CmdPidDrive) testCommand).start( - 0.0, testChoices.drivePower, testChoices.tunePidCoeffs, - new TrcPose2D(testChoices.tuneDistance*12.0, 0.0, 0.0)); - robot.robotDrive.pidDrive.setTraceLevel(TrcDbgTrace.MsgLevel.INFO, logEvents, debugPid, false); - } - break; - - case TUNE_Y_PID: - if (testCommand != null) - { - ((CmdPidDrive) testCommand).start( - 0.0, testChoices.drivePower, testChoices.tunePidCoeffs, - new TrcPose2D(0.0, testChoices.tuneDistance*12.0, 0.0)); - robot.robotDrive.pidDrive.setTraceLevel(TrcDbgTrace.MsgLevel.INFO, logEvents, debugPid, false); - } - break; - - case TUNE_TURN_PID: - if (testCommand != null) - { - ((CmdPidDrive) testCommand).start( - 0.0, testChoices.drivePower, testChoices.tunePidCoeffs, - new TrcPose2D(0.0, 0.0, testChoices.tuneHeading)); - robot.robotDrive.pidDrive.setTraceLevel(TrcDbgTrace.MsgLevel.INFO, logEvents, debugPid, false); - } - break; - - case PURE_PURSUIT_DRIVE: + case PP_DRIVE: if (robot.robotDrive != null) { robot.robotDrive.purePursuitDrive.setTraceLevel( TrcDbgTrace.MsgLevel.INFO, logEvents, debugPid, false); - // - // Doing a 48x48-inch square box with robot heading always pointing to the center of the box. - // - // Set the current position as the absolute field origin so the path can be an absolute path. - TrcPose2D startPose = new TrcPose2D(0.0, 0.0, 0.0); - robot.robotDrive.driveBase.setFieldPosition(startPose); robot.robotDrive.purePursuitDrive.start( - false, robot.robotInfo.profiledMaxVelocity, robot.robotInfo.profiledMaxAcceleration, - robot.robotInfo.profiledMaxDeceleration, new TrcPose2D(0.0, 48.0, 90.0)); + true, robot.robotInfo.profiledMaxVelocity, robot.robotInfo.profiledMaxAcceleration, + robot.robotInfo.profiledMaxAcceleration, + new TrcPose2D(Dashboard.Drive.xTarget*12.0, Dashboard.Drive.yTarget*12.0, + Dashboard.Drive.turnTarget)); } break; } @@ -373,46 +301,60 @@ public class FtcTest extends FtcTeleOp // // Display test status. // - switch (testChoices.test) + if (testChoices.test == Test.DRIVE_SPEED_TEST) { - case DRIVE_SPEED_TEST: - if (robot.robotDrive != null) + if (robot.robotDrive != null) + { + double currTime = TrcTimer.getCurrentTime(); + TrcPose2D velPose = robot.robotDrive.driveBase.getFieldVelocity(); + double velocity = TrcUtil.magnitude(velPose.x, velPose.y); + double acceleration = 0.0; + double deceleration = 0.0; + double deltaTime = currTime - prevTime; + + if (prevTime != 0.0) { - double currTime = TrcTimer.getCurrentTime(); - TrcPose2D velPose = robot.robotDrive.driveBase.getFieldVelocity(); - double velocity = TrcUtil.magnitude(velPose.x, velPose.y); - double acceleration = 0.0; - - if (prevTime != 0.0) + if (velocity > prevVelocity) { - acceleration = (velocity - prevVelocity)/(currTime - prevTime); + acceleration = (velocity - prevVelocity)/deltaTime; } - - if (velocity > maxDriveVelocity) + else { - maxDriveVelocity = velocity; + deceleration = (prevVelocity - velocity)/deltaTime; } - - if (acceleration > maxDriveAcceleration) - { - maxDriveAcceleration = acceleration; - } - - if (velPose.angle > maxTurnVelocity) - { - maxTurnVelocity = velPose.angle; - } - - prevTime = currTime; - prevVelocity = velocity; - - robot.dashboard.displayPrintf(lineNum++, "Drive Vel: (%.1f/%.1f)", velocity, maxDriveVelocity); - robot.dashboard.displayPrintf( - lineNum++, "Drive Accel: (%.1f/%.1f)", acceleration, maxDriveAcceleration); - robot.dashboard.displayPrintf( - lineNum++, "Turn Vel: (%.1f/%.1f)", velPose.angle, maxTurnVelocity); } - break; + + if (velocity > maxDriveVelocity) + { + maxDriveVelocity = velocity; + } + + if (acceleration > maxDriveAcceleration) + { + maxDriveAcceleration = acceleration; + } + + if (deceleration > maxDriveDeceleration) + { + maxDriveDeceleration = deceleration; + } + + if (velPose.angle > maxTurnVelocity) + { + maxTurnVelocity = velPose.angle; + } + + prevTime = currTime; + prevVelocity = velocity; + + robot.dashboard.displayPrintf(lineNum++, "Drive Vel: (%.1f/%.1f)", velocity, maxDriveVelocity); + robot.dashboard.displayPrintf( + lineNum++, "Drive Accel: (%.1f/%.1f)", acceleration, maxDriveAcceleration); + robot.dashboard.displayPrintf( + lineNum++, "Drive Decel: (%.1f/%.1f)", deceleration, maxDriveDeceleration); + robot.dashboard.displayPrintf( + lineNum++, "Turn Vel: (%.1f/%.1f)", velPose.angle, maxTurnVelocity); + } } if (elapsedTimer != null) @@ -450,7 +392,7 @@ public class FtcTest extends FtcTeleOp case Y_TIMED_DRIVE: if (robot.robotDrive != null) { - robot.dashboard.displayPrintf(lineNum++, "Timed Drive: %.0f sec", testChoices.driveTime); + robot.dashboard.displayPrintf(lineNum++, "Timed Drive: %.0f sec", Dashboard.Drive.driveTime); robot.dashboard.displayPrintf( lineNum++, "RobotPose=%s", robot.robotDrive.driveBase.getFieldPosition()); robot.dashboard.displayPrintf( @@ -465,19 +407,26 @@ public class FtcTest extends FtcTeleOp case TUNE_X_PID: case TUNE_Y_PID: case TUNE_TURN_PID: - if (robot.robotDrive != null && testChoices.tunePidCoeffs != null) + if (robot.robotDrive != null) { - robot.dashboard.displayPrintf(7, "TunePid=%s", testChoices.tunePidCoeffs); + robot.dashboard.putObject( + "robotVelocity", robot.robotDrive.purePursuitDrive.getPathRobotVelocity()); + robot.dashboard.putObject( + "targetVelocity", robot.robotDrive.purePursuitDrive.getPathTargetVelocity()); + robot.dashboard.putObject( + "robotPosition", robot.robotDrive.purePursuitDrive.getPathRelativePosition()); + robot.dashboard.putObject( + "targetPosition", robot.robotDrive.purePursuitDrive.getPathPositionTarget()); } // // Intentionally falling through. // - case PURE_PURSUIT_DRIVE: + case PP_DRIVE: case PID_DRIVE: if (robot.robotDrive != null) { TrcPidController xPidCtrl, yPidCtrl, turnPidCtrl; - if (testChoices.test == Test.PURE_PURSUIT_DRIVE) + if (testChoices.test == Test.PP_DRIVE) { xPidCtrl = robot.robotDrive.purePursuitDrive.getXPosPidCtrl(); yPidCtrl = robot.robotDrive.purePursuitDrive.getYPosPidCtrl(); @@ -542,7 +491,61 @@ public class FtcTest extends FtcTeleOp switch (button) { case A: - if (testChoices.test == Test.CALIBRATE_SWERVE_STEERING) + if (testChoices.test == Test.TUNE_COLORBLOB_VISION && + robot.vision != null && robot.vision.rawColorBlobVision != null) + { + if (pressed) + { + // Update all vision tuning parameters. + robot.vision.vision.setManualExposure(Dashboard.Vision.exposure, null); + } + passToTeleOp = false; + } + else if (robot.robotDrive != null && + (testChoices.test == Test.TUNE_X_PID || testChoices.test == Test.TUNE_Y_PID || + testChoices.test == Test.TUNE_TURN_PID)) + { + if (pressed) + { + robot.robotDrive.driveBase.resetOdometry(); + switch (testChoices.test) + { + case TUNE_X_PID: + robot.robotDrive.purePursuitDrive.setXPositionPidCoefficients( + Dashboard.Drive.pidCoeffs); + robot.robotDrive.purePursuitDrive.setMoveOutputLimit(Dashboard.Drive.drivePower); + robot.robotDrive.purePursuitDrive.start( + true, Dashboard.Drive.maxVelocity, Dashboard.Drive.maxAcceleration, + Dashboard.Drive.maxDeclearion, + new TrcPose2D(Dashboard.Drive.xTarget*12.0, 0.0, 0.0)); + Dashboard.Drive.xTarget *= -1.0; + break; + + case TUNE_Y_PID: + robot.robotDrive.purePursuitDrive.setYPositionPidCoefficients( + Dashboard.Drive.pidCoeffs); + robot.robotDrive.purePursuitDrive.setMoveOutputLimit(Dashboard.Drive.drivePower); + robot.robotDrive.purePursuitDrive.start( + true, Dashboard.Drive.maxVelocity, Dashboard.Drive.maxAcceleration, + Dashboard.Drive.maxDeclearion, + new TrcPose2D(0.0, Dashboard.Drive.yTarget*12.0, 0.0)); + Dashboard.Drive.yTarget *= -1.0; + break; + + case TUNE_TURN_PID: + robot.robotDrive.purePursuitDrive.setTurnPidCoefficients(Dashboard.Drive.pidCoeffs); + robot.robotDrive.purePursuitDrive.setRotOutputLimit(Dashboard.Drive.drivePower); + robot.robotDrive.purePursuitDrive.start( + true, Dashboard.Drive.maxVelocity, Dashboard.Drive.maxAcceleration, + Dashboard.Drive.maxDeclearion, + new TrcPose2D(0.0, 0.0, Dashboard.Drive.turnTarget)); + break; + } + + } + passToTeleOp = false; + } + else if (testChoices.test == Test.CALIBRATE_SWERVE_STEERING) { if (pressed && robot.robotDrive != null && robot.robotDrive instanceof FtcSwerveDrive) { @@ -565,21 +568,7 @@ public class FtcTest extends FtcTeleOp break; case B: - if (testChoices.test == Test.TUNE_COLORBLOB_VISION && - robot.vision != null && robot.vision.rawColorBlobVision != null) - { - if (pressed) - { - // Increment to next color threshold index. - colorThresholdIndex++; - if (colorThresholdIndex >= colorThresholds.length) - { - colorThresholdIndex = colorThresholds.length - 1; - } - } - passToTeleOp = false; - } - else if (testChoices.test == Test.VISION_TEST) + if (testChoices.test == Test.VISION_TEST) { if (pressed) { @@ -592,23 +581,12 @@ public class FtcTest extends FtcTeleOp break; case X: - if (testChoices.test == Test.TUNE_COLORBLOB_VISION && - robot.vision != null && robot.vision.rawColorBlobVision != null) - { - if (pressed) - { - // Decrement to previous color threshold index. - colorThresholdIndex--; - if (colorThresholdIndex < 0) - { - colorThresholdIndex = 0; - } - } - passToTeleOp = false; - } + case Y: + case LeftBumper: + case RightBumper: break; - case Y: + case DpadUp: if (testChoices.test == Test.TUNE_COLORBLOB_VISION && robot.vision != null && robot.vision.rawColorBlobVision != null) { @@ -629,56 +607,31 @@ public class FtcTest extends FtcTeleOp robot.vision.limelightVision.setPipeline(pipelineIndex); robot.globalTracer.traceInfo(moduleName, "Switch Limelight pipeline to " + pipelineIndex); } - } - break; - - case LeftBumper: - if (testChoices.test == Test.VISION_TEST && robot.vision != null && robot.vision.vision != null) - { - if (pressed) - { - exposure -= 100; - robot.vision.vision.setManualExposure(exposure, null); - } passToTeleOp = false; } - break; - - case RightBumper: - if (testChoices.test == Test.VISION_TEST && robot.vision != null && robot.vision.vision != null) + else if (testChoices.test == Test.SUBSYSTEMS_TEST) { - if (pressed) + if (RobotParams.Preferences.tuneDriveBase) { - exposure += 100; - robot.vision.vision.setManualExposure(exposure, null); - } - passToTeleOp = false; - } - break; - - case DpadUp: - if (testChoices.test == Test.SUBSYSTEMS_TEST) - { - // If we are moving swerve steering, make sure TeleOp doesn't interfere. - teleOpControlEnabled = !pressed; - if (pressed && robot.robotDrive != null && robot.robotDrive instanceof FtcSwerveDrive) - { - FtcSwerveDrive swerveDrive = (FtcSwerveDrive) robot.robotDrive; - swerveDrive.setSteerAngle(0.0, false, true); - } - passToTeleOp = false; - } - else if (testChoices.test == Test.TUNE_COLORBLOB_VISION && - robot.vision != null && robot.vision.rawColorBlobVision != null) - { - if (pressed && - colorThresholds[colorThresholdIndex] + colorThresholdMultiplier <= - COLOR_THRESHOLD_HIGH_RANGES[colorThresholdIndex/2]) - { - // Increment color threshold value. - colorThresholds[colorThresholdIndex] += colorThresholdMultiplier; - updateColorThresholds(); + // If we are moving swerve steering, make sure TeleOp doesn't interfere. + teleOpControlEnabled = !pressed; + if (pressed && robot.robotDrive != null && robot.robotDrive instanceof FtcSwerveDrive) + { + FtcSwerveDrive swerveDrive = (FtcSwerveDrive) robot.robotDrive; + swerveDrive.setSteerAngle(0.0, false, true); + } } +// else if (RobotParams.Preferences.tuneElevator) +// { +// if (pressed) +// { +// robot.elevator.setPositionPidParameters( +// Dashboard.Subsystem.pidCoeffs, Dashboard.Subsystem.ffCoeffs, +// Dashboard.Subsystem.pidTolerance, Dashboard.Subsystem.softwarePid, +// Dashboard.Subsystem.enableSquid); +// robot.elevator.presetPositionUp(null, Dashboard.Subsystem.powerLimit); +// } +// } passToTeleOp = false; } break; @@ -686,26 +639,27 @@ public class FtcTest extends FtcTeleOp case DpadDown: if (testChoices.test == Test.SUBSYSTEMS_TEST) { - // If we are moving swerve steering, make sure TeleOp doesn't interfere. - teleOpControlEnabled = !pressed; - if (pressed && robot.robotDrive != null && robot.robotDrive instanceof FtcSwerveDrive) + if (RobotParams.Preferences.tuneDriveBase) { - FtcSwerveDrive swerveDrive = (FtcSwerveDrive) robot.robotDrive; - swerveDrive.setSteerAngle(180.0, false, true); - } - passToTeleOp = false; - } - else if (testChoices.test == Test.TUNE_COLORBLOB_VISION && - robot.vision != null && robot.vision.rawColorBlobVision != null) - { - if (pressed && - colorThresholds[colorThresholdIndex] - colorThresholdMultiplier >= - COLOR_THRESHOLD_LOW_RANGES[colorThresholdIndex/2]) - { - // Decrement color threshold value. - colorThresholds[colorThresholdIndex] -= colorThresholdMultiplier; - updateColorThresholds(); + // If we are moving swerve steering, make sure TeleOp doesn't interfere. + teleOpControlEnabled = !pressed; + if (pressed && robot.robotDrive != null && robot.robotDrive instanceof FtcSwerveDrive) + { + FtcSwerveDrive swerveDrive = (FtcSwerveDrive) robot.robotDrive; + swerveDrive.setSteerAngle(180.0, false, true); + } } +// else if (RobotParams.Preferences.tuneElevator) +// { +// if (pressed) +// { +// robot.elevator.setPositionPidParameters( +// Dashboard.Subsystem.pidCoeffs, Dashboard.Subsystem.ffCoeffs, +// Dashboard.Subsystem.pidTolerance, Dashboard.Subsystem.softwarePid, +// Dashboard.Subsystem.enableSquid); +// robot.elevator.presetPositionDown(null, Dashboard.Subsystem.powerLimit); +// } +// } passToTeleOp = false; } break; @@ -713,22 +667,15 @@ public class FtcTest extends FtcTeleOp case DpadLeft: if (testChoices.test == Test.SUBSYSTEMS_TEST) { - // If we are moving swerve steering, make sure TeleOp doesn't interfere. - teleOpControlEnabled = !pressed; - if (pressed && robot.robotDrive != null && robot.robotDrive instanceof FtcSwerveDrive) + if (RobotParams.Preferences.tuneDriveBase) { - FtcSwerveDrive swerveDrive = (FtcSwerveDrive) robot.robotDrive; - swerveDrive.setSteerAngle(270.0, false, true); - } - passToTeleOp = false; - } - else if (testChoices.test == Test.TUNE_COLORBLOB_VISION && - robot.vision != null && robot.vision.rawColorBlobVision != null) - { - if (pressed && colorThresholdMultiplier * 10.0 <= 100.0) - { - // Increment the significant multiplier. - colorThresholdMultiplier *= 10.0; + // If we are moving swerve steering, make sure TeleOp doesn't interfere. + teleOpControlEnabled = !pressed; + if (pressed && robot.robotDrive != null && robot.robotDrive instanceof FtcSwerveDrive) + { + FtcSwerveDrive swerveDrive = (FtcSwerveDrive) robot.robotDrive; + swerveDrive.setSteerAngle(270.0, false, true); + } } passToTeleOp = false; } @@ -737,22 +684,15 @@ public class FtcTest extends FtcTeleOp case DpadRight: if (testChoices.test == Test.SUBSYSTEMS_TEST) { - // If we are moving swerve steering, make sure TeleOp doesn't interfere. - teleOpControlEnabled = !pressed; - if (pressed && robot.robotDrive != null && robot.robotDrive instanceof FtcSwerveDrive) + if (RobotParams.Preferences.tuneDriveBase) { - FtcSwerveDrive swerveDrive = (FtcSwerveDrive) robot.robotDrive; - swerveDrive.setSteerAngle(90.0, false, true); - } - passToTeleOp = false; - } - else if (testChoices.test == Test.TUNE_COLORBLOB_VISION && - robot.vision != null && robot.vision.rawColorBlobVision != null) - { - if (pressed && colorThresholdMultiplier / 10.0 >= 1.0) - { - // Decrement the significant multiplier. - colorThresholdMultiplier /= 10.0; + // If we are moving swerve steering, make sure TeleOp doesn't interfere. + teleOpControlEnabled = !pressed; + if (pressed && robot.robotDrive != null && robot.robotDrive instanceof FtcSwerveDrive) + { + FtcSwerveDrive swerveDrive = (FtcSwerveDrive) robot.robotDrive; + swerveDrive.setSteerAngle(90.0, false, true); + } } passToTeleOp = false; } @@ -811,14 +751,6 @@ public class FtcTest extends FtcTeleOp } } //operatorButtonEvent - /** - * This method displays the current color thresholds on the dashboard. - */ - private void updateColorThresholds() - { - robot.dashboard.displayPrintf(7, "Thresholds: %s", Arrays.toString(colorThresholds)); - } //updateColorThresholds - /** * This method creates and displays the test menus and record the selected choices. */ @@ -827,37 +759,7 @@ public class FtcTest extends FtcTeleOp // // Create menus. // - testMenu = new FtcChoiceMenu<>("Tests:", null); - FtcValueMenu xTargetMenu = new FtcValueMenu( - "xTarget:", testMenu, -10.0, 10.0, 0.5, 0.0, " %.1f ft"); - FtcValueMenu yTargetMenu = new FtcValueMenu( - "yTarget:", testMenu, -10.0, 10.0, 0.5, 0.0, " %.1f ft"); - FtcValueMenu turnTargetMenu = new FtcValueMenu( - "turnTarget:", testMenu, -180.0, 180.0, 5.0, 0.0, " %.0f deg"); - FtcValueMenu driveTimeMenu = new FtcValueMenu( - "Drive time:", testMenu, 1.0, 10.0, 1.0, 4.0, " %.0f sec"); - FtcValueMenu drivePowerMenu = new FtcValueMenu( - "Drive power:", testMenu, -1.0, 1.0, 0.1, 0.5, " %.1f"); - // - // PID Tuning menus. - // - FtcValueMenu tuneKpMenu = new FtcValueMenu( - "Kp:", testMenu, 0.0, 1.0, 0.001, this::getTuneKp, " %f"); - FtcValueMenu tuneKiMenu = new FtcValueMenu( - "Ki:", tuneKpMenu, 0.0, 1.0, 0.001, this::getTuneKi, " %f"); - FtcValueMenu tuneKdMenu = new FtcValueMenu( - "Kd:", tuneKiMenu, 0.0, 1.0, 0.001, this::getTuneKd, " %f"); - FtcValueMenu tuneKfMenu = new FtcValueMenu( - "Kf:", tuneKdMenu, 0.0, 1.0, 0.001, this::getTuneKf, " %f"); - FtcValueMenu tuneDistanceMenu = new FtcValueMenu( - "PID Tune distance:", tuneKfMenu, -10.0, 10.0, 0.5, 0.0, - " %.1f ft"); - FtcValueMenu tuneHeadingMenu = new FtcValueMenu( - "PID Tune heading:", tuneDistanceMenu, -180.0, 180.0, 5.0, 0.0, - " %.0f deg"); - FtcValueMenu tuneDrivePowerMenu = new FtcValueMenu( - "PID Tune drive power:", tuneHeadingMenu, -1.0, 1.0, 0.1, 1.0, - " %.1f"); + FtcChoiceMenu testMenu = new FtcChoiceMenu<>("Tests:", null); // // Populate menus. // @@ -867,25 +769,14 @@ public class FtcTest extends FtcTeleOp testMenu.addChoice("Tune ColorBlob vision", Test.TUNE_COLORBLOB_VISION, false); testMenu.addChoice("Drive speed test", Test.DRIVE_SPEED_TEST, false); testMenu.addChoice("Drive motors test", Test.DRIVE_MOTORS_TEST, false); - testMenu.addChoice("X Timed drive", Test.X_TIMED_DRIVE, false, driveTimeMenu); - testMenu.addChoice("Y Timed drive", Test.Y_TIMED_DRIVE, false, driveTimeMenu); - testMenu.addChoice("PID drive", Test.PID_DRIVE, false, xTargetMenu); - testMenu.addChoice("Tune X PID", Test.TUNE_X_PID, false, tuneKpMenu); - testMenu.addChoice("Tune Y PID", Test.TUNE_Y_PID, false, tuneKpMenu); - testMenu.addChoice("Tune Turn PID", Test.TUNE_TURN_PID, false, tuneKpMenu); - testMenu.addChoice("Pure Pursuit Drive", Test.PURE_PURSUIT_DRIVE, false); + testMenu.addChoice("X Timed drive", Test.X_TIMED_DRIVE, false); + testMenu.addChoice("Y Timed drive", Test.Y_TIMED_DRIVE, false); + testMenu.addChoice("PID drive", Test.PID_DRIVE, false); + testMenu.addChoice("Pure Pursuit Drive", Test.PP_DRIVE, false); + testMenu.addChoice("Tune X PID", Test.TUNE_X_PID, false); + testMenu.addChoice("Tune Y PID", Test.TUNE_Y_PID, false); + testMenu.addChoice("Tune Turn PID", Test.TUNE_TURN_PID, false); testMenu.addChoice("Calibrate Swerve Steering", Test.CALIBRATE_SWERVE_STEERING, false); - - xTargetMenu.setChildMenu(yTargetMenu); - yTargetMenu.setChildMenu(turnTargetMenu); - turnTargetMenu.setChildMenu(drivePowerMenu); - driveTimeMenu.setChildMenu(drivePowerMenu); - tuneKpMenu.setChildMenu(tuneKiMenu); - tuneKiMenu.setChildMenu(tuneKdMenu); - tuneKdMenu.setChildMenu(tuneKfMenu); - tuneKfMenu.setChildMenu(tuneDistanceMenu); - tuneDistanceMenu.setChildMenu(tuneHeadingMenu); - tuneHeadingMenu.setChildMenu(tuneDrivePowerMenu); // // Traverse menus. // @@ -894,136 +785,12 @@ public class FtcTest extends FtcTeleOp // Fetch choices. // testChoices.test = testMenu.getCurrentChoiceObject(); - testChoices.xTarget = xTargetMenu.getCurrentValue(); - testChoices.yTarget = yTargetMenu.getCurrentValue(); - testChoices.turnTarget = turnTargetMenu.getCurrentValue(); - testChoices.driveTime = driveTimeMenu.getCurrentValue(); - testChoices.drivePower = drivePowerMenu.getCurrentValue(); - testChoices.tunePidCoeffs = new TrcPidController.PidCoefficients( - tuneKpMenu.getCurrentValue(), tuneKiMenu.getCurrentValue(), - tuneKdMenu.getCurrentValue(),tuneKfMenu.getCurrentValue()); - testChoices.tuneDistance = tuneDistanceMenu.getCurrentValue(); - testChoices.tuneHeading = tuneHeadingMenu.getCurrentValue(); - testChoices.tuneDrivePower = tuneDrivePowerMenu.getCurrentValue(); - - TrcPidController tunePidCtrl = getTunePidController(testChoices.test); - if (tunePidCtrl != null) - { - // - // Write the user input PID coefficients to a cache file so tune PID menu can read them as start value - // next time. - // - pidCoeffCache.writeCachedPidCoeff(tunePidCtrl, testChoices.tunePidCoeffs); - } // // Show choices. // robot.dashboard.displayPrintf(1, "Test Choices: %s", testChoices); } //doTestMenus - /** - * This method returns the PID controller for the tune test. - * - * @param test specifies the selected test. - * @return tune PID controller. - */ - private TrcPidController getTunePidController(Test test) - { - TrcPidController pidCtrl; - - switch (test) - { - case TUNE_X_PID: - pidCtrl = robot.robotDrive.pidDrive.getXPidCtrl(); - break; - - case TUNE_Y_PID: - pidCtrl = robot.robotDrive.pidDrive.getYPidCtrl(); - break; - - case TUNE_TURN_PID: - pidCtrl = robot.robotDrive.pidDrive.getTurnPidCtrl(); - break; - - default: - pidCtrl = null; - } - - return pidCtrl; - } //getTunePidController - - /** - * This method is called by the tuneKpMenu to get the start value to be displayed as the current value of the menu. - * - * @return start Kp value of the PID controller being tuned. - */ - private double getTuneKp() - { - double value = 0.0; - TrcPidController tunePidCtrl = getTunePidController(testMenu.getCurrentChoiceObject()); - - if (tunePidCtrl != null) - { - value = pidCoeffCache.getCachedPidCoeff(tunePidCtrl).kP; - } - - return value; - } //getTuneKp - - /** - * This method is called by the tuneKiMenu to get the start value to be displayed as the current value of the menu. - * - * @return start Ki value of the PID controller being tuned. - */ - private double getTuneKi() - { - double value = 0.0; - TrcPidController tunePidCtrl = getTunePidController(testMenu.getCurrentChoiceObject()); - - if (tunePidCtrl != null) - { - value = pidCoeffCache.getCachedPidCoeff(tunePidCtrl).kI; - } - - return value; - } //getTuneKi - - /** - * This method is called by the tuneKdMenu to get the start value to be displayed as the current value of the menu. - * - * @return start Kd value of the PID controller being tuned. - */ - private double getTuneKd() - { - double value = 0.0; - TrcPidController tunePidCtrl = getTunePidController(testMenu.getCurrentChoiceObject()); - - if (tunePidCtrl != null) - { - value = pidCoeffCache.getCachedPidCoeff(tunePidCtrl).kD; - } - - return value; - } //getTuneKd - - /** - * This method is called by the tuneKfMenu to get the start value to be displayed as the current value of the menu. - * - * @return start Kf value of the PID controller being tuned. - */ - double getTuneKf() - { - double value = 0.0; - TrcPidController tunePidCtrl = getTunePidController(testMenu.getCurrentChoiceObject()); - - if (tunePidCtrl != null) - { - value = pidCoeffCache.getCachedPidCoeff(tunePidCtrl).kF; - } - - return value; - } //getTuneKF - /** * 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 diff --git a/TeamCode/src/main/java/teamcode/Robot.java b/TeamCode/src/main/java/teamcode/Robot.java index 69f654e..acb59be 100644 --- a/TeamCode/src/main/java/teamcode/Robot.java +++ b/TeamCode/src/main/java/teamcode/Robot.java @@ -32,7 +32,7 @@ import ftclib.sensor.FtcRobotBattery; import teamcode.subsystems.LEDIndicator; import teamcode.subsystems.RobotBase; import teamcode.subsystems.RumbleIndicator; -import teamcode.subsystems.Vision; +import teamcode.vision.Vision; import trclib.motor.TrcMotor; import trclib.motor.TrcServo; import trclib.pathdrive.TrcPose2D; @@ -56,6 +56,7 @@ public class Robot private static TrcPose2D endOfAutoRobotPose = null; private static double nextStatusUpdateTime = 0.0; // Robot Drive. + public RobotBase robotBase; public FtcRobotDrive.RobotInfo robotInfo; public FtcRobotDrive robotDrive; // Vision subsystems. @@ -83,7 +84,7 @@ public class Robot nextStatusUpdateTime = TrcTimer.getCurrentTime(); speak("Init starting"); // Create and initialize Robot Base. - RobotBase robotBase = new RobotBase(); + robotBase = new RobotBase(); robotInfo = robotBase.getRobotInfo(); robotDrive = robotBase.getRobotDrive(); // Create and initialize vision subsystems. @@ -271,31 +272,6 @@ public class Robot } } //stopMode - /** - * This method update all subsystem status on the dashboard. - * - * @param startLineNum specifies the first Dashboard line for printing status. - */ - public void updateStatus(int startLineNum) - { - double currTime = TrcTimer.getCurrentTime(); - if (currTime > nextStatusUpdateTime) - { - int lineNum = startLineNum; - nextStatusUpdateTime = currTime + RobotParams.Robot.DASHBOARD_UPDATE_INTERVAL; - if (robotDrive != null) - { - dashboard.displayPrintf(lineNum++, "DriveBase: Pose=%s", robotDrive.driveBase.getFieldPosition()); - } - // - // Display other subsystem status here. - // - if (RobotParams.Preferences.showSubsystems) - { - } - } - } //updateStatus - /** * This method is called to cancel all pending operations and release the ownership of all subsystems. */ diff --git a/TeamCode/src/main/java/teamcode/RobotParams.java b/TeamCode/src/main/java/teamcode/RobotParams.java index 7e7f049..a4560b8 100644 --- a/TeamCode/src/main/java/teamcode/RobotParams.java +++ b/TeamCode/src/main/java/teamcode/RobotParams.java @@ -50,7 +50,10 @@ public class RobotParams public static final boolean useBatteryMonitor = false; // Driver feedback // Status Update: Status Update may affect robot loop time, don't do it when in competition. - public static final boolean doStatusUpdate = !inCompetition; + public static final boolean updateDashboard = !inCompetition; + public static final boolean showDriveBase = false; + public static final boolean showPidDrive = false; + public static final boolean showVision = false; public static final boolean showSubsystems = true; public static final boolean useBlinkinLED = false; public static final boolean useGobildaLED = false; @@ -59,11 +62,11 @@ public class RobotParams public static final boolean useVision = false; public static final boolean useWebCam = false; // false to use Android phone camera. public static final boolean useBuiltinCamBack = false; // For Android Phone as Robot Controller. - public static final boolean tuneColorBlobVision = false; public static final boolean useLimelightVision = false; public static final boolean useCameraStreamProcessor = false; public static final boolean useWebcamAprilTagVision = false; public static final boolean useColorBlobVision = false; + public static final boolean useSolvePnp = false; public static final boolean showVisionView = !inCompetition; public static final boolean showVisionStat = false; // Drive Base @@ -72,6 +75,9 @@ public class RobotParams public static final boolean useSparkfunOTOS = false; // Subsystems public static final boolean useSubsystems = false; + // Tuning + public static final boolean tuneColorBlobVision = false; + public static final boolean tuneDriveBase = false; } //class Preferences /** diff --git a/TeamCode/src/main/java/teamcode/subsystems/RobotBase.java b/TeamCode/src/main/java/teamcode/subsystems/RobotBase.java index 3402493..5d31b18 100644 --- a/TeamCode/src/main/java/teamcode/subsystems/RobotBase.java +++ b/TeamCode/src/main/java/teamcode/subsystems/RobotBase.java @@ -32,9 +32,10 @@ import ftclib.motor.FtcMotorActuator; import ftclib.sensor.FtcPinpointOdometry; import ftclib.sensor.FtcSparkFunOtos; import teamcode.RobotParams; +import teamcode.vision.Vision; +import trclib.controller.TrcPidController; import trclib.dataprocessor.TrcUtil; import trclib.drivebase.TrcDriveBase; -import trclib.robotcore.TrcPidController; /** * This class creates the appropriate Robot Drive Base according to the specified robot type. @@ -96,9 +97,11 @@ public class RobotBase // Robot Drive Characteristics robotMaxVelocity = 80.0; // inches/sec robotMaxAcceleration = 350.0; // inches/sec2 + robotMaxDeceleration = 300.0; // inches/sec2 robotMaxTurnRate = 80.0; // degrees/sec profiledMaxVelocity = robotMaxVelocity; profiledMaxAcceleration = robotMaxAcceleration; + profiledMaxDeceleration = robotMaxDeceleration; profiledMaxTurnRate = robotMaxTurnRate; // DriveBase PID Parameters drivePidTolerance = 2.0; @@ -111,9 +114,15 @@ public class RobotBase turnMaxPidRampRate = null; // PID Stall Detection pidStallDetectionEnabled = true; + // PidDrive Parameters + usePidDrive = true; + enablePidDriveSquid = true; // PurePursuit Parameters + usePurePursuitDrive = true; + enablePurePursuitDriveSquid = true; ppdFollowingDistance = 6.0; velPidCoeffs = new TrcPidController.PidCoefficients(0.0, 0.0, 0.0, 1.0/profiledMaxVelocity, 0.0); + fastModeEnabled = true; } //DifferentialParams } //class DifferentialParams @@ -123,44 +132,41 @@ public class RobotBase public static class MecanumParams extends FtcRobotDrive.RobotInfo { // Optii Odometry Wheel - private static final double ODWHEEL_DIAMETER = 35.0 * TrcUtil.INCHES_PER_MM; + private static final double ODWHEEL_DIAMETER_MM = 35.0; + private static final double ODWHEEL_DIAMETER = ODWHEEL_DIAMETER_MM*TrcUtil.INCHES_PER_MM; private static final double ODWHEEL_CPR = 4096.0; public MecanumParams() { robotName = "MecanumRobot"; - // Robot Dimensions + // Robot Dimensions (measured from CAD model if possible) robotLength = RobotParams.Robot.ROBOT_LENGTH; robotWidth = RobotParams.Robot.ROBOT_WIDTH; - wheelBaseLength = (24.0 * 14)*TrcUtil.INCHES_PER_MM; - wheelBaseWidth = 16.0; - // IMU - imuName = "imu"; - hubLogoDirection = RevHubOrientationOnRobot.LogoFacingDirection.UP; - hubUsbDirection = RevHubOrientationOnRobot.UsbFacingDirection.FORWARD; + wheelBaseLength = 360.0 * TrcUtil.INCHES_PER_MM; + wheelBaseWidth = (312.0 + 103.623) * TrcUtil.INCHES_PER_MM; + // IMU (not used if using AbsoluteOdometry). + imuName = null; + hubLogoDirection = RevHubOrientationOnRobot.LogoFacingDirection.UP; // Control Hub orientation + hubUsbDirection = RevHubOrientationOnRobot.UsbFacingDirection.FORWARD; // Control Hub orientation // Drive Motors driveMotorType = FtcMotorActuator.MotorType.DcMotor; driveMotorNames = new String[] {"lfDriveMotor", "rfDriveMotor", "lbDriveMotor", "rbDriveMotor"}; driveMotorInverted = new boolean[] {true, false, true, false}; odometryType = TrcDriveBase.OdometryType.AbsoluteOdometry; - // Odometry Wheels + // Odometry Wheels (Offset from wheel base center) odWheelXScale = odWheelYScale = Math.PI * ODWHEEL_DIAMETER / ODWHEEL_CPR; - xOdWheelSensorNames = new String[] {"xOdWheelSensor"}; - xOdWheelIndices = new int[] {0}; - xOdWheelXOffsets = new double[] {0.0}; - xOdWheelYOffsets = new double[] {-168.0 * TrcUtil.INCHES_PER_MM}; - yOdWheelSensorNames = new String[] {"yLeftOdWheelSensor", "yRightOdWheelSensor"}; - yOdWheelIndices = new int[] {1, 2}; - yOdWheelXOffsets = new double[] {-144.0 * TrcUtil.INCHES_PER_MM, 144.0 * TrcUtil.INCHES_PER_MM}; - yOdWheelYOffsets = new double[] {-12.0 * TrcUtil.INCHES_PER_MM, -12.0 * TrcUtil.INCHES_PER_MM}; + xOdWheelXOffsets = new double[] {4.0 * TrcUtil.INCHES_PER_MM}; + xOdWheelYOffsets = new double[] {-132.0 * TrcUtil.INCHES_PER_MM}; + yOdWheelXOffsets = new double[] {-156.0 * TrcUtil.INCHES_PER_MM, 156.0 * TrcUtil.INCHES_PER_MM}; + yOdWheelYOffsets = new double[] {-4.0 * TrcUtil.INCHES_PER_MM, -4.0 * TrcUtil.INCHES_PER_MM}; // Absolute Odometry if (odometryType == TrcDriveBase.OdometryType.AbsoluteOdometry) { if (RobotParams.Preferences.usePinpointOdometry) { FtcPinpointOdometry.Config ppOdoConfig = new FtcPinpointOdometry.Config() - .setPodOffsets(-144.0, -168.0) - .setEncoderResolution(ODWHEEL_CPR / Math.PI * ODWHEEL_DIAMETER) + .setPodOffsets(-156.0, -131.4) // Offsets from robot center in mm + .setEncoderResolution(ODWHEEL_CPR / (Math.PI * ODWHEEL_DIAMETER_MM)) .setEncodersInverted(false, false); absoluteOdometry = new FtcPinpointOdometry("pinpointOdo", ppOdoConfig); headingWrapRangeLow = -180.0; @@ -169,8 +175,8 @@ public class RobotBase else if (RobotParams.Preferences.useSparkfunOTOS) { FtcSparkFunOtos.Config otosConfig = new FtcSparkFunOtos.Config() - .setOffset(0.0, -12.0 * TrcUtil.INCHES_PER_MM, 0.0) - .setScale(1.0, 1.0); + .setOffset(-4.0 * TrcUtil.INCHES_PER_MM, 24.0 * TrcUtil.INCHES_PER_MM, 0.0) + .setScale(1.0, 1.0); //??? absoluteOdometry = new FtcSparkFunOtos("sparkfunOtos", otosConfig); } } @@ -178,39 +184,48 @@ public class RobotBase { absoluteOdometry = null; } - // Drive Motor Odometry + // Drive Motor Odometry only (not used). xDrivePosScale = 1.0; // in/count yDrivePosScale = 1.0; // in/count // Robot Drive Characteristics robotMaxVelocity = 80.0; // inches/sec robotMaxAcceleration = 350.0; // inches/sec2 + robotMaxDeceleration = 300.0; robotMaxTurnRate = 80.0; // degrees/sec profiledMaxVelocity = robotMaxVelocity; profiledMaxAcceleration = robotMaxAcceleration; + profiledMaxDeceleration = robotMaxDeceleration; profiledMaxTurnRate = robotMaxTurnRate; // DriveBase PID Parameters drivePidTolerance = 1.0; turnPidTolerance = 1.0; - xDrivePidCoeffs = new TrcPidController.PidCoefficients(0.95, 0.0, 0.001, 0.0, 0.0); + xDrivePidCoeffs = new TrcPidController.PidCoefficients(0.072, 0.001, 0.0065, 0.0, 2.0); xDrivePidPowerLimit = 1.0; xDriveMaxPidRampRate = null; - yDrivePidCoeffs = new TrcPidController.PidCoefficients(0.06, 0.0, 0.002, 0.0, 0.0); + yDrivePidCoeffs = new TrcPidController.PidCoefficients(0.02, 0.00175, 0.002, 0.0, 2.0); yDrivePidPowerLimit = 1.0; yDriveMaxPidRampRate = null; - turnPidCoeffs = new TrcPidController.PidCoefficients(0.02, 0.0, 0.002, 0.0, 0.0); + turnPidCoeffs = new TrcPidController.PidCoefficients(0.032, 0.1, 0.0025, 0.0, 5.0); turnPidPowerLimit = 0.5; turnMaxPidRampRate = null; // PID Stall Detection pidStallDetectionEnabled = true; - // PurePursuit Parameters + // PidDrive Parameters + usePidDrive = true; + enablePidDriveSquid = true; + // PurePursuit Parameters. + usePurePursuitDrive = true; + enablePurePursuitDriveSquid = true; ppdFollowingDistance = 6.0; velPidCoeffs = new TrcPidController.PidCoefficients(0.0, 0.0, 0.0, 1.0/profiledMaxVelocity, 0.0); + fastModeEnabled = true; // Vision webCam1 = new Vision.FrontCamParams(); webCam2 = new Vision.BackCamParams(); limelight = new Vision.LimelightParams(); // Miscellaneous - indicatorName = "blinkin"; + indicatorName = RobotParams.Preferences.useBlinkinLED? "blinkin": + RobotParams.Preferences.useGobildaLED? "gobildaLED": null; } //MecanumParams } //class MecanumParams @@ -220,44 +235,41 @@ public class RobotBase public static class SwerveParams extends FtcSwerveDrive.SwerveInfo { // Optii Odometry Wheel - private static final double ODWHEEL_DIAMETER = 35.0 * TrcUtil.INCHES_PER_MM; + private static final double ODWHEEL_DIAMETER_MM = 35.0; + private static final double ODWHEEL_DIAMETER = ODWHEEL_DIAMETER_MM*TrcUtil.INCHES_PER_MM; private static final double ODWHEEL_CPR = 4096.0; public SwerveParams() { robotName = "SwerveRobot"; - // Robot Dimensions + // Robot Dimensions (measured from CAD model if possible) robotLength = RobotParams.Robot.ROBOT_LENGTH; robotWidth = RobotParams.Robot.ROBOT_WIDTH; wheelBaseLength = (24.0 * 14)*TrcUtil.INCHES_PER_MM; wheelBaseWidth = 16.0; - // IMU - imuName = "imu"; - hubLogoDirection = RevHubOrientationOnRobot.LogoFacingDirection.UP; - hubUsbDirection = RevHubOrientationOnRobot.UsbFacingDirection.FORWARD; + // IMU (not used if using AbsoluteOdometry). + imuName = null; + hubLogoDirection = RevHubOrientationOnRobot.LogoFacingDirection.UP; // Control Hub orientation + hubUsbDirection = RevHubOrientationOnRobot.UsbFacingDirection.FORWARD; // Control Hub orientation // Drive Motors driveMotorType = FtcMotorActuator.MotorType.DcMotor; driveMotorNames = new String[] {"lfDriveMotor", "rfDriveMotor", "lbDriveMotor", "rbDriveMotor"}; driveMotorInverted = new boolean[] {true, false, true, false}; - odometryType = TrcDriveBase.OdometryType.OdometryWheels; - // Odometry Wheels + odometryType = TrcDriveBase.OdometryType.AbsoluteOdometry; + // Odometry Wheels (Offset from wheel base center) odWheelXScale = odWheelYScale = Math.PI * ODWHEEL_DIAMETER / ODWHEEL_CPR; - xOdWheelSensorNames = null; - xOdWheelIndices = new int[] {FtcRobotDrive.INDEX_RIGHT_BACK}; - xOdWheelXOffsets = new double[] {0.0}; - xOdWheelYOffsets = new double[] {-168.0 * TrcUtil.INCHES_PER_MM}; - yOdWheelSensorNames = null; - yOdWheelIndices = new int[] {FtcRobotDrive.INDEX_LEFT_FRONT, FtcRobotDrive.INDEX_RIGHT_FRONT}; - yOdWheelXOffsets = new double[] {-144.0 * TrcUtil.INCHES_PER_MM, 144.0 * TrcUtil.INCHES_PER_MM}; - yOdWheelYOffsets = new double[] {-12.0 * TrcUtil.INCHES_PER_MM, -12.0 * TrcUtil.INCHES_PER_MM}; + xOdWheelXOffsets = new double[] {4.0 * TrcUtil.INCHES_PER_MM}; + xOdWheelYOffsets = new double[] {-132.0 * TrcUtil.INCHES_PER_MM}; + yOdWheelXOffsets = new double[] {-156.0 * TrcUtil.INCHES_PER_MM, 156.0 * TrcUtil.INCHES_PER_MM}; + yOdWheelYOffsets = new double[] {-4.0 * TrcUtil.INCHES_PER_MM, -4.0 * TrcUtil.INCHES_PER_MM}; // Absolute Odometry if (odometryType == TrcDriveBase.OdometryType.AbsoluteOdometry) { if (RobotParams.Preferences.usePinpointOdometry) { FtcPinpointOdometry.Config ppOdoConfig = new FtcPinpointOdometry.Config() - .setPodOffsets(-144.0, -168.0) - .setEncoderResolution(ODWHEEL_CPR / Math.PI * ODWHEEL_DIAMETER) + .setPodOffsets(-156.0, -131.4) // Offsets from robot center in mm + .setEncoderResolution(ODWHEEL_CPR / (Math.PI * ODWHEEL_DIAMETER_MM)) .setEncodersInverted(false, false); absoluteOdometry = new FtcPinpointOdometry("pinpointOdo", ppOdoConfig); headingWrapRangeLow = -180.0; @@ -266,8 +278,8 @@ public class RobotBase else if (RobotParams.Preferences.useSparkfunOTOS) { FtcSparkFunOtos.Config otosConfig = new FtcSparkFunOtos.Config() - .setOffset(0.0, -12.0 * TrcUtil.INCHES_PER_MM, 0.0) - .setScale(1.0, 1.0); + .setOffset(-4.0 * TrcUtil.INCHES_PER_MM, 24.0 * TrcUtil.INCHES_PER_MM, 0.0) + .setScale(1.0, 1.0); //??? absoluteOdometry = new FtcSparkFunOtos("sparkfunOtos", otosConfig); } } @@ -275,36 +287,48 @@ public class RobotBase { absoluteOdometry = null; } - // Drive Motor Odometry + // Drive Motor Odometry only (not used). xDrivePosScale = 1.0; // in/count yDrivePosScale = 1.0; // in/count // Robot Drive Characteristics robotMaxVelocity = 80.0; // inches/sec robotMaxAcceleration = 350.0; // inches/sec2 + robotMaxDeceleration = 300.0; robotMaxTurnRate = 80.0; // degrees/sec profiledMaxVelocity = robotMaxVelocity; profiledMaxAcceleration = robotMaxAcceleration; + profiledMaxDeceleration = robotMaxDeceleration; profiledMaxTurnRate = robotMaxTurnRate; // DriveBase PID Parameters drivePidTolerance = 1.0; turnPidTolerance = 1.0; - xDrivePidCoeffs = yDrivePidCoeffs = new TrcPidController.PidCoefficients(0.95, 0.0, 0.001, 0.0, 0.0); - xDrivePidPowerLimit = yDrivePidPowerLimit = 1.0; - xDriveMaxPidRampRate = yDriveMaxPidRampRate = null; - turnPidCoeffs = new TrcPidController.PidCoefficients(0.02, 0.0, 0.002, 0.0, 0.0); + xDrivePidCoeffs = new TrcPidController.PidCoefficients(0.072, 0.001, 0.0065, 0.0, 2.0); + xDrivePidPowerLimit = 1.0; + xDriveMaxPidRampRate = null; + yDrivePidCoeffs = new TrcPidController.PidCoefficients(0.02, 0.00175, 0.002, 0.0, 2.0); + yDrivePidPowerLimit = 1.0; + yDriveMaxPidRampRate = null; + turnPidCoeffs = new TrcPidController.PidCoefficients(0.032, 0.1, 0.0025, 0.0, 5.0); turnPidPowerLimit = 0.5; turnMaxPidRampRate = null; // PID Stall Detection pidStallDetectionEnabled = true; - // PurePursuit Parameters + // PidDrive Parameters + usePidDrive = true; + enablePidDriveSquid = true; + // PurePursuit Parameters. + usePurePursuitDrive = true; + enablePurePursuitDriveSquid = true; ppdFollowingDistance = 6.0; velPidCoeffs = new TrcPidController.PidCoefficients(0.0, 0.0, 0.0, 1.0/profiledMaxVelocity, 0.0); + fastModeEnabled = true; // Vision webCam1 = new Vision.FrontCamParams(); webCam2 = new Vision.BackCamParams(); limelight = new Vision.LimelightParams(); // Miscellaneous - indicatorName = "blinkin"; + indicatorName = RobotParams.Preferences.useBlinkinLED? "blinkin": + RobotParams.Preferences.useGobildaLED? "gobildaLED": null; // Steer Encoders steerEncoderNames = new String[] {"lfSteerEncoder", "rfSteerEncoder", "lbSteerEncoder", "rbSteerEncoder"}; steerEncoderInverted = new boolean[] {false, false, false, false}; @@ -378,4 +402,31 @@ public class RobotBase return robotDrive; } //getRobotDrive + /** + * This method update the dashboard with the drive base status. + * + * @param lineNum specifies the starting line number to print the subsystem status. + * @return updated line number for the next subsystem to print. + */ + public int updateStatus(int lineNum) + { + if (robotDrive != null) + { + if (RobotParams.Preferences.showPidDrive) + { + TrcPidController xPidCtrl = robotDrive.pidDrive.getXPidCtrl(); + if (xPidCtrl != null) + { + xPidCtrl.displayPidInfo(lineNum); + lineNum += 2; + } + robotDrive.pidDrive.getYPidCtrl().displayPidInfo(lineNum); + lineNum += 2; + robotDrive.pidDrive.getTurnPidCtrl().displayPidInfo(lineNum); + lineNum += 2; + } + } + return lineNum; + } //updateStatus + } //class RobotBase diff --git a/TeamCode/src/main/java/teamcode/autotasks/TaskAuto.java b/TeamCode/src/main/java/teamcode/tasks/TaskAuto.java similarity index 70% rename from TeamCode/src/main/java/teamcode/autotasks/TaskAuto.java rename to TeamCode/src/main/java/teamcode/tasks/TaskAuto.java index e57fd95..ce54b70 100644 --- a/TeamCode/src/main/java/teamcode/autotasks/TaskAuto.java +++ b/TeamCode/src/main/java/teamcode/tasks/TaskAuto.java @@ -20,7 +20,7 @@ * SOFTWARE. */ -package teamcode.autotasks; +package teamcode.tasks; import androidx.annotation.NonNull; @@ -57,34 +57,30 @@ public class TaskAuto extends TrcAutoTask } //toString } //class TaskParams - private final String ownerName; private final Robot robot; - private String currOwner = null; - /** * Constructor: Create an instance of the object. * - * @param ownerName specifies the owner name to take subsystem ownership, can be null if no ownership required. * @param robot specifies the robot object that contains all the necessary subsystems. */ - public TaskAuto(String ownerName, Robot robot) + public TaskAuto(Robot robot) { - super(moduleName, ownerName, TrcTaskMgr.TaskType.POST_PERIODIC_TASK); - this.ownerName = ownerName; + super(moduleName, TrcTaskMgr.TaskType.POST_PERIODIC_TASK); this.robot = robot; } //TaskAuto /** * This method starts the auto-assist operation. * + * @param owner specifies the owner to acquire subsystem ownerships, can be null if not requiring ownership. * @param completionEvent specifies the event to signal when done, can be null if none provided. */ - public void autoAssist(TrcEvent completionEvent) + public void autoAssist(String owner, TrcEvent completionEvent) { TaskParams taskParams = new TaskParams(); - tracer.traceInfo(moduleName, "taskParams=(" + taskParams + ", event=" + completionEvent); - startAutoTask(State.START, taskParams, completionEvent); + tracer.traceInfo(moduleName, "owner=" + owner + ", taskParams=(" + taskParams + ", event=" + completionEvent); + startAutoTask(owner, State.START, taskParams, completionEvent); } //autoAssist // @@ -95,65 +91,55 @@ public class TaskAuto extends TrcAutoTask * This method is called by the super class to acquire ownership of all subsystems involved in the auto-assist * operation. This is typically done before starting an auto-assist operation. * + * @param owner specifies the owner to acquire the subsystem ownerships. * @return true if acquired all subsystems ownership, false otherwise. It releases all ownership if any acquire * failed. */ @Override - protected boolean acquireSubsystemsOwnership() + protected boolean acquireSubsystemsOwnership(String owner) { - boolean success = ownerName == null || - (robot.robotDrive.driveBase.acquireExclusiveAccess(ownerName)); - - if (success) - { - currOwner = ownerName; - tracer.traceInfo(moduleName, "Successfully acquired subsystem ownerships."); - } - else - { - TrcOwnershipMgr ownershipMgr = TrcOwnershipMgr.getInstance(); - tracer.traceWarn( - moduleName, - "Failed to acquire subsystem ownership (currOwner=" + currOwner + - ", robotDrive=" + ownershipMgr.getOwner(robot.robotDrive.driveBase) + ")."); - releaseSubsystemsOwnership(); - } - - return success; + // Call each subsystem.acquireExclusiveAccess(owner) and return true only if all acquires returned true. + // For example: + // return subsystem1.acquireExclusiveAccess(owner) && subsystem2.acquireExclusiveAccess(owner) ... + return robot.robotDrive.driveBase.acquireExclusiveAccess(owner); } //acquireSubsystemsOwnership /** * This method is called by the super class to release ownership of all subsystems involved in the auto-assist * operation. This is typically done if the auto-assist operation is completed or canceled. + * + * @param owner specifies the owner that acquired the subsystem ownerships. */ @Override - protected void releaseSubsystemsOwnership() + protected void releaseSubsystemsOwnership(String owner) { - if (ownerName != null) + if (owner != null) { TrcOwnershipMgr ownershipMgr = TrcOwnershipMgr.getInstance(); tracer.traceInfo( moduleName, - "Releasing subsystem ownership (currOwner=" + currOwner + - ", robotDrive=" + ownershipMgr.getOwner(robot.robotDrive.driveBase) + ")."); - robot.robotDrive.driveBase.releaseExclusiveAccess(currOwner); - currOwner = null; + "Releasing subsystem ownership on behalf of " + owner + + ", robotDrive=" + ownershipMgr.getOwner(robot.robotDrive.driveBase)); + robot.robotDrive.driveBase.releaseExclusiveAccess(owner); } } //releaseSubsystemsOwnership /** * This method is called by the super class to stop all the subsystems. + * + * @param owner specifies the owner that acquired the subsystem ownerships. */ @Override - protected void stopSubsystems() + protected void stopSubsystems(String owner) { tracer.traceInfo(moduleName, "Stopping subsystems."); - robot.robotDrive.cancel(currOwner); + robot.robotDrive.cancel(owner); } //stopSubsystems /** * This methods is called periodically to run the auto-assist task. * + * @param owner specifies the owner that acquired the subsystem ownerships. * @param params specifies the task parameters. * @param state specifies the current state of the task. * @param taskType specifies the type of task being run. @@ -163,7 +149,8 @@ public class TaskAuto extends TrcAutoTask */ @Override protected void runTaskState( - Object params, State state, TrcTaskMgr.TaskType taskType, TrcRobot.RunMode runMode, boolean slowPeriodicLoop) + String owner, Object params, State state, TrcTaskMgr.TaskType taskType, TrcRobot.RunMode runMode, + boolean slowPeriodicLoop) { TaskParams taskParams = (TaskParams) params; diff --git a/TeamCode/src/main/java/teamcode/subsystems/Vision.java b/TeamCode/src/main/java/teamcode/vision/Vision.java similarity index 91% rename from TeamCode/src/main/java/teamcode/subsystems/Vision.java rename to TeamCode/src/main/java/teamcode/vision/Vision.java index df49c32..02a252c 100644 --- a/TeamCode/src/main/java/teamcode/subsystems/Vision.java +++ b/TeamCode/src/main/java/teamcode/vision/Vision.java @@ -20,7 +20,7 @@ * SOFTWARE. */ -package teamcode.subsystems; +package teamcode.vision; import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; @@ -28,6 +28,9 @@ import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.vision.VisionProcessor; import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; +import org.opencv.core.CvType; +import org.opencv.core.Mat; +import org.opencv.core.MatOfDouble; import org.opencv.imgproc.Imgproc; import org.openftc.easyopencv.OpenCvCamera; import org.openftc.easyopencv.OpenCvCameraFactory; @@ -45,8 +48,10 @@ import ftclib.vision.FtcRawEocvVision; import ftclib.vision.FtcVision; import ftclib.vision.FtcVisionAprilTag; import ftclib.vision.FtcVisionEocvColorBlob; +import teamcode.Dashboard; import teamcode.Robot; import teamcode.RobotParams; +import teamcode.subsystems.LEDIndicator; import trclib.dataprocessor.TrcUtil; import trclib.pathdrive.TrcPose2D; import trclib.pathdrive.TrcPose3D; @@ -167,7 +172,7 @@ public class Vision private static final int colorConversion = Imgproc.COLOR_RGB2YCrCb; 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 = + public static final TrcOpenCvColorBlobPipeline.FilterContourParams colorBlobFilterContourParams = new TrcOpenCvColorBlobPipeline.FilterContourParams() .setMinArea(500.0) .setMinPerimeter(100.0) @@ -176,15 +181,14 @@ 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 static final double objectWidth = 3.5; + private static final double objectHeight = 1.5; + // Logitech C920 + private static final double fx = 622.001; + private static final double fy = 622.001; + private static final double cx = 319.803; + private static final double cy = 241.251; + private static final MatOfDouble distCoeffs = new MatOfDouble(0.1208, -0.261599, 0, 0, 0.10308, 0, 0, 0); private final TrcDbgTrace tracer; private final Robot robot; @@ -222,6 +226,12 @@ public class Vision opMode.hardwareMap.get(WebcamName.class, robot.robotInfo.webCam1.camName): null; webcam2 = robot.robotInfo.webCam2 != null? opMode.hardwareMap.get(WebcamName.class, robot.robotInfo.webCam2.camName): null; + Mat cameraMatrix = new Mat(3, 3, CvType.CV_64FC1); + cameraMatrix.put(0, 0, + fx, 0, cx, + 0, fy, cy, + 0, 0, 1); + // TuneColorBlobVision: must use webcam1. if (RobotParams.Preferences.tuneColorBlobVision && webcam1 != null) { OpenCvCamera openCvCamera; @@ -237,14 +247,16 @@ public class Vision openCvCamera = OpenCvCameraFactory.getInstance().createWebcam(webcam1); } -// if (RobotParams.Preferences.useCameraStreamProcessor) -// { -// FtcDashboard.getInstance().startCameraStream(openCvCamera, 0); -// } + if (RobotParams.Preferences.useCameraStreamProcessor) + { + com.acmerobotics.dashboard.FtcDashboard.getInstance().startCameraStream(openCvCamera, 0); + } tracer.traceInfo(moduleName, "Starting RawEocvColorBlobVision..."); rawColorBlobPipeline = new FtcRawEocvColorBlobPipeline( - "rawColorBlobPipeline", colorConversion, redBlobColorThresholds, tuneFilterContourParams, true); + "rawColorBlobPipeline", colorConversion, Dashboard.Vision.colorThresholds, + Dashboard.Vision.filterContourParams, true, objectWidth, objectHeight, + RobotParams.Preferences.useSolvePnp? cameraMatrix: null, distCoeffs, robot.robotInfo.webCam1.camPose); // By default, display original Mat. rawColorBlobPipeline.setVideoOutput(0); rawColorBlobPipeline.setAnnotateEnabled(true); @@ -257,6 +269,7 @@ public class Vision } else { + // LimelightVision (not a Vision Processor). if (RobotParams.Preferences.useLimelightVision && robot.robotInfo.limelight != null) { limelightVision = new FtcLimelightVision( @@ -270,7 +283,7 @@ public class Vision { cameraStreamProcessor = new FtcCameraStreamProcessor(); visionProcessorsList.add(cameraStreamProcessor); -// FtcDashboard.getInstance().startCameraStream(cameraStreamProcessor, 0); + com.acmerobotics.dashboard.FtcDashboard.getInstance().startCameraStream(cameraStreamProcessor, 0); } if (RobotParams.Preferences.useWebcamAprilTagVision) @@ -289,17 +302,34 @@ public class Vision if (RobotParams.Preferences.useColorBlobVision && robot.robotInfo.webCam1 != null) { - tracer.traceInfo(moduleName, "Starting Webcam ColorBlobVision..."); + Mat camMatrix; + TrcHomographyMapper.Rectangle camRect, worldRect; + if (RobotParams.Preferences.useSolvePnp) + { + camMatrix = cameraMatrix; + camRect = null; + worldRect = null; + } + else + { + camMatrix = null; + camRect = robot.robotInfo.webCam1.cameraRect; + worldRect = robot.robotInfo.webCam1.worldRect; + } + + tracer.traceInfo(moduleName, "Starting Webcam ColorBlobVision..."); redBlobVision = new FtcVisionEocvColorBlob( LEDIndicator.RED_BLOB, colorConversion, redBlobColorThresholds, colorBlobFilterContourParams, - true, robot.robotInfo.webCam1.cameraRect, robot.robotInfo.webCam1.worldRect, true); + true, objectWidth, objectHeight, camMatrix, distCoeffs, robot.robotInfo.webCam1.camPose, camRect, + worldRect, true); redBlobProcessor = redBlobVision.getVisionProcessor(); visionProcessorsList.add(redBlobProcessor); blueBlobVision = new FtcVisionEocvColorBlob( LEDIndicator.BLUE_BLOB, colorConversion, blueBlobColorThresholds, colorBlobFilterContourParams, - true, robot.robotInfo.webCam1.cameraRect, robot.robotInfo.webCam1.worldRect, true); + true, objectWidth, objectHeight, camMatrix, distCoeffs, robot.robotInfo.webCam1.camPose, camRect, + worldRect, true); blueBlobProcessor = blueBlobVision.getVisionProcessor(); visionProcessorsList.add(blueBlobProcessor); } @@ -899,11 +929,11 @@ public class Vision { double offset = 0.0; - if ( resultType == FtcLimelightVision.ResultType.Fiducial) + if (resultType == FtcLimelightVision.ResultType.Fiducial) { offset = 5.75; } - else if ( resultType == FtcLimelightVision.ResultType.Python) + else if (resultType == FtcLimelightVision.ResultType.Python) { offset = 10.0; } @@ -926,4 +956,40 @@ public class Vision return (int)((a.objPose.y - b.objPose.y)*100); } //compareDistance + /** + * This method update the dashboard with vision status. + * + * @param lineNum specifies the starting line number to print the subsystem status. + * @return updated line number for the next subsystem to print. + */ + public int updateStatus(int lineNum) + { + if (rawColorBlobVision != null) + { + lineNum = rawColorBlobVision.updateStatus(lineNum); + } + + if (limelightVision != null) + { + lineNum = limelightVision.updateStatus(lineNum); + } + + if (aprilTagVision != null) + { + lineNum = aprilTagVision.updateStatus(lineNum); + } + + if (redBlobVision != null) + { + lineNum = redBlobVision.updateStatus(lineNum); + } + + if (blueBlobVision != null) + { + lineNum = blueBlobVision.updateStatus(lineNum); + } + + return lineNum; + } //updateStatus + } //class Vision diff --git a/TeamCode/src/main/java/trclib b/TeamCode/src/main/java/trclib index 9cd282a..1d053fe 160000 --- a/TeamCode/src/main/java/trclib +++ b/TeamCode/src/main/java/trclib @@ -1 +1 @@ -Subproject commit 9cd282a764f0372150c85f6a753c59d6740f812c +Subproject commit 1d053febdd45da8ef7c8d6a1c0d99b52a916c550 diff --git a/build.dependencies.gradle b/build.dependencies.gradle index a2b4ea1..6ec75ac 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -1,6 +1,7 @@ repositories { mavenCentral() google() // Needed for androidx + maven { url = 'https://maven.brott.dev/' } } dependencies { @@ -13,5 +14,6 @@ dependencies { implementation 'org.firstinspires.ftc:FtcCommon:10.1.0' implementation 'org.firstinspires.ftc:Vision:10.1.0' implementation 'androidx.appcompat:appcompat:1.2.0' + implementation 'com.acmerobotics.dashboard:dashboard:0.4.16' }