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/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

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
 
     /**

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;
+    protected boolean driverAltFunc = false;
-    private boolean operatorAltFunc = 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;

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 logEvents = false;
-    private static final boolean debugPid = true;
+    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(
+            return "test=\"" + test + "\"";
-                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);
         }   //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<Test> 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:
+                if (robot.robotDrive != null)
-            case TUNE_Y_PID:
-            case TUNE_TURN_PID:
-                if (robot.robotDrive != null &&
-                    (testChoices.test != Test.TUNE_X_PID || robot.robotDrive.driveBase.supportsHolonomicDrive()))
                 {
                     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();
+                    Dashboard.Vision.colorThresholds = robot.vision.getRawColorBlobThresholds();
-                    colorThresholdIndex = 0;
+                    Dashboard.Vision.filterContourParams = Vision.colorBlobFilterContourParams.clone();
-                    colorThresholdMultiplier = 1.0;
+                    Dashboard.Vision.exposure =
-                    updateColorThresholds();
+                        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,
+                        0.0, Dashboard.Drive.drivePower, null,
-                        new TrcPose2D(testChoices.xTarget*12.0, testChoices.yTarget*12.0, testChoices.turnTarget));
+                        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:
+            case PP_DRIVE:
-                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:
                 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,
+                        true, robot.robotInfo.profiledMaxVelocity, robot.robotInfo.profiledMaxAcceleration,
-                        robot.robotInfo.profiledMaxDeceleration, new TrcPose2D(0.0, 48.0, 90.0));
+                        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();
+                    if (velocity > prevVelocity)
-                    TrcPose2D velPose = robot.robotDrive.driveBase.getFieldVelocity();
-                    double velocity = TrcUtil.magnitude(velPose.x, velPose.y);
-                    double acceleration = 0.0;
-
-                    if (prevTime != 0.0)
                     {
-                        acceleration = (velocity - prevVelocity)/(currTime - prevTime);
+                        acceleration = (velocity - prevVelocity)/deltaTime;
                     }
-
+                    else
-                    if (velocity > maxDriveVelocity)
                     {
-                        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 &&
+                if (testChoices.test == Test.VISION_TEST)
-                    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 (pressed)
                     {
@@ -592,23 +581,12 @@ public class FtcTest extends FtcTeleOp
                 break;
 
             case X:
-                if (testChoices.test == Test.TUNE_COLORBLOB_VISION &&
+            case Y:
-                    robot.vision != null && robot.vision.rawColorBlobVision != null)
+            case LeftBumper:
-                {
+            case RightBumper:
-                    if (pressed)
-                    {
-                        // Decrement to previous color threshold index.
-                        colorThresholdIndex--;
-                        if (colorThresholdIndex < 0)
-                        {
-                            colorThresholdIndex = 0;
-                        }
-                    }
-                    passToTeleOp = false;
-                }
                 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;
+                else if (testChoices.test == Test.SUBSYSTEMS_TEST)
-
-            case RightBumper:
-                if (testChoices.test == Test.VISION_TEST && robot.vision != null && robot.vision.vision != null)
                 {
-                    if (pressed)
+                    if (RobotParams.Preferences.tuneDriveBase)
                     {
-                        exposure += 100;
+                        // If we are moving swerve steering, make sure TeleOp doesn't interfere.
-                        robot.vision.vision.setManualExposure(exposure, null);
+                        teleOpControlEnabled = !pressed;
-                    }
+                        if (pressed && robot.robotDrive != null && robot.robotDrive instanceof FtcSwerveDrive)
-                    passToTeleOp = false;
+                        {
-                }
+                            FtcSwerveDrive swerveDrive = (FtcSwerveDrive) robot.robotDrive;
-                break;
+                            swerveDrive.setSteerAngle(0.0, false, true);
-
+                        }
-            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();
                     }
+//                    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.
+                    if (RobotParams.Preferences.tuneDriveBase)
-                    teleOpControlEnabled = !pressed;
-                    if (pressed && robot.robotDrive != null && robot.robotDrive instanceof FtcSwerveDrive)
                     {
-                        FtcSwerveDrive swerveDrive = (FtcSwerveDrive) robot.robotDrive;
+                        // If we are moving swerve steering, make sure TeleOp doesn't interfere.
-                        swerveDrive.setSteerAngle(180.0, false, true);
+                        teleOpControlEnabled = !pressed;
-                    }
+                        if (pressed && robot.robotDrive != null && robot.robotDrive instanceof FtcSwerveDrive)
-                    passToTeleOp = false;
+                        {
-                }
+                            FtcSwerveDrive swerveDrive = (FtcSwerveDrive) robot.robotDrive;
-                else if (testChoices.test == Test.TUNE_COLORBLOB_VISION &&
+                            swerveDrive.setSteerAngle(180.0, false, true);
-                         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();
                     }
+//                    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.
+                    if (RobotParams.Preferences.tuneDriveBase)
-                    teleOpControlEnabled = !pressed;
-                    if (pressed && robot.robotDrive != null && robot.robotDrive instanceof FtcSwerveDrive)
                     {
-                        FtcSwerveDrive swerveDrive = (FtcSwerveDrive) robot.robotDrive;
+                        // If we are moving swerve steering, make sure TeleOp doesn't interfere.
-                        swerveDrive.setSteerAngle(270.0, false, true);
+                        teleOpControlEnabled = !pressed;
-                    }
+                        if (pressed && robot.robotDrive != null && robot.robotDrive instanceof FtcSwerveDrive)
-                    passToTeleOp = false;
+                        {
-                }
+                            FtcSwerveDrive swerveDrive = (FtcSwerveDrive) robot.robotDrive;
-                else if (testChoices.test == Test.TUNE_COLORBLOB_VISION &&
+                            swerveDrive.setSteerAngle(270.0, false, true);
-                         robot.vision != null && robot.vision.rawColorBlobVision != null)
+                        }
-                {
-                    if (pressed && colorThresholdMultiplier * 10.0 <= 100.0)
-                    {
-                        // Increment the significant multiplier.
-                        colorThresholdMultiplier *= 10.0;
                     }
                     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.
+                    if (RobotParams.Preferences.tuneDriveBase)
-                    teleOpControlEnabled = !pressed;
-                    if (pressed && robot.robotDrive != null && robot.robotDrive instanceof FtcSwerveDrive)
                     {
-                        FtcSwerveDrive swerveDrive = (FtcSwerveDrive) robot.robotDrive;
+                        // If we are moving swerve steering, make sure TeleOp doesn't interfere.
-                        swerveDrive.setSteerAngle(90.0, false, true);
+                        teleOpControlEnabled = !pressed;
-                    }
+                        if (pressed && robot.robotDrive != null && robot.robotDrive instanceof FtcSwerveDrive)
-                    passToTeleOp = false;
+                        {
-                }
+                            FtcSwerveDrive swerveDrive = (FtcSwerveDrive) robot.robotDrive;
-                else if (testChoices.test == Test.TUNE_COLORBLOB_VISION &&
+                            swerveDrive.setSteerAngle(90.0, false, true);
-                         robot.vision != null && robot.vision.rawColorBlobVision != null)
+                        }
-                {
-                    if (pressed && colorThresholdMultiplier / 10.0 >= 1.0)
-                    {
-                        // Decrement the significant multiplier.
-                        colorThresholdMultiplier /= 10.0;
                     }
                     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);
+        FtcChoiceMenu<Test> 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");
         //
         // 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("X Timed drive", Test.X_TIMED_DRIVE, false);
-        testMenu.addChoice("Y Timed drive", Test.Y_TIMED_DRIVE, false, driveTimeMenu);
+        testMenu.addChoice("Y Timed drive", Test.Y_TIMED_DRIVE, false);
-        testMenu.addChoice("PID drive", Test.PID_DRIVE, false, xTargetMenu);
+        testMenu.addChoice("PID drive", Test.PID_DRIVE, false);
-        testMenu.addChoice("Tune X PID", Test.TUNE_X_PID, false, tuneKpMenu);
+        testMenu.addChoice("Pure Pursuit Drive", Test.PP_DRIVE, false);
-        testMenu.addChoice("Tune Y PID", Test.TUNE_Y_PID, false, tuneKpMenu);
+        testMenu.addChoice("Tune X PID", Test.TUNE_X_PID, false);
-        testMenu.addChoice("Tune Turn PID", Test.TUNE_TURN_PID, false, tuneKpMenu);
+        testMenu.addChoice("Tune Y PID", Test.TUNE_Y_PID, false);
-        testMenu.addChoice("Pure Pursuit Drive", Test.PURE_PURSUIT_DRIVE, 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

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.
      */

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
 
     /**

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;
+            wheelBaseLength = 360.0 * TrcUtil.INCHES_PER_MM;
-            wheelBaseWidth = 16.0;
+            wheelBaseWidth = (312.0 + 103.623) * TrcUtil.INCHES_PER_MM;
-            // IMU
+            // IMU (not used if using AbsoluteOdometry).
-            imuName = "imu";
+            imuName = null;
-            hubLogoDirection = RevHubOrientationOnRobot.LogoFacingDirection.UP;
+            hubLogoDirection = RevHubOrientationOnRobot.LogoFacingDirection.UP;     // Control Hub orientation
-            hubUsbDirection = RevHubOrientationOnRobot.UsbFacingDirection.FORWARD;
+            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"};
+            xOdWheelXOffsets = new double[] {4.0 * TrcUtil.INCHES_PER_MM};
-            xOdWheelIndices = new int[] {0};
+            xOdWheelYOffsets = new double[] {-132.0 * TrcUtil.INCHES_PER_MM};
-            xOdWheelXOffsets = new double[] {0.0};
+            yOdWheelXOffsets = new double[] {-156.0 * TrcUtil.INCHES_PER_MM, 156.0 * TrcUtil.INCHES_PER_MM};
-            xOdWheelYOffsets = new double[] {-168.0 * TrcUtil.INCHES_PER_MM};
+            yOdWheelYOffsets = new double[] {-4.0 * TrcUtil.INCHES_PER_MM, -4.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};
             // Absolute Odometry
             if (odometryType == TrcDriveBase.OdometryType.AbsoluteOdometry)
             {
                 if (RobotParams.Preferences.usePinpointOdometry)
                 {
                     FtcPinpointOdometry.Config ppOdoConfig = new FtcPinpointOdometry.Config()
-                        .setPodOffsets(-144.0, -168.0)
+                        .setPodOffsets(-156.0, -131.4)  // Offsets from robot center in mm
-                        .setEncoderResolution(ODWHEEL_CPR / Math.PI * ODWHEEL_DIAMETER)
+                        .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)
+                        .setOffset(-4.0 * TrcUtil.INCHES_PER_MM, 24.0 * TrcUtil.INCHES_PER_MM, 0.0)
-                        .setScale(1.0, 1.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
+            // IMU (not used if using AbsoluteOdometry).
-            imuName = "imu";
+            imuName = null;
-            hubLogoDirection = RevHubOrientationOnRobot.LogoFacingDirection.UP;
+            hubLogoDirection = RevHubOrientationOnRobot.LogoFacingDirection.UP;     // Control Hub orientation
-            hubUsbDirection = RevHubOrientationOnRobot.UsbFacingDirection.FORWARD;
+            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;
+            odometryType = TrcDriveBase.OdometryType.AbsoluteOdometry;
-            // Odometry Wheels
+            // Odometry Wheels (Offset from wheel base center)
             odWheelXScale = odWheelYScale = Math.PI * ODWHEEL_DIAMETER / ODWHEEL_CPR;
-            xOdWheelSensorNames = null;
+            xOdWheelXOffsets = new double[] {4.0 * TrcUtil.INCHES_PER_MM};
-            xOdWheelIndices = new int[] {FtcRobotDrive.INDEX_RIGHT_BACK};
+            xOdWheelYOffsets = new double[] {-132.0 * TrcUtil.INCHES_PER_MM};
-            xOdWheelXOffsets = new double[] {0.0};
+            yOdWheelXOffsets = new double[] {-156.0 * TrcUtil.INCHES_PER_MM, 156.0 * TrcUtil.INCHES_PER_MM};
-            xOdWheelYOffsets = new double[] {-168.0 * TrcUtil.INCHES_PER_MM};
+            yOdWheelYOffsets = new double[] {-4.0 * TrcUtil.INCHES_PER_MM, -4.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};
             // Absolute Odometry
             if (odometryType == TrcDriveBase.OdometryType.AbsoluteOdometry)
             {
                 if (RobotParams.Preferences.usePinpointOdometry)
                 {
                     FtcPinpointOdometry.Config ppOdoConfig = new FtcPinpointOdometry.Config()
-                        .setPodOffsets(-144.0, -168.0)
+                        .setPodOffsets(-156.0, -131.4)  // Offsets from robot center in mm
-                        .setEncoderResolution(ODWHEEL_CPR / Math.PI * ODWHEEL_DIAMETER)
+                        .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)
+                        .setOffset(-4.0 * TrcUtil.INCHES_PER_MM, 24.0 * TrcUtil.INCHES_PER_MM, 0.0)
-                        .setScale(1.0, 1.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);
+            xDrivePidCoeffs = new TrcPidController.PidCoefficients(0.072, 0.001, 0.0065, 0.0, 2.0);
-            xDrivePidPowerLimit = yDrivePidPowerLimit = 1.0;
+            xDrivePidPowerLimit = 1.0;
-            xDriveMaxPidRampRate = yDriveMaxPidRampRate = null;
+            xDriveMaxPidRampRate = null;
-            turnPidCoeffs = new TrcPidController.PidCoefficients(0.02, 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.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

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<TaskAuto.State>
         }   //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);
+        super(moduleName, TrcTaskMgr.TaskType.POST_PERIODIC_TASK);
-        this.ownerName = ownerName;
         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);
+        tracer.traceInfo(moduleName, "owner=" + owner + ", taskParams=(" + taskParams + ", event=" + completionEvent);
-        startAutoTask(State.START, taskParams, completionEvent);
+        startAutoTask(owner, State.START, taskParams, completionEvent);
     }   //autoAssist
 
     //
@@ -95,65 +91,55 @@ public class TaskAuto extends TrcAutoTask<TaskAuto.State>
      * 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 ||
+        // Call each subsystem.acquireExclusiveAccess(owner) and return true only if all acquires returned true.
-                          (robot.robotDrive.driveBase.acquireExclusiveAccess(ownerName));
+        // For example:
-
+        // return subsystem1.acquireExclusiveAccess(owner) && subsystem2.acquireExclusiveAccess(owner) ...
-        if (success)
+        return robot.robotDrive.driveBase.acquireExclusiveAccess(owner);
-        {
-            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;
     }   //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 +
+                "Releasing subsystem ownership on behalf of " + owner +
-                ", robotDrive=" + ownershipMgr.getOwner(robot.robotDrive.driveBase) + ").");
+                ", robotDrive=" + ownershipMgr.getOwner(robot.robotDrive.driveBase));
-            robot.robotDrive.driveBase.releaseExclusiveAccess(currOwner);
+            robot.robotDrive.driveBase.releaseExclusiveAccess(owner);
-            currOwner = null;
         }
     }   //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<TaskAuto.State>
      */
     @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;
 

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 =
+    private static final double objectWidth = 3.5;
-        new TrcOpenCvColorBlobPipeline.FilterContourParams()
+    private static final double objectHeight = 1.5;
-            .setMinArea(10.0)
+    // Logitech C920
-            .setMinPerimeter(12.0)
+    private static final double fx = 622.001;
-            .setWidthRange(0.0, 1000.0)
+    private static final double fy = 622.001;
-            .setHeightRange(0.0, 1000.0)
+    private static final double cx = 319.803;
-            .setSolidityRange(0.0, 100.0)
+    private static final double cy = 241.251;
-            .setVerticesRange(0.0, 1000.0)
+    private static final MatOfDouble distCoeffs = new MatOfDouble(0.1208, -0.261599, 0, 0, 0.10308, 0, 0, 0);
-            .setAspectRatioRange(0.5, 2.5);
 
     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)
+            if (RobotParams.Preferences.useCameraStreamProcessor)
-//            {
+            {
-//                FtcDashboard.getInstance().startCameraStream(openCvCamera, 0);
+                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

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'
 }