Sync'd latest changes.

Updated ftclib, trclib.
Reorganized params.

TeamCode/src/main/java/teamcode/FtcAuto.java

@@ -184,8 +184,6 @@ public class FtcAuto extends FtcOpMode
 //                robot.vision.setBlueBlobVisionEnabled(true);
 //            }
         }
-
-        robot.zeroCalibrate();
     }   //robotInit
 
     //

TeamCode/src/main/java/teamcode/FtcTeleOp.java

@@ -68,8 +68,6 @@ public class FtcTeleOp extends FtcOpMode
         // Create and initialize robot object.
         //
         robot = new Robot(TrcRobot.getRunMode());
-        drivePowerScale = RobotParams.Robot.DRIVE_NORMAL_SCALE;
-        turnPowerScale = RobotParams.Robot.TURN_NORMAL_SCALE;
 
         //
         // Open trace log.
@@ -86,10 +84,14 @@ public class FtcTeleOp extends FtcOpMode
         //
         driverGamepad = new FtcGamepad("DriverGamepad", gamepad1);
         driverGamepad.setButtonEventHandler(this::driverButtonEvent);
+        driverGamepad.setLeftStickInverted(false, true);
+
         operatorGamepad = new FtcGamepad("OperatorGamepad", gamepad2);
         operatorGamepad.setButtonEventHandler(this::operatorButtonEvent);
-        driverGamepad.setLeftStickInverted(false, true);
         operatorGamepad.setRightStickInverted(false, true);
+
+        drivePowerScale = RobotParams.Robot.DRIVE_NORMAL_SCALE;
+        turnPowerScale = RobotParams.Robot.TURN_NORMAL_SCALE;
         setDriveOrientation(RobotParams.Robot.DRIVE_ORIENTATION);
     }   //robotInit
 
@@ -122,10 +124,18 @@ public class FtcTeleOp extends FtcOpMode
         //
         // Enable AprilTag vision for re-localization.
         //
-        if (robot.vision != null && robot.vision.aprilTagVision != null)
+        if (robot.vision != null)
         {
-            robot.globalTracer.traceInfo(moduleName, "Enabling AprilTagVision.");
+            if (robot.vision.limelightVision != null)
-            robot.vision.setAprilTagVisionEnabled(true);
+            {
+                robot.globalTracer.traceInfo(moduleName, "Enabling Limelight AprilTagVision.");
+                robot.vision.setLimelightVisionEnabled(0, true);
+            }
+            else if (robot.vision.aprilTagVision != null)
+            {
+                robot.globalTracer.traceInfo(moduleName, "Enabling WebCam AprilTagVision.");
+                robot.vision.setAprilTagVisionEnabled(true);
+            }
         }
     }   //startMode
 
@@ -204,6 +214,7 @@ public class FtcTeleOp extends FtcOpMode
             //
             if (RobotParams.Preferences.useSubsystems)
             {
+                // Analog control of subsystems.
             }
             // Display subsystem status.
             if (RobotParams.Preferences.doStatusUpdate)
