From 0869026b76961be787e44875850a725785dbb435 Mon Sep 17 00:00:00 2001 From: Titan Robotics Club Date: Fri, 25 Apr 2025 20:43:46 -0700 Subject: [PATCH] Sync'd the latest changes from 2025 season. Moved all auto tasks from autotasks to tasks. Added new Dashboard class. Added solvePnP support to vision. Added updateStatus to vision. Added updateStatus too FtcAuto, FtcTeleOp. Removed updateStatus from Robot.java. Added asymetric motion profile support to PurePursuit. Added updateStatus to RobotBase. Added solvePnp support for vision. Updated trclib, ftclib. Added acme FtcDashboard support. Added support to tune using acme FtcDashboard. Added maxDeceleration support in SpeedTest. --- TeamCode/src/main/java/ftclib | 2 +- .../src/main/java/teamcode/Dashboard.java | 112 +++ TeamCode/src/main/java/teamcode/FtcAuto.java | 5 + .../src/main/java/teamcode/FtcTeleOp.java | 12 +- TeamCode/src/main/java/teamcode/FtcTest.java | 651 ++++++------------ TeamCode/src/main/java/teamcode/Robot.java | 30 +- .../src/main/java/teamcode/RobotParams.java | 10 +- .../java/teamcode/subsystems/RobotBase.java | 161 +++-- .../{autotasks => tasks}/TaskAuto.java | 67 +- .../{subsystems => vision}/Vision.java | 110 ++- TeamCode/src/main/java/trclib | 2 +- build.dependencies.gradle | 2 + 12 files changed, 568 insertions(+), 596 deletions(-) create mode 100644 TeamCode/src/main/java/teamcode/Dashboard.java rename TeamCode/src/main/java/teamcode/{autotasks => tasks}/TaskAuto.java (70%) rename TeamCode/src/main/java/teamcode/{subsystems => vision}/Vision.java (91%) 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' }