@@ -249,29 +260,24 @@ public class FtcTeleOp extends FtcOpMode
         switch (button)
         {
             case A:
-                // Toggle between field or robot oriented driving, only applicable for holonomic drive base.
+                if (robot.robotDrive != null && pressed)
-                if (driverAltFunc)
                 {
-                    if (pressed && robot.robotDrive != null)
+                    if (driverAltFunc)
                     {
                         if (robot.robotDrive.driveBase.isGyroAssistEnabled())
                         {
-                            // Disable GyroAssist drive.
                             robot.globalTracer.traceInfo(moduleName, ">>>>> Disabling GyroAssist.");
                             robot.robotDrive.driveBase.setGyroAssistEnabled(null);
                         }
                         else
                         {
-                            // Enable GyroAssist drive.
                             robot.globalTracer.traceInfo(moduleName, ">>>>> Enabling GyroAssist.");
                             robot.robotDrive.driveBase.setGyroAssistEnabled(robot.robotDrive.pidDrive.getTurnPidCtrl());
                         }
                     }
-                }
+                    else if (robot.robotDrive.driveBase.supportsHolonomicDrive())
-                else
-                {
-                    if (pressed && robot.robotDrive != null && robot.robotDrive.driveBase.supportsHolonomicDrive())
                     {
+                        // Toggle between field or robot oriented driving, only applicable for holonomic drive base.
                         if (robot.robotDrive.driveBase.getDriveOrientation() != TrcDriveBase.DriveOrientation.FIELD)
                         {
                             robot.globalTracer.traceInfo(moduleName, ">>>>> Enabling FIELD mode.");
@@ -321,7 +327,7 @@ public class FtcTeleOp extends FtcOpMode
             case Back:
                 if (pressed)
                 {
-                    robot.globalTracer.traceInfo(moduleName, ">>>>> ZeroCalibrate pressed.");
+                    robot.globalTracer.traceInfo(moduleName, ">>>>> ZeroCalibrating.");
                     robot.cancelAll();
                     robot.zeroCalibrate();
                     if (robot.robotDrive != null && robot.robotDrive instanceof FtcSwerveDrive)
@@ -334,7 +340,9 @@ public class FtcTeleOp extends FtcOpMode
                 break;
 
             case Start:
-                if (robot.vision != null && robot.vision.aprilTagVision != null && robot.robotDrive != null)
+                if (robot.vision != null &&
+                    (robot.vision.isLimelightVisionEnabled() || robot.vision.isAprilTagVisionEnabled()) &&
+                    robot.robotDrive != null)
                 {
                     // On press of the button, we will start looking for AprilTag for re-localization.
                     // On release of the button, we will set the robot's field location if we found the AprilTag.

TeamCode/src/main/java/teamcode/FtcTest.java

@@ -37,7 +37,7 @@ import ftclib.driverio.FtcMenu;
 import ftclib.driverio.FtcValueMenu;
 import ftclib.robotcore.FtcPidCoeffCache;
 import ftclib.vision.FtcLimelightVision;
-import teamcode.vision.Vision;
+import teamcode.subsystems.Vision;
 import trclib.command.CmdDriveMotorsTest;
 import trclib.command.CmdPidDrive;
 import trclib.command.CmdTimedDrive;
@@ -126,6 +126,7 @@ public class FtcTest extends FtcTeleOp
     // Drive Speed Test.
     private double maxDriveVelocity = 0.0;
     private double maxDriveAcceleration = 0.0;
+    private double maxTurnVelocity = 0.0;
     private double prevTime = 0.0;
     private double prevVelocity = 0.0;
     // Swerve Steering Calibration.
@@ -395,12 +396,19 @@ public class FtcTest extends FtcTeleOp
                         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;
         }

TeamCode/src/main/java/teamcode/Robot.java

@@ -31,7 +31,7 @@ import ftclib.robotcore.FtcOpMode;
 import ftclib.sensor.FtcRobotBattery;
 import teamcode.subsystems.LEDIndicator;
 import teamcode.subsystems.RobotBase;
-import teamcode.vision.Vision;
+import teamcode.subsystems.Vision;
 import trclib.motor.TrcMotor;
 import trclib.motor.TrcServo;
 import trclib.pathdrive.TrcPose2D;
@@ -62,6 +62,7 @@ public class Robot
     public LEDIndicator ledIndicator;
     public FtcRobotBattery battery;
     // Subsystems.
+    // Autotasks.
 
     /**
      * Constructor: Create an instance of the object.
@@ -92,7 +93,7 @@ public class Robot
         }
         // If robotType is VisionOnly, the robot controller is disconnected from the robot for testing vision.
         // In this case, we should not instantiate any robot hardware.
-        if (RobotParams.Preferences.robotType != RobotParams.RobotType.VisionOnly)
+        if (RobotParams.Preferences.robotType != RobotBase.RobotType.VisionOnly)
         {
             // Create and initialize sensors and indicators.
             if (robotInfo.indicatorName != null)
@@ -109,6 +110,10 @@ public class Robot
             //
             if (RobotParams.Preferences.useSubsystems)
             {
+                // Create subsystems.
+                // Zero calibrate all subsystems only at init time.
+                zeroCalibrate();
+                // Create autotasks.
             }
         }
 
@@ -283,11 +288,7 @@ public class Robot
     {
         globalTracer.traceInfo(moduleName, "Cancel all operations.");
 
-        if (robotDrive != null)
+        if (robotDrive != null) robotDrive.cancel();
-        {
-            // Cancel all auto-assist driving.
-            robotDrive.cancel();
-        }
     }   //cancelAll
 
     /**
@@ -316,6 +317,88 @@ public class Robot
     {
     }   //setRobotStartPosition
 
+    /**
+     * This method adjusts the given pose in the red alliance to be the specified alliance.
+     *
+     * @param x specifies x position in the red alliance in the specified unit.
+     * @param y specifies y position in the red alliance in the specified unit.
+     * @param heading specifies heading in the red alliance in degrees.
+     * @param alliance specifies the alliance to be converted to.
+     * @param isTileUnit specifies true if x and y are in tile unit, false if in inches.
+     * @return pose adjusted to be in the specified alliance in inches.
+     */
+    public TrcPose2D adjustPoseByAlliance(
+        double x, double y, double heading, FtcAuto.Alliance alliance, boolean isTileUnit)
+    {
+        TrcPose2D newPose = new TrcPose2D(x, y, heading);
+
+        if (alliance == FtcAuto.Alliance.BLUE_ALLIANCE)
+        {
+            // Translate blue alliance pose to red alliance pose.
+            if (RobotParams.Game.fieldIsMirrored)
+            {
+                // Mirrored field.
+                double angleDelta = (newPose.angle - 90.0)*2.0;
+                newPose.angle -= angleDelta;
+                newPose.y = -newPose.y;
+            }
+            else
+            {
+                // Symmetrical field.
+                newPose.x = -newPose.x;
+                newPose.y = -newPose.y;
+                newPose.angle = (newPose.angle + 180.0) % 360.0;
+            }
+        }
+
+        if (isTileUnit)
+        {
+            newPose.x *= RobotParams.Field.FULL_TILE_INCHES;
+            newPose.y *= RobotParams.Field.FULL_TILE_INCHES;
+        }
+
+        return newPose;
+    }   //adjustPoseByAlliance
+
+    /**
+     * This method adjusts the given pose in the red alliance to be the specified alliance.
+     *
+     * @param x specifies x position in the red alliance in tile unit.
+     * @param y specifies y position in the red alliance in tile unit.
+     * @param heading specifies heading in the red alliance in degrees.
+     * @param alliance specifies the alliance to be converted to.
+     * @return pose adjusted to be in the specified alliance in inches.
+     */
+    public TrcPose2D adjustPoseByAlliance(double x, double y, double heading, FtcAuto.Alliance alliance)
+    {
+        return adjustPoseByAlliance(x, y, heading, alliance, true);
+    }   //adjustPoseByAlliance
+
+    /**
+     * This method adjusts the given pose in the red alliance to be the specified alliance.
+     *
+     * @param pose specifies pose in the red alliance in the specified unit.
+     * @param alliance specifies the alliance to be converted to.
+     * @param isTileUnit specifies true if pose is in tile units, false in inches.
+     * @return pose adjusted to be in the specified alliance in inches.
+     */
+    public TrcPose2D adjustPoseByAlliance(TrcPose2D pose, FtcAuto.Alliance alliance, boolean isTileUnit)
+    {
+        return adjustPoseByAlliance(pose.x, pose.y, pose.angle, alliance, isTileUnit);
+    }   //adjustPoseByAlliance
+
+    /**
+     * This method adjusts the given pose in the red alliance to be the specified alliance.
+     *
+     * @param pose specifies pose in the blue alliance in tile unit.
+     * @param alliance specifies the alliance to be converted to.
+     * @return pose adjusted to be in the specified alliance in inches.
+     */
+    public TrcPose2D adjustPoseByAlliance(TrcPose2D pose, FtcAuto.Alliance alliance)
+    {
+        return adjustPoseByAlliance(pose, alliance, true);
+    }   //adjustPoseByAlliance
+
     /**
      * This method sends the text string to the Driver Station to be spoken using text to speech.
      *

TeamCode/src/main/java/teamcode/RobotParams.java

@@ -24,29 +24,104 @@ package teamcode;
 
 import android.os.Environment;
 
-import com.qualcomm.hardware.rev.RevHubOrientationOnRobot.*;
+import teamcode.subsystems.RobotBase;
-
-import org.openftc.easyopencv.OpenCvCameraRotation;
-
-import ftclib.drivebase.FtcRobotDrive;
-import ftclib.drivebase.FtcSwerveDrive;
-import ftclib.motor.FtcMotorActuator.MotorType;
-import ftclib.sensor.FtcPinpointOdometry;
-import ftclib.sensor.FtcSparkFunOtos;
-import trclib.dataprocessor.TrcUtil;
-import trclib.drivebase.TrcDriveBase;
 import trclib.drivebase.TrcDriveBase.DriveOrientation;
 import trclib.driverio.TrcGameController.DriveMode;
 import trclib.pathdrive.TrcPose2D;
-import trclib.pathdrive.TrcPose3D;
-import trclib.robotcore.TrcPidController.PidCoefficients;
-import trclib.vision.TrcHomographyMapper;
 
 /**
  * This class contains robot and subsystem constants and parameters.
  */
 public class RobotParams
 {
+    /**
+     * This class contains robot preferences. It enables/disables various robot features. This is especially useful
+     * during robot development where some subsystems may not be available or ready yet. By disabling unavailable
+     * subsystems, one can test the rest of the robot without the fear of code crashing when some subsystems are not
+     * found.
+     */
+    public static class Preferences
+    {
+        // Global config
+        public static final RobotBase.RobotType robotType       = RobotBase.RobotType.MecanumRobot;
+        public static final boolean inCompetition               = false;
+        public static final boolean useTraceLog                 = true;
+        public static final boolean useLoopPerformanceMonitor   = true;
+        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 showSubsystems              = true;
+        public static final boolean useBlinkinLED               = false;
+        public static final boolean useGobildaLED               = false;
+        // Vision
+        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 useAprilTagVision           = false;
+        public static final boolean useColorBlobVision          = false;
+        public static final boolean doWatershed                 = false;
+        public static final boolean showVisionView              = !inCompetition;
+        public static final boolean showVisionStat              = false;
+        // Drive Base
+        public static final boolean useDriveBase                = false;
+        public static final boolean usePinpointOdometry         = false;
+        public static final boolean useSparkfunOTOS             = false;
+        // Subsystems
+        public static final boolean useSubsystems               = false;
+    }   //class Preferences
+
+    /**
+     * This class contains miscellaneous robot info.
+     */
+    public static class Robot
+    {
+        public static final String TEAM_FOLDER_PATH             =
+            Environment.getExternalStorageDirectory().getPath() + "/FIRST/ftcTeam";
+        public static final String LOG_FOLDER_PATH              = TEAM_FOLDER_PATH + "/tracelogs";
+        public static final String STEER_ZERO_CAL_FILE          = TEAM_FOLDER_PATH + "/SteerZeroCalibration.txt";
+        public static final double DASHBOARD_UPDATE_INTERVAL    = 0.2;      // in msec
+        public static final String ROBOT_CODEBASE               = "Robot2025";
+        // Robot Drive Parameters.
+        public static final DriveMode DRIVE_MODE                = DriveMode.ArcadeMode;
+        public static final DriveOrientation DRIVE_ORIENTATION  = DriveOrientation.ROBOT;
+        public static final double DRIVE_SLOW_SCALE             = 0.3;
+        public static final double DRIVE_NORMAL_SCALE           = 1.0;
+        public static final double TURN_SLOW_SCALE              = 0.3;
+        public static final double TURN_NORMAL_SCALE            = 0.6;
+        public static final double ROBOT_LENGTH                 = 18.0;
+        public static final double ROBOT_WIDTH                  = 18.0;
+    }   //class Robot
+
+    /**
+     * This class contains season specific game element information.
+     */
+    public static class Game
+    {
+        public static final boolean fieldIsMirrored             = false;
+        // AprilTag locations.
+        public static final TrcPose2D[] APRILTAG_POSES          = new TrcPose2D[] {
+            new TrcPose2D(0.0, 0.0, 0.0),   // TagId 1
+            new TrcPose2D(0.0, 0.0, 0.0),   // TagId 2
+            new TrcPose2D(0.0, 0.0, 0.0),   // TagId 3
+            new TrcPose2D(0.0, 0.0, 0.0)    // TagId 4
+        };
+        // Robot start locations.
+    }   //class Game
+
+    /**
+     * This class contains field dimension constants. Generally, these should not be changed.
+     */
+    public static class Field
+    {
+        public static final double FULL_FIELD_INCHES            = 141.24;
+        public static final double HALF_FIELD_INCHES            = FULL_FIELD_INCHES/2.0;
+        public static final double FULL_TILE_INCHES             = FULL_FIELD_INCHES/6.0;
+    }   //class Field
+
     /**
      * This class contains Gobilda motor parameters.
      */
@@ -70,510 +145,4 @@ public class RobotParams
             MOTOR_5203_435_ENC_PPR * MOTOR_5203_435_MAX_RPM / 60.0;     // 2787.9135 pps
     }   //class Gobilda
 
-    /**
-     * This class contains field dimension constants. Generally, these should not be changed.
-     */
-    public static class Field
-    {
-        public static final double FULL_FIELD_INCHES            = 141.24;
-        public static final double HALF_FIELD_INCHES            = FULL_FIELD_INCHES/2.0;
-        public static final double FULL_TILE_INCHES             = FULL_FIELD_INCHES/6.0;
-    }   //class Field
-
-    /**
-     * This class contains season specific game element information.
-     */
-    public static class Game
-    {
-        public static final TrcPose2D[] APRILTAG_POSES          = new TrcPose2D[] {
-            new TrcPose2D(0.0, 0.0, 0.0),   // TagId 1
-            new TrcPose2D(0.0, 0.0, 0.0),   // TagId 2
-            new TrcPose2D(0.0, 0.0, 0.0),   // TagId 3
-            new TrcPose2D(0.0, 0.0, 0.0)    // TagId 4
-        };
-    }   //class Game
-
-    /**
-     * This class contains miscellaneous robot info.
-     */
-    public static class Robot
-    {
-        public static final String TEAM_FOLDER_PATH             =
-            Environment.getExternalStorageDirectory().getPath() + "/FIRST/ftcTeam";
-        public static final String LOG_FOLDER_PATH              = TEAM_FOLDER_PATH + "/tracelogs";
-        public static final String STEER_ZERO_CAL_FILE          = TEAM_FOLDER_PATH + "/SteerZeroCalibration.txt";
-        public static final double DASHBOARD_UPDATE_INTERVAL    = 0.1;      // in msec
-        public static final String ROBOT_CODEBASE               = "Robot2025";
-        public static final double ROBOT_LENGTH                 = 17.0;
-        public static final double ROBOT_WIDTH                  = 17.0;
-        // Robot Drive Parameters.
-        public static final DriveMode DRIVE_MODE                = DriveMode.ArcadeMode;
-        public static final DriveOrientation DRIVE_ORIENTATION  = DriveOrientation.ROBOT;
-        public static final double DRIVE_SLOW_SCALE             = 0.3;
-        public static final double DRIVE_NORMAL_SCALE           = 1.0;
-        public static final double TURN_SLOW_SCALE              = 0.3;
-        public static final double TURN_NORMAL_SCALE            = 0.6;
-    }   //class Robot
-
-    /**
-     * When the season starts, the competition robot may not be ready for programmers. It's crucial to save time by
-     * developing code on robots of previous seasons. By adding previous robots to the list of RobotType, one can
-     * easily switch the code to handle different robots.
-     */
-    public enum RobotType
-    {
-        // This is useful for developing Vision code where all you need is a Control Hub and camera.
-        VisionOnly,
-        // Generic Differential Drive Base Robot
-        DifferentialRobot,
-        // Generic Mecanum Drive Base Robot
-        MecanumRobot,
-        // Generic Swerve Drive Base Robot
-        SwerveRobot
-    }   //enum RobotType
-
-    /**
-     * This class contains robot preferences. It enables/disables various robot features. This is especially useful
-     * during robot development where some subsystems may not be available or ready yet. By disabling unavailable
-     * subsystems, one can test the rest of the robot without the fear of code crashing when some subsystems are not
-     * found.
-     */
-    public static class Preferences
-    {
-        // Global config
-        public static final RobotType robotType                 = RobotType.MecanumRobot;
-        public static final boolean inCompetition               = false;
-        public static final boolean useTraceLog                 = true;
-        public static final boolean useLoopPerformanceMonitor   = true;
-        public static final boolean useBatteryMonitor           = false;
-        // 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 showSubsystems              = true;
-        public static final boolean useBlinkinLED               = false;
-        public static final boolean useGobildaLED               = false;
-        // Vision
-        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 useAprilTagVision           = false;
-        public static final boolean useColorBlobVision          = false;
-        public static final boolean showVisionView              = !inCompetition;
-        public static final boolean showVisionStat              = false;
-        // Drive Base
-        public static final boolean useDriveBase                = false;
-        public static final boolean usePinpointOdometry         = false;
-        public static final boolean useSparkfunOTOS             = false;
-        // Subsystems
-        public static final boolean useSubsystems               = false;
-    }   //class Preferences
-
-    //
-    // Robot Parameters.
-    //
-
-    /**
-     * This class contains the parameters of the front camera.
-     */
-    public static class FrontCamParams extends FtcRobotDrive.VisionInfo
-    {
-        public FrontCamParams()
-        {
-            camName = "Webcam 1";
-            camImageWidth = 640;
-            camImageHeight = 480;
-            camXOffset = 0.0;                   // Inches to the right from robot center
-            camYOffset = 2.0;                   // Inches forward from robot center
-            camZOffset = 9.75;                  // Inches up from the floor
-            camPitch = 15.0;                    // degrees down from horizontal
-            camYaw = 0.0;                       // degrees clockwise from robot front
-            camRoll = 0.0;
-            camPose = new TrcPose3D(camXOffset, camYOffset, camZOffset, camYaw, camPitch, camRoll);
-            camOrientation = OpenCvCameraRotation.UPRIGHT;
-            // Homography: cameraRect in pixels, worldRect in inches
-            cameraRect = new TrcHomographyMapper.Rectangle(
-                0.0, 120.0,                                             // Camera Top Left
-                camImageWidth -1, 120.0,                                // Camera Top Right
-                0.0, camImageHeight - 1,                                // Camera Bottom Left
-                camImageWidth - 1, camImageHeight - 1);                 // Camera Bottom Right
-            worldRect = new TrcHomographyMapper.Rectangle(
-                -12.5626, 48.0 - Robot.ROBOT_LENGTH/2.0 - camYOffset,   // World Top Left
-                11.4375, 44.75 - Robot.ROBOT_LENGTH/2.0 - camYOffset,   // World Top Right
-                -2.5625, 21.0 - Robot.ROBOT_LENGTH/2.0 - camYOffset,    // World Bottom Left
-                2.5626, 21.0 - Robot.ROBOT_LENGTH/2.0 - camYOffset);    // World Bottom Right
-        }   //FrontCamParams
-    }   //class FrontCamParams
-
-    /**
-     * This class contains the parameters of the back camera.
-     */
-    public static class BackCamParams extends FtcRobotDrive.VisionInfo
-    {
-        public BackCamParams()
-        {
-            camName = "Webcam 2";
-            camImageWidth = 640;
-            camImageHeight = 480;
-            camXOffset = 0.0;                   // Inches to the right from robot center
-            camYOffset = 2.0;                   // Inches forward from robot center
-            camZOffset = 9.75;                  // Inches up from the floor
-            camPitch = 15.0;                    // degrees down from horizontal
-            camYaw = 0.0;                       // degrees clockwise from robot front
-            camRoll = 0.0;
-            camPose = new TrcPose3D(camXOffset, camYOffset, camZOffset, camYaw, camPitch, camRoll);
-            camOrientation = OpenCvCameraRotation.UPRIGHT;
-            // Homography: cameraRect in pixels, worldRect in inches
-            cameraRect = new TrcHomographyMapper.Rectangle(
-                0.0, 120.0,                                             // Camera Top Left
-                camImageWidth -1, 120.0,                                // Camera Top Right
-                0.0, camImageHeight - 1,                                // Camera Bottom Left
-                camImageWidth - 1, camImageHeight - 1);                 // Camera Bottom Right
-            worldRect = new TrcHomographyMapper.Rectangle(
-                -12.5626, 48.0 - Robot.ROBOT_LENGTH/2.0 - camYOffset,   // World Top Left
-                11.4375, 44.75 - Robot.ROBOT_LENGTH/2.0 - camYOffset,   // World Top Right
-                -2.5625, 21.0 - Robot.ROBOT_LENGTH/2.0 - camYOffset,    // World Bottom Left
-                2.5626, 21.0 - Robot.ROBOT_LENGTH/2.0 - camYOffset);    // World Bottom Right
-        }   //BackCamParams
-    }   //class BackCamParams
-
-    /**
-     * This class contains the parameters of the Limelight vision processor.
-     */
-    public static class LimelightParams extends FtcRobotDrive.VisionInfo
-    {
-        public LimelightParams()
-        {
-            camName = "Limelight3a";
-            camImageWidth = 640;
-            camImageHeight = 480;
-            camXOffset = 0.0;                   // Inches to the right from robot center
-            camYOffset = 2.0;                   // Inches forward from robot center
-            camZOffset = 9.75;                  // Inches up from the floor
-            camPitch = 15.0;                    // degrees down from horizontal
-            camYaw = 0.0;                       // degrees clockwise from robot front
-            camRoll = 0.0;
-            camPose = new TrcPose3D(camXOffset, camYOffset, camZOffset, camYaw, camPitch, camRoll);
-            camOrientation = OpenCvCameraRotation.UPRIGHT;
-            // Homography: cameraRect in pixels, worldRect in inches
-            cameraRect = new TrcHomographyMapper.Rectangle(
-                0.0, 120.0,                                             // Camera Top Left
-                camImageWidth - 1, 120.0,                                // Camera Top Right
-                0.0, camImageHeight - 1,                                // Camera Bottom Left
-                camImageWidth - 1, camImageHeight - 1);                 // Camera Bottom Right
-            worldRect = new TrcHomographyMapper.Rectangle(
-                -12.5626, 48.0 - Robot.ROBOT_LENGTH/2.0 - camYOffset,   // World Top Left
-                11.4375, 44.75 - Robot.ROBOT_LENGTH/2.0 - camYOffset,   // World Top Right
-                -2.5625, 21.0 - Robot.ROBOT_LENGTH/2.0 - camYOffset,    // World Bottom Left
-                2.5626, 21.0 - Robot.ROBOT_LENGTH/2.0 - camYOffset);    // World Bottom Right
-        }   //LimelightParams
-    }   //class LimelightParams
-
-    public static class VisionOnlyParams extends FtcRobotDrive.RobotInfo
-    {
-        public VisionOnlyParams()
-        {
-            robotName = "VisionOnly";
-            webCam1 = new FrontCamParams();
-            webCam2 = new BackCamParams();
-            limelight = new LimelightParams();
-        }   //VisionOnlyParams
-    }   //class VisionOnlyParams
-
-    /**
-     * This class contains the Differential Robot Parameters.
-     */
-    public static class DifferentialParams extends FtcRobotDrive.RobotInfo
-    {
-        // Optii Odometry Wheel
-        private static final double ODWHEEL_DIAMETER = 35.0 * TrcUtil.INCHES_PER_MM;
-        private static final double ODWHEEL_CPR = 4096.0;
-
-        public DifferentialParams()
-        {
-            robotName = "DifferentialRobot";
-            // Robot Dimensions
-            robotLength = Robot.ROBOT_LENGTH;
-            robotWidth = Robot.ROBOT_WIDTH;
-            wheelBaseLength = (24.0 * 14)*TrcUtil.INCHES_PER_MM;
-            wheelBaseWidth = 16.0;
-            // IMU
-            imuName = "imu";
-            hubLogoDirection = LogoFacingDirection.UP;
-            hubUsbDirection = UsbFacingDirection.FORWARD;
-            // Drive Motors
-            driveMotorType = MotorType.DcMotor;
-            driveMotorNames = new String[] {"lfDriveMotor", "rfDriveMotor"};
-            driveMotorInverted = new boolean[] {true, false};
-            odometryType = TrcDriveBase.OdometryType.MotorOdometry;
-            // Odometry Wheels
-            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, -12.0 * TrcUtil.INCHES_PER_MM};
-            yOdWheelYOffsets = new double[] {144.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(0.0, 0.0)
-                        .setEncoderResolution(ODWHEEL_CPR / Math.PI * ODWHEEL_DIAMETER)
-                        .setEncodersInverted(false, false);
-                    absoluteOdometry = new FtcPinpointOdometry("pinpointOdo", ppOdoConfig);
-                }
-                else if (RobotParams.Preferences.useSparkfunOTOS)
-                {
-                    FtcSparkFunOtos.Config otosConfig = new FtcSparkFunOtos.Config()
-                        .setOffset(0.0, 0.0, 0.0)
-                        .setScale(1.0, 1.0);
-                    absoluteOdometry = new FtcSparkFunOtos("sparkfunOtos", otosConfig);
-                }
-            }
-            else
-            {
-                absoluteOdometry = null;
-            }
-            // Drive Motor Odometry
-            yDrivePosScale = 0.02166184604662450653409090909091;        // in/count
-            // Robot Drive Characteristics
-            robotMaxVelocity = 23.0;        // inches/sec
-            robotMaxAcceleration  = 500.0;  // inches/sec2
-            robotMaxTurnRate = 100.0;       // degrees/sec
-            profiledMaxVelocity = robotMaxVelocity;
-            profiledMaxAcceleration = robotMaxAcceleration;
-            profiledMaxTurnRate = robotMaxTurnRate;
-            // DriveBase PID Parameters
-            drivePidTolerance = 1.0;
-            turnPidTolerance = 1.0;
-            yDrivePidCoeffs = new PidCoefficients(0.06, 0.0, 0.002, 0.0, 0.0);
-            yDrivePidPowerLimit = 1.0;
-            yDriveMaxPidRampRate = null;
-            turnPidCoeffs = new PidCoefficients(0.02, 0.0, 0.002, 0.0, 0.0);
-            turnPidPowerLimit = 0.5;
-            turnMaxPidRampRate = null;
-            // PID Stall Detection
-            pidStallDetectionEnabled = true;
-            // PurePursuit Parameters
-            ppdFollowingDistance = 6.0;
-            velPidCoeffs = new PidCoefficients(0.0, 0.0, 0.0, 1.0 / profiledMaxVelocity, 0.0);
-            // Vision
-            webCam1 = new FrontCamParams();
-            webCam2 = new BackCamParams();
-            limelight = new LimelightParams();
-            // Miscellaneous
-            indicatorName = "blinkin";
-        }   //DifferentialParams
-    }   //class DifferentialParams
-
-    /**
-     * This class contains the Mecanum Robot Parameters.
-     */
-    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_CPR = 4096.0;
-
-        public MecanumParams()
-        {
-            robotName = "MecanumRobot";
-            // Robot Dimensions
-            robotLength = Robot.ROBOT_LENGTH;
-            robotWidth = Robot.ROBOT_WIDTH;
-            wheelBaseLength = (24.0 * 14)*TrcUtil.INCHES_PER_MM;
-            wheelBaseWidth = 16.0;
-            // IMU
-            imuName = "imu";
-            hubLogoDirection = LogoFacingDirection.UP;
-            hubUsbDirection = UsbFacingDirection.FORWARD;
-            // Drive Motors
-            driveMotorType = MotorType.DcMotor;
-            driveMotorNames = new String[] {"lfDriveMotor", "rfDriveMotor", "lbDriveMotor", "rbDriveMotor"};
-            driveMotorInverted = new boolean[] {true, false, true, false};
-            odometryType = TrcDriveBase.OdometryType.AbsoluteOdometry;
-            // Odometry Wheels
-            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, -12.0 * TrcUtil.INCHES_PER_MM};
-            yOdWheelYOffsets = new double[] {144.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(0.0, 0.0)
-                        .setEncoderResolution(ODWHEEL_CPR / Math.PI * ODWHEEL_DIAMETER)
-                        .setEncodersInverted(false, false);
-                    absoluteOdometry = new FtcPinpointOdometry("pinpointOdo", ppOdoConfig);
-                }
-                else if (RobotParams.Preferences.useSparkfunOTOS)
-                {
-                    FtcSparkFunOtos.Config otosConfig = new FtcSparkFunOtos.Config()
-                        .setOffset(0.0, 0.0, 0.0)
-                        .setScale(1.0, 1.0);
-                    absoluteOdometry = new FtcSparkFunOtos("sparkfunOtos", otosConfig);
-                }
-            }
-            else
-            {
-                absoluteOdometry = null;
-            }
-            // Drive Motor Odometry
-            xDrivePosScale = 0.01924724265461924299065420560748;        // in/count
-            yDrivePosScale = 0.02166184604662450653409090909091;        // in/count
-            // Robot Drive Characteristics
-            robotMaxVelocity = 23.0;        // inches/sec
-            robotMaxAcceleration  = 500.0;  // inches/sec2
-            robotMaxTurnRate = 100.0;       // degrees/sec
-            profiledMaxVelocity = robotMaxVelocity;
-            profiledMaxAcceleration = robotMaxAcceleration;
-            profiledMaxTurnRate = robotMaxTurnRate;
-            // DriveBase PID Parameters
-            drivePidTolerance = 1.0;
-            turnPidTolerance = 1.0;
-            xDrivePidCoeffs = new PidCoefficients(0.95, 0.0, 0.001, 0.0, 0.0);
-            xDrivePidPowerLimit = 1.0;
-            xDriveMaxPidRampRate = null;
-            yDrivePidCoeffs = new PidCoefficients(0.06, 0.0, 0.002, 0.0, 0.0);
-            yDrivePidPowerLimit = 1.0;
-            yDriveMaxPidRampRate = null;
-            turnPidCoeffs = new PidCoefficients(0.02, 0.0, 0.002, 0.0, 0.0);
-            turnPidPowerLimit = 0.5;
-            turnMaxPidRampRate = null;
-            // PID Stall Detection
-            pidStallDetectionEnabled = true;
-            // PurePursuit Parameters
-            ppdFollowingDistance = 6.0;
-            velPidCoeffs = new PidCoefficients(0.0, 0.0, 0.0, 1.0 / profiledMaxVelocity, 0.0);
-            // Vision
-            webCam1 = new FrontCamParams();
-            webCam2 = new BackCamParams();
-            limelight = new LimelightParams();
-            // Miscellaneous
-            indicatorName = "blinkin";
-        }   //MecanumParams
-    }   //class MecanumParams
-
-    /**
-     * This class contains the Swerve Drive Base Parameters.
-     */
-    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_CPR = 4096.0;
-
-        public SwerveParams()
-        {
-            robotName = "SwerveRobot";
-            // Robot Dimensions
-            robotLength = Robot.ROBOT_LENGTH;
-            robotWidth = Robot.ROBOT_WIDTH;
-            wheelBaseLength = (24.0 * 14)*TrcUtil.INCHES_PER_MM;
-            wheelBaseWidth = 16.0;
-            // IMU
-            imuName = "imu";
-            hubLogoDirection = LogoFacingDirection.UP;
-            hubUsbDirection = UsbFacingDirection.FORWARD;
-            // Drive Motors
-            driveMotorType = MotorType.DcMotor;
-            driveMotorNames = new String[] {"lfDriveMotor", "rfDriveMotor", "lbDriveMotor", "rbDriveMotor"};
-            driveMotorInverted = new boolean[] {true, false, true, false};
-            odometryType = TrcDriveBase.OdometryType.OdometryWheels;
-            // Odometry Wheels
-            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, -12.0 * TrcUtil.INCHES_PER_MM};
-            yOdWheelYOffsets = new double[] {144.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(0.0, 0.0)
-                        .setEncoderResolution(ODWHEEL_CPR / Math.PI * ODWHEEL_DIAMETER)
-                        .setEncodersInverted(false, false);
-                    absoluteOdometry = new FtcPinpointOdometry("pinpointOdo", ppOdoConfig);
-                }
-                else if (RobotParams.Preferences.useSparkfunOTOS)
-                {
-                    FtcSparkFunOtos.Config otosConfig = new FtcSparkFunOtos.Config()
-                        .setOffset(0.0, 0.0, 0.0)
-                        .setScale(1.0, 1.0);
-                    absoluteOdometry = new FtcSparkFunOtos("sparkfunOtos", otosConfig);
-                }
-            }
-            else
-            {
-                absoluteOdometry = null;
-            }
-            // Drive Motor Odometry
-            xDrivePosScale = 0.01924724265461924299065420560748;        // in/count
-            yDrivePosScale = 0.01924724265461924299065420560748;        // in/count
-            // Robot Drive Characteristics
-            robotMaxVelocity = 23.0;        // inches/sec
-            robotMaxAcceleration  = 500.0;  // inches/sec2
-            robotMaxTurnRate = 100.0;       // degrees/sec
-            profiledMaxVelocity = robotMaxVelocity;
-            profiledMaxAcceleration = robotMaxAcceleration;
-            profiledMaxTurnRate = robotMaxTurnRate;
-            // DriveBase PID Parameters
-            drivePidTolerance = 1.0;
-            turnPidTolerance = 1.0;
-            xDrivePidCoeffs = yDrivePidCoeffs = new PidCoefficients(0.95, 0.0, 0.001, 0.0, 0.0);
-            xDrivePidPowerLimit = yDrivePidPowerLimit = 1.0;
-            xDriveMaxPidRampRate = yDriveMaxPidRampRate = null;
-            turnPidCoeffs = new PidCoefficients(0.02, 0.0, 0.002, 0.0, 0.0);
-            turnPidPowerLimit = 0.5;
-            turnMaxPidRampRate = null;
-            // PID Stall Detection
-            pidStallDetectionEnabled = true;
-            // PurePursuit Parameters
-            ppdFollowingDistance = 6.0;
-            velPidCoeffs = new PidCoefficients(0.0, 0.0, 0.0, 1.0 / profiledMaxVelocity, 0.0);
-            // Vision
-            webCam1 = new FrontCamParams();
-            webCam2 = new BackCamParams();
-            limelight = new LimelightParams();
-            // Miscellaneous
-            indicatorName = "blinkin";
-            // Steer Encoders
-            steerEncoderNames = new String[] {"lfSteerEncoder", "rfSteerEncoder", "lbSteerEncoder", "rbSteerEncoder"};
-            steerEncoderInverted = new boolean[] {false, false, false, false};
-            steerEncoderZeros = new double[] {0.474812, 0.467663, 0.541338, 0.545340};
-            steerZerosFilePath = Robot.STEER_ZERO_CAL_FILE;
-            // Steer Motors
-            steerMotorType = MotorType.CRServo;
-            steerMotorNames = new String[] {"lfSteerServo", "rfSteerServo", "lbSteerServo", "rbSteerServo"};
-            steerMotorInverted = new boolean[] {true, true, true, true};
-            steerMotorPidCoeffs = new PidCoefficients(0.0, 0.0, 0.0, 0.0, 0.0);
-            steerMotorPidTolerance = 1.0;
-            // Swerve Modules
-            swerveModuleNames = new String[] {"lfWheel", "rfWheel", "lbWheel", "rbWheel"};
-        }   //SwerveParams
-    }   //class SwerveParams
-
-    //
-    // Subsystems.
-    //
-
 }   //class RobotParams

TeamCode/src/main/java/teamcode/autotasks/TaskAuto.java

@@ -78,15 +78,6 @@ public class TaskAuto extends TrcAutoTask<TaskAuto.State>
         startAutoTask(State.START, new TaskParams(), completionEvent);
     }   //autoAssist
 
-    /**
-     * This method cancels an in progress auto-assist operation if any.
-     */
-    public void autoAssistCancel()
-    {
-        tracer.traceInfo(moduleName, "Canceling auto-assist.");
-        stopAutoTask(false);
-    }   //autoAssistCancel
-
     //
     // Implement TrcAutoTask abstract methods.
     //

TeamCode/src/main/java/teamcode/subsystems/LEDIndicator.java

@@ -61,12 +61,12 @@ public class LEDIndicator
                 // Highest priority.
                 new TrcGobildaIndicatorLight.Pattern(RED_BLOB, TrcGobildaIndicatorLight.Color.Red),
                 new TrcGobildaIndicatorLight.Pattern(BLUE_BLOB, TrcGobildaIndicatorLight.Color.Blue),
-                new TrcGobildaIndicatorLight.Pattern(NO_BLOB, TrcGobildaIndicatorLight.Color.Azure),
+                new TrcGobildaIndicatorLight.Pattern(NO_BLOB, TrcGobildaIndicatorLight.Color.Cyan),
                 new TrcGobildaIndicatorLight.Pattern(APRIL_TAG, TrcGobildaIndicatorLight.Color.Green),
                 new TrcGobildaIndicatorLight.Pattern(DRIVE_ORIENTATION_FIELD, TrcGobildaIndicatorLight.Color.Violet),
                 new TrcGobildaIndicatorLight.Pattern(DRIVE_ORIENTATION_ROBOT, TrcGobildaIndicatorLight.Color.White),
                 new TrcGobildaIndicatorLight.Pattern(DRIVE_ORIENTATION_INVERTED, TrcGobildaIndicatorLight.Color.Orange),
-                new TrcGobildaIndicatorLight.Pattern(OFF_PATTERN, TrcGobildaIndicatorLight.Color.Off)
+                new TrcGobildaIndicatorLight.Pattern(OFF_PATTERN, TrcGobildaIndicatorLight.Color.Black)
                 // Lowest priority.
             };
             indicator = new FtcGobildaIndicatorLight(indicatorName);

TeamCode/src/main/java/teamcode/subsystems/RobotBase.java

@@ -22,17 +22,305 @@
 
 package teamcode.subsystems;
 
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+
 import ftclib.drivebase.FtcDifferentialDrive;
 import ftclib.drivebase.FtcMecanumDrive;
 import ftclib.drivebase.FtcRobotDrive;
 import ftclib.drivebase.FtcSwerveDrive;
+import ftclib.motor.FtcMotorActuator;
+import ftclib.sensor.FtcPinpointOdometry;
+import ftclib.sensor.FtcSparkFunOtos;
 import teamcode.RobotParams;
+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.
  */
 public class RobotBase
 {
+    /**
+     * When the season starts, the competition robot may not be ready for programmers. It's crucial to save time by
+     * developing code on robots of previous seasons. By adding previous robots to the list of RobotType, one can
+     * easily switch the code to handle different robots.
+     */
+    public enum RobotType
+    {
+        // This is useful for developing Vision code where all you need is a Control Hub and camera.
+        VisionOnly,
+        // Generic Differential Drive Base Robot
+        DifferentialRobot,
+        // Generic Mecanum Drive Base Robot
+        MecanumRobot,
+        // Generic Swerve Drive Base Robot
+        SwerveRobot
+    }   //enum RobotType
+
+    public static class VisionOnlyParams extends FtcRobotDrive.RobotInfo
+    {
+        public VisionOnlyParams()
+        {
+            robotName = "VisionOnly";
+            webCam1 = new Vision.FrontCamParams();
+            webCam2 = new Vision.BackCamParams();
+            limelight = new Vision.LimelightParams();
+        }   //VisionOnlyParams
+    }   //class VisionOnlyParams
+
+    /**
+     * This class contains the Differential Robot Parameters.
+     */
+    public static class DifferentialParams extends FtcRobotDrive.RobotInfo
+    {
+        public DifferentialParams()
+        {
+            robotName = "DifferentialRobot";
+            // 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;
+            // Drive Motors
+            driveMotorType = FtcMotorActuator.MotorType.DcMotor;
+            driveMotorNames = new String[] {"lfDriveMotor", "rfDriveMotor"};
+            driveMotorInverted = new boolean[] {true, false};
+            odometryType = TrcDriveBase.OdometryType.MotorOdometry;
+            // Drive Motor Odometry
+            yDrivePosScale = 0.02166184604662450653409090909091;        // in/count
+            // Robot Drive Characteristics
+            robotMaxVelocity = 80.0;        // inches/sec
+            robotMaxAcceleration  = 350.0;  // inches/sec2
+            robotMaxTurnRate = 80.0;        // degrees/sec
+            profiledMaxVelocity = robotMaxVelocity;
+            profiledMaxAcceleration = robotMaxAcceleration;
+            profiledMaxTurnRate = robotMaxTurnRate;
+            // DriveBase PID Parameters
+            drivePidTolerance = 1.0;
+            turnPidTolerance = 1.0;
+            yDrivePidCoeffs = new TrcPidController.PidCoefficients(0.06, 0.0, 0.002, 0.0, 0.0);
+            yDrivePidPowerLimit = 1.0;
+            yDriveMaxPidRampRate = null;
+            turnPidCoeffs = new TrcPidController.PidCoefficients(0.02, 0.0, 0.002, 0.0, 0.0);
+            turnPidPowerLimit = 0.5;
+            turnMaxPidRampRate = null;
+            // PID Stall Detection
+            pidStallDetectionEnabled = true;
+            // PurePursuit Parameters
+            ppdFollowingDistance = 6.0;
+            velPidCoeffs = new TrcPidController.PidCoefficients(0.0, 0.0, 0.0, 1.0/profiledMaxVelocity, 0.0);
+        }   //DifferentialParams
+    }   //class DifferentialParams
+
+    /**
+     * This class contains the Mecanum Robot Parameters.
+     */
+    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_CPR = 4096.0;
+
+        public MecanumParams()
+        {
+            robotName = "MecanumRobot";
+            // Robot Dimensions
+            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;
+            // 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
+            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};
+            // 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)
+                        .setEncodersInverted(false, false);
+                    absoluteOdometry = new FtcPinpointOdometry("pinpointOdo", ppOdoConfig);
+                    headingWrapRangeLow = -180.0;
+                    headingWrapRangeHigh = 180.0;
+                }
+                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);
+                    absoluteOdometry = new FtcSparkFunOtos("sparkfunOtos", otosConfig);
+                }
+            }
+            else
+            {
+                absoluteOdometry = null;
+            }
+            // Drive Motor Odometry
+            xDrivePosScale = 1.0;   // in/count
+            yDrivePosScale = 1.0;   // in/count
+            // Robot Drive Characteristics
+            robotMaxVelocity = 80.0;        // inches/sec
+            robotMaxAcceleration  = 350.0;  // inches/sec2
+            robotMaxTurnRate = 80.0;        // degrees/sec
+            profiledMaxVelocity = robotMaxVelocity;
+            profiledMaxAcceleration = robotMaxAcceleration;
+            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);
+            xDrivePidPowerLimit = 1.0;
+            xDriveMaxPidRampRate = null;
+            yDrivePidCoeffs = new TrcPidController.PidCoefficients(0.06, 0.0, 0.002, 0.0, 0.0);
+            yDrivePidPowerLimit = 1.0;
+            yDriveMaxPidRampRate = null;
+            turnPidCoeffs = new TrcPidController.PidCoefficients(0.02, 0.0, 0.002, 0.0, 0.0);
+            turnPidPowerLimit = 0.5;
+            turnMaxPidRampRate = null;
+            // PID Stall Detection
+            pidStallDetectionEnabled = true;
+            // PurePursuit Parameters
+            ppdFollowingDistance = 6.0;
+            velPidCoeffs = new TrcPidController.PidCoefficients(0.0, 0.0, 0.0, 1.0/profiledMaxVelocity, 0.0);
+            // Vision
+            webCam1 = new Vision.FrontCamParams();
+            webCam2 = new Vision.BackCamParams();
+            limelight = new Vision.LimelightParams();
+            // Miscellaneous
+            indicatorName = "blinkin";
+        }   //MecanumParams
+    }   //class MecanumParams
+
+    /**
+     * This class contains the Swerve Drive Base Parameters.
+     */
+    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_CPR = 4096.0;
+
+        public SwerveParams()
+        {
+            robotName = "SwerveRobot";
+            // Robot Dimensions
+            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;
+            // 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
+            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};
+            // 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)
+                        .setEncodersInverted(false, false);
+                    absoluteOdometry = new FtcPinpointOdometry("pinpointOdo", ppOdoConfig);
+                    headingWrapRangeLow = -180.0;
+                    headingWrapRangeHigh = 180.0;
+                }
+                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);
+                    absoluteOdometry = new FtcSparkFunOtos("sparkfunOtos", otosConfig);
+                }
+            }
+            else
+            {
+                absoluteOdometry = null;
+            }
+            // Drive Motor Odometry
+            xDrivePosScale = 1.0;   // in/count
+            yDrivePosScale = 1.0;   // in/count
+            // Robot Drive Characteristics
+            robotMaxVelocity = 80.0;        // inches/sec
+            robotMaxAcceleration  = 350.0;  // inches/sec2
+            robotMaxTurnRate = 80.0;        // degrees/sec
+            profiledMaxVelocity = robotMaxVelocity;
+            profiledMaxAcceleration = robotMaxAcceleration;
+            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);
+            turnPidPowerLimit = 0.5;
+            turnMaxPidRampRate = null;
+            // PID Stall Detection
+            pidStallDetectionEnabled = true;
+            // PurePursuit Parameters
+            ppdFollowingDistance = 6.0;
+            velPidCoeffs = new TrcPidController.PidCoefficients(0.0, 0.0, 0.0, 1.0/profiledMaxVelocity, 0.0);
+            // Vision
+            webCam1 = new Vision.FrontCamParams();
+            webCam2 = new Vision.BackCamParams();
+            limelight = new Vision.LimelightParams();
+            // Miscellaneous
+            indicatorName = "blinkin";
+            // Steer Encoders
+            steerEncoderNames = new String[] {"lfSteerEncoder", "rfSteerEncoder", "lbSteerEncoder", "rbSteerEncoder"};
+            steerEncoderInverted = new boolean[] {false, false, false, false};
+            steerEncoderZeros = new double[] {0.474812, 0.467663, 0.541338, 0.545340};
+            steerZerosFilePath = RobotParams.Robot.STEER_ZERO_CAL_FILE;
+            // Steer Motors
+            steerMotorType = FtcMotorActuator.MotorType.CRServo;
+            steerMotorNames = new String[] {"lfSteerServo", "rfSteerServo", "lbSteerServo", "rbSteerServo"};
+            steerMotorInverted = new boolean[] {true, true, true, true};
+            steerMotorPidCoeffs = new TrcPidController.PidCoefficients(0.0, 0.0, 0.0, 0.0, 0.0);
+            steerMotorPidTolerance = 1.0;
+            // Swerve Modules
+            swerveModuleNames = new String[] {"lfWheel", "rfWheel", "lbWheel", "rbWheel"};
+        }   //SwerveParams
+    }   //class SwerveParams
+
     private final FtcRobotDrive.RobotInfo robotInfo;
     private final FtcRobotDrive robotDrive;
 
@@ -44,24 +332,23 @@ public class RobotBase
         switch (RobotParams.Preferences.robotType)
         {
             case VisionOnly:
-                robotInfo = new RobotParams.VisionOnlyParams();
+                robotInfo = new VisionOnlyParams();
                 robotDrive = null;
                 break;
 
             case DifferentialRobot:
-                robotInfo = new RobotParams.DifferentialParams();
+                robotInfo = new DifferentialParams();
                 robotDrive = RobotParams.Preferences.useDriveBase? new FtcDifferentialDrive(robotInfo): null;
                 break;
 
             case MecanumRobot:
-                robotInfo = new RobotParams.MecanumParams();
+                robotInfo = new MecanumParams();
                 robotDrive = RobotParams.Preferences.useDriveBase? new FtcMecanumDrive(robotInfo): null;
                 break;
 
             case SwerveRobot:
-                robotInfo = new RobotParams.SwerveParams();
+                robotInfo = new SwerveParams();
-                robotDrive = RobotParams.Preferences.useDriveBase?
+                robotDrive = RobotParams.Preferences.useDriveBase? new FtcSwerveDrive((SwerveParams) robotInfo): null;
-                    new FtcSwerveDrive((RobotParams.SwerveParams) robotInfo): null;
                 break;
 
             default:

TeamCode/src/main/java/teamcode/subsystems/Vision.java

@@ -20,9 +20,7 @@
  * SOFTWARE.
  */
 
-package teamcode.vision;
+package teamcode.subsystems;
-
-import com.qualcomm.robotcore.hardware.LED;
 
 import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection;
 import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
@@ -33,9 +31,11 @@ import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
 import org.opencv.imgproc.Imgproc;
 import org.openftc.easyopencv.OpenCvCamera;
 import org.openftc.easyopencv.OpenCvCameraFactory;
+import org.openftc.easyopencv.OpenCvCameraRotation;
 
 import java.util.ArrayList;
 
+import ftclib.drivebase.FtcRobotDrive;
 import ftclib.robotcore.FtcOpMode;
 import ftclib.vision.FtcCameraStreamProcessor;
 import ftclib.vision.FtcEocvColorBlobProcessor;
@@ -47,9 +47,10 @@ import ftclib.vision.FtcVisionAprilTag;
 import ftclib.vision.FtcVisionEocvColorBlob;
 import teamcode.Robot;
 import teamcode.RobotParams;
-import teamcode.subsystems.LEDIndicator;
 import trclib.pathdrive.TrcPose2D;
+import trclib.pathdrive.TrcPose3D;
 import trclib.robotcore.TrcDbgTrace;
+import trclib.vision.TrcHomographyMapper;
 import trclib.vision.TrcOpenCvColorBlobPipeline;
 import trclib.vision.TrcOpenCvDetector;
 import trclib.vision.TrcVisionTargetInfo;
@@ -63,6 +64,90 @@ public class Vision
 {
     private final String moduleName = getClass().getSimpleName();
 
+    /**
+     * This class contains the parameters of the front camera.
+     */
+    public static class FrontCamParams extends FtcRobotDrive.VisionInfo
+    {
+        public FrontCamParams()
+        {
+            camName = "Webcam 1";
+            camImageWidth = 640;
+            camImageHeight = 480;
+            camXOffset = 0.0;                   // Inches to the right from robot center
+            camYOffset = 2.0;                   // Inches forward from robot center
+            camZOffset = 9.75;                  // Inches up from the floor
+            camPitch = 15.0;                    // degrees down from horizontal
+            camYaw = 0.0;                       // degrees clockwise from robot front
+            camRoll = 0.0;
+            camPose = new TrcPose3D(camXOffset, camYOffset, camZOffset, camYaw, camPitch, camRoll);
+            camOrientation = OpenCvCameraRotation.UPRIGHT;
+            // Homography: cameraRect in pixels, worldRect in inches
+            cameraRect = new TrcHomographyMapper.Rectangle(
+                0.0, 120.0,                                             // Camera Top Left
+                camImageWidth -1, 120.0,                                // Camera Top Right
+                0.0, camImageHeight - 1,                                // Camera Bottom Left
+                camImageWidth - 1, camImageHeight - 1);                 // Camera Bottom Right
+            worldRect = new TrcHomographyMapper.Rectangle(
+                -12.5626, 48.0 - RobotParams.Robot.ROBOT_LENGTH/2.0 - camYOffset,   // World Top Left
+                11.4375, 44.75 - RobotParams.Robot.ROBOT_LENGTH/2.0 - camYOffset,   // World Top Right
+                -2.5625, 21.0 - RobotParams.Robot.ROBOT_LENGTH/2.0 - camYOffset,    // World Bottom Left
+                2.5626, 21.0 - RobotParams.Robot.ROBOT_LENGTH/2.0 - camYOffset);    // World Bottom Right
+        }   //FrontCamParams
+    }   //class FrontCamParams
+
+    /**
+     * This class contains the parameters of the back camera.
+     */
+    public static class BackCamParams extends FtcRobotDrive.VisionInfo
+    {
+        public BackCamParams()
+        {
+            camName = "Webcam 2";
+            camImageWidth = 640;
+            camImageHeight = 480;
+            camXOffset = 0.0;                   // Inches to the right from robot center
+            camYOffset = 2.0;                   // Inches forward from robot center
+            camZOffset = 9.75;                  // Inches up from the floor
+            camPitch = 15.0;                    // degrees down from horizontal
+            camYaw = 0.0;                       // degrees clockwise from robot front
+            camRoll = 0.0;
+            camPose = new TrcPose3D(camXOffset, camYOffset, camZOffset, camYaw, camPitch, camRoll);
+            camOrientation = OpenCvCameraRotation.UPRIGHT;
+            // Homography: cameraRect in pixels, worldRect in inches
+            cameraRect = new TrcHomographyMapper.Rectangle(
+                0.0, 120.0,                                             // Camera Top Left
+                camImageWidth -1, 120.0,                                // Camera Top Right
+                0.0, camImageHeight - 1,                                // Camera Bottom Left
+                camImageWidth - 1, camImageHeight - 1);                 // Camera Bottom Right
+            worldRect = new TrcHomographyMapper.Rectangle(
+                -12.5626, 48.0 - RobotParams.Robot.ROBOT_LENGTH/2.0 - camYOffset,   // World Top Left
+                11.4375, 44.75 - RobotParams.Robot.ROBOT_LENGTH/2.0 - camYOffset,   // World Top Right
+                -2.5625, 21.0 - RobotParams.Robot.ROBOT_LENGTH/2.0 - camYOffset,    // World Bottom Left
+                2.5626, 21.0 - RobotParams.Robot.ROBOT_LENGTH/2.0 - camYOffset);    // World Bottom Right
+        }   //BackCamParams
+    }   //class BackCamParams
+
+    /**
+     * This class contains the parameters of the Limelight vision processor.
+     */
+    public static class LimelightParams extends FtcRobotDrive.VisionInfo
+    {
+        public LimelightParams()
+        {
+            camName = "Limelight3a";
+            camImageWidth = 640;
+            camImageHeight = 480;
+            camXOffset = 0.0;                   // Inches to the right from robot center
+            camYOffset = 2.0;                   // Inches forward from robot center
+            camZOffset = 9.75;                  // Inches up from the floor
+            camPitch = 15.0;                    // degrees down from horizontal
+            camYaw = 0.0;                       // degrees clockwise from robot front
+            camRoll = 0.0;
+            camPose = new TrcPose3D(camXOffset, camYOffset, camZOffset, camYaw, camPitch, camRoll);
+        }   //LimelightParams
+    }   //class LimelightParams
+
     public enum ColorBlobType
     {
         RedBlob,
@@ -145,7 +230,8 @@ public class Vision
 
             tracer.traceInfo(moduleName, "Starting RawEocvColorBlobVision...");
             rawColorBlobPipeline = new FtcRawEocvColorBlobPipeline(
-                "rawColorBlobPipeline", colorConversion, redBlobColorThresholds, colorBlobFilterContourParams, true);
+                "rawColorBlobPipeline", colorConversion, redBlobColorThresholds, colorBlobFilterContourParams,
+                true, false);
             // By default, display original Mat.
             rawColorBlobPipeline.setVideoOutput(0);
             rawColorBlobPipeline.setAnnotateEnabled(true);
@@ -162,6 +248,7 @@ public class Vision
             {
                 limelightVision = new FtcLimelightVision(
                     robot.robotInfo.limelight.camName, robot.robotInfo.limelight.camPose, this::getTargetGroundOffset);
+                limelightVision.setPipeline(0);
             }
             // Creating Vision Processors for VisionPortal.
             ArrayList<VisionProcessor> visionProcessorsList = new ArrayList<>();
@@ -192,13 +279,13 @@ public class Vision
                 tracer.traceInfo(moduleName, "Starting ColorBlobVision...");
 
                 redBlobVision = new FtcVisionEocvColorBlob(
-                    "RedBlob", colorConversion, redBlobColorThresholds, colorBlobFilterContourParams, true,
+                    "RedBlob", colorConversion, redBlobColorThresholds, colorBlobFilterContourParams, true, false,
                     robot.robotInfo.webCam1.cameraRect, robot.robotInfo.webCam1.worldRect, true);
                 redBlobProcessor = redBlobVision.getVisionProcessor();
                 visionProcessorsList.add(redBlobProcessor);
 
                 blueBlobVision = new FtcVisionEocvColorBlob(
-                    "BlueBlob", colorConversion, blueBlobColorThresholds, colorBlobFilterContourParams, true,
+                    "BlueBlob", colorConversion, blueBlobColorThresholds, colorBlobFilterContourParams, true, false,
                     robot.robotInfo.webCam1.cameraRect, robot.robotInfo.webCam1.worldRect, true);
                 blueBlobProcessor = blueBlobVision.getVisionProcessor();
                 visionProcessorsList.add(blueBlobProcessor);
@@ -314,10 +401,12 @@ public class Vision
         int[] gainSetting = vision.getGainSetting();
         int currGain = vision.getCurrentGain();
 
-        robot.dashboard.displayPrintf(
+        if (exposureSetting != null && gainSetting != null)
-            lineNum, "Exp: %d (%d:%d), Gain: %d (%d:%d)",
+        {
-            currExposure, exposureSetting[0], exposureSetting[1],
+            robot.dashboard.displayPrintf(
-            currGain, gainSetting != null? gainSetting[0]: 0, gainSetting != null? gainSetting[1]: 0);
+                lineNum, "Exp: %d (%d:%d), Gain: %d (%d:%d)",
+                currExposure, exposureSetting[0], exposureSetting[1], currGain, gainSetting[0], gainSetting[1]);
+        }
     }   //displayExposureSettings
 
     /**
@@ -386,7 +475,9 @@ public class Vision
         if (lineNum != -1)
         {
             robot.dashboard.displayPrintf(
-                lineNum, "RawColorBlob: %s", colorBlobInfo != null? colorBlobInfo: "Not found.");
+                lineNum, "RawColorBlob: %s, heading=%.3f",
+                colorBlobInfo != null? colorBlobInfo: "Not found.",
+                robot.robotDrive != null? robot.robotDrive.driveBase.getHeading(): 0.0);
         }
 
         return colorBlobInfo;
@@ -598,7 +689,17 @@ public class Vision
     {
         TrcPose2D robotPose = null;
 
-        if (aprilTagVision != null)
+        if (isLimelightVisionEnabled())
+        {
+            TrcVisionTargetInfo<FtcLimelightVision.DetectedObject> aprilTagInfo =
+                getLimelightDetectedObject(FtcLimelightVision.ResultType.Fiducial, null, -1);
+
+            if (aprilTagInfo != null)
+            {
+                robotPose = aprilTagInfo.detectedObj.robotPose;
+            }
+        }
+        else if (isAprilTagVisionEnabled())
         {
             // Find any AprilTag in view.
             TrcVisionTargetInfo<FtcVisionAprilTag.DetectedObject> aprilTagInfo = getDetectedAprilTag(null, -1);
@@ -676,20 +777,17 @@ public class Vision
         ColorBlobType colorBlobType, int lineNum)
     {
         TrcVisionTargetInfo<TrcOpenCvColorBlobPipeline.DetectedObject> colorBlobInfo = null;
-        String colorBlobName = null;
 
         switch (colorBlobType)
         {
             case RedBlob:
                 colorBlobInfo = redBlobVision != null? redBlobVision.getBestDetectedTargetInfo(
                     null, this::compareDistance, 0.0, 0.0): null;
-                colorBlobName = LEDIndicator.RED_BLOB;
                 break;
 
             case BlueBlob:
                 colorBlobInfo = blueBlobVision != null? blueBlobVision.getBestDetectedTargetInfo(
                     null, this::compareDistance, 0.0, 0.0): null;
-                colorBlobName = LEDIndicator.BLUE_BLOB;
                 break;
 
             case AnyColorBlob:
@@ -717,7 +815,6 @@ public class Vision
                         colorBlobList.sort(this::compareDistance);
                     }
                     colorBlobInfo = colorBlobList.get(0);
-                    colorBlobName = colorBlobInfo.detectedObj.label;
                 }
                 break;
         }
@@ -730,13 +827,21 @@ public class Vision
 
         if (colorBlobInfo != null && robot.ledIndicator != null)
         {
-            robot.ledIndicator.setDetectedPattern(colorBlobName);
+            robot.ledIndicator.setDetectedPattern(colorBlobInfo.detectedObj.label);
         }
 
         if (lineNum != -1)
         {
-            robot.dashboard.displayPrintf(
+            if (colorBlobInfo != null)
-                lineNum, "%s: %s", colorBlobName, colorBlobInfo != null? colorBlobInfo: "Not found.");
+            {
+                robot.dashboard.displayPrintf(
+                    lineNum, "%s: %s (rotatedAngle=%.1f",
+                    colorBlobInfo.detectedObj.label, colorBlobInfo, colorBlobInfo.detectedObj.rotatedRect.angle);
+            }
+            else
+            {
+                robot.dashboard.displayPrintf(lineNum, "No colorblob found.");
+            }
         }
 
         return colorBlobInfo;
@@ -772,7 +877,7 @@ public class Vision
         TrcVisionTargetInfo<TrcOpenCvColorBlobPipeline.DetectedObject> a,
         TrcVisionTargetInfo<TrcOpenCvColorBlobPipeline.DetectedObject> b)
     {
-        return (int)((b.objPose.y - a.objPose.y)*100);
+        return (int)((a.objPose.y - b.objPose.y)*100);
     }   //compareDistance
 
 }   //class Vision