Sync'd latest changes from IntoTheDeep.

Updated ftclib, trclib.

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

@@ -43,7 +43,7 @@ import trclib.timer.TrcTimer;
 /**
  * This class contains the Autonomous Mode program.
  */
-@Autonomous(name="FtcAutonomous", group="Ftcxxxx")
+@Autonomous(name="FtcAutonomous", group="FtcTeam")
 public class FtcAuto extends FtcOpMode
 {
     private final String moduleName = getClass().getSimpleName();
@@ -73,9 +73,9 @@ public class FtcAuto extends FtcOpMode
     public static class AutoChoices
     {
         public double delay = 0.0;
-        public Alliance alliance = Alliance.RED_ALLIANCE;
+        public Alliance alliance = null;
-        public StartPos startPos = StartPos.LEFT;
+        public StartPos startPos = null;
-        public AutoStrategy strategy = AutoStrategy.DO_NOTHING;
+        public AutoStrategy strategy = null;
         public double xTarget = 0.0;
         public double yTarget = 0.0;
         public double turnTarget = 0.0;
@@ -192,7 +192,8 @@ public class FtcAuto extends FtcOpMode
 
     /**
      * This method is called periodically after robotInit() is called but before competition starts. For example,
-     * we can put vision code here to detect target before autonomous starts.
+     * we can put vision code here to detect target before autonomous starts, or code to move the subsystems to a
+     * particular configuration to be within the 18-inch starting restriction.
      */
     @Override
     public void initPeriodic()

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

@@ -29,6 +29,7 @@ import java.util.Locale;
 import ftclib.drivebase.FtcSwerveDrive;
 import ftclib.driverio.FtcGamepad;
 import ftclib.robotcore.FtcOpMode;
+import teamcode.subsystems.RumbleIndicator;
 import trclib.drivebase.TrcDriveBase;
 import trclib.pathdrive.TrcPose2D;
 import trclib.robotcore.TrcDbgTrace;
@@ -38,7 +39,7 @@ import trclib.timer.TrcTimer;
 /**
  * This class contains the TeleOp Mode program.
  */
-@TeleOp(name="FtcTeleOp", group="Ftcxxxx")
+@TeleOp(name="FtcTeleOp", group="FtcTeam")
 public class FtcTeleOp extends FtcOpMode
 {
     private final String moduleName = getClass().getSimpleName();
@@ -50,6 +51,7 @@ public class FtcTeleOp extends FtcOpMode
     private double turnPowerScale;
     private boolean driverAltFunc = false;
     private boolean operatorAltFunc = false;
+    private boolean statusUpdateOn = false;
     private boolean relocalizing = false;
     private TrcPose2D robotFieldPose = null;
 
@@ -85,11 +87,19 @@ public class FtcTeleOp extends FtcOpMode
         driverGamepad = new FtcGamepad("DriverGamepad", gamepad1);
         driverGamepad.setButtonEventHandler(this::driverButtonEvent);
         driverGamepad.setLeftStickInverted(false, true);
+        driverGamepad.setRightStickInverted(false, true);
 
         operatorGamepad = new FtcGamepad("OperatorGamepad", gamepad2);
         operatorGamepad.setButtonEventHandler(this::operatorButtonEvent);
+        operatorGamepad.setLeftStickInverted(false, true);
         operatorGamepad.setRightStickInverted(false, true);
 
+        if (RobotParams.Preferences.useRumble)
+        {
+            robot.driverRumble = new RumbleIndicator("DriverRumble", driverGamepad);
+            robot.operatorRumble = new RumbleIndicator("OperatorRumble", operatorGamepad);
+        }
+
         drivePowerScale = RobotParams.Robot.DRIVE_NORMAL_SCALE;
         turnPowerScale = RobotParams.Robot.TURN_NORMAL_SCALE;
         setDriveOrientation(RobotParams.Robot.DRIVE_ORIENTATION);
@@ -208,6 +218,19 @@ public class FtcTeleOp extends FtcOpMode
                         1, "RobotDrive: Power=(%.2f,y=%.2f,rot=%.2f),Mode:%s",
                         inputs[0], inputs[1], inputs[2], robot.robotDrive.driveBase.getDriveOrientation());
                 }
+                // Check for EndGame warning.
+                if (elapsedTime > RobotParams.Game.ENDGAME_DEADLINE)
+                {
+                    if (robot.driverRumble != null)
+                    {
+                        robot.driverRumble.setRumblePattern(RumbleIndicator.ENDGAME_DEADLINE);
+                    }
+
+                    if (robot.operatorRumble != null)
+                    {
+                        robot.operatorRumble.setRumblePattern(RumbleIndicator.ENDGAME_DEADLINE);
+                    }
+                }
             }
             //
             // Other subsystems.
@@ -217,7 +240,7 @@ public class FtcTeleOp extends FtcOpMode
                 // Analog control of subsystems.
             }
             // Display subsystem status.
-            if (RobotParams.Preferences.doStatusUpdate)
+            if (RobotParams.Preferences.doStatusUpdate || statusUpdateOn)
             {
                 robot.updateStatus(2);
             }
@@ -294,7 +317,14 @@ public class FtcTeleOp extends FtcOpMode
 
             case B:
             case X:
+                break;
+
             case Y:
+                if (driverAltFunc && pressed)
+                {
+                    robot.globalTracer.traceInfo(moduleName, ">>>>> Cancel all.");
+                    robot.cancelAll();
+                }
                 break;
 
             case LeftBumper:
@@ -303,18 +333,29 @@ public class FtcTeleOp extends FtcOpMode
                 break;
 
             case RightBumper:
-                // Press and hold for slow drive.
+                if (driverAltFunc)
-                if (pressed)
                 {
-                    robot.globalTracer.traceInfo(moduleName, ">>>>> DrivePower slow.");
+                    if (!RobotParams.Preferences.doStatusUpdate)
-                    drivePowerScale = RobotParams.Robot.DRIVE_SLOW_SCALE;
+                    {
-                    turnPowerScale = RobotParams.Robot.TURN_SLOW_SCALE;
+                        // Toggle status update ON/OFF.
+                        statusUpdateOn = !statusUpdateOn;
+                    }
                 }
                 else
                 {
-                    robot.globalTracer.traceInfo(moduleName, ">>>>> DrivePower normal.");
+                    // Press and hold for slow drive.
-                    drivePowerScale = RobotParams.Robot.DRIVE_NORMAL_SCALE;
+                    if (pressed)
-                    turnPowerScale = RobotParams.Robot.TURN_NORMAL_SCALE;
+                    {
+                        robot.globalTracer.traceInfo(moduleName, ">>>>> DrivePower slow.");
+                        drivePowerScale = RobotParams.Robot.DRIVE_SLOW_SCALE;
+                        turnPowerScale = RobotParams.Robot.TURN_SLOW_SCALE;
+                    }
+                    else
+                    {
+                        robot.globalTracer.traceInfo(moduleName, ">>>>> DrivePower normal.");
+                        drivePowerScale = RobotParams.Robot.DRIVE_NORMAL_SCALE;
+                        turnPowerScale = RobotParams.Robot.TURN_NORMAL_SCALE;
+                    }
                 }
                 break;
 
@@ -401,9 +442,9 @@ public class FtcTeleOp extends FtcOpMode
                 if (pressed)
                 {
                     // Zero calibrate all subsystems (arm, elevator and turret).
-                    robot.globalTracer.traceInfo(moduleName, ">>>>> ZeroCalibrate pressed.");
+                    robot.globalTracer.traceInfo(moduleName, ">>>>> ZeroCalibrating.");
                     robot.cancelAll();
-                    robot.zeroCalibrate(moduleName);
+                    robot.zeroCalibrate(moduleName, null);
                 }
                 break;
 

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

@@ -54,7 +54,7 @@ import trclib.timer.TrcTimer;
  * testing purposes. It provides numerous tests for diagnosing problems with the robot. It also provides tools
  * for tuning and calibration.
  */
-@TeleOp(name="FtcTest", group="Ftcxxxx")
+@TeleOp(name="FtcTest", group="FtcTeam")
 public class FtcTest extends FtcTeleOp
 {
     private final String moduleName = getClass().getSimpleName();
@@ -232,10 +232,16 @@ public class FtcTest extends FtcTeleOp
                     // Vision generally will impact performance, so we only enable it if it's needed.
                     if (robot.vision.aprilTagVision != null)
                     {
-                        robot.globalTracer.traceInfo(moduleName, "Enabling AprilTagVision.");
+                        robot.globalTracer.traceInfo(moduleName, "Enabling AprilTagVision for Webcam.");
                         robot.vision.setAprilTagVisionEnabled(true);
                     }
 
+                    if (robot.vision.limelightVision != null)
+                    {
+                        robot.globalTracer.traceInfo(moduleName, "Enabling AprilTagVision for Limelight.");
+                        robot.vision.setLimelightVisionEnabled(0, true);
+                    }
+
                     if (robot.vision.redBlobVision != null)
                     {
                         robot.globalTracer.traceInfo(moduleName, "Enabling RedBlobVision.");
@@ -247,12 +253,6 @@ public class FtcTest extends FtcTeleOp
                         robot.globalTracer.traceInfo(moduleName, "Enabling BlueBlobVision.");
                         robot.vision.setColorBlobVisionEnabled(Vision.ColorBlobType.BlueBlob, true);
                     }
-
-                    if (robot.vision.limelightVision != null)
-                    {
-                        robot.globalTracer.traceInfo(moduleName, "Enabling LimelightVision.");
-                        robot.vision.setLimelightVisionEnabled(0, true);
-                    }
                 }
                 break;
 
@@ -436,12 +436,12 @@ public class FtcTest extends FtcTeleOp
             {
                 case SENSORS_TEST:
                 case SUBSYSTEMS_TEST:
-                    doSensorsTest();
+                    doSensorsTest(lineNum);
                     break;
 
                 case VISION_TEST:
                 case TUNE_COLORBLOB_VISION:
-                    doVisionTest();
+                    doVisionTest(lineNum);
                     break;
 
                 case X_TIMED_DRIVE:
@@ -617,6 +617,17 @@ public class FtcTest extends FtcTeleOp
                     }
                     passToTeleOp = false;
                 }
+                else if (testChoices.test == Test.VISION_TEST &&
+                         robot.vision != null && robot.vision.isLimelightVisionEnabled())
+                {
+                    if (pressed)
+                    {
+                        int pipelineIndex = (robot.vision.limelightVision.getPipeline() + 1) %
+                                            Vision.LimelightParams.NUM_PIPELINES;
+                        robot.vision.limelightVision.setPipeline(pipelineIndex);
+                        robot.globalTracer.traceInfo(moduleName, "Switch Limelight pipeline to " + pipelineIndex);
+                    }
+                }
                 break;
 
             case LeftBumper:
@@ -1015,10 +1026,11 @@ public class FtcTest extends FtcTeleOp
      * This method reads all sensors and prints out their values. This is a very useful diagnostic tool to check
      * if all sensors are working properly. For encoders, since test sensor mode is also teleop mode, you can
      * operate the gamepads to turn the motors and check the corresponding encoder counts.
+     *
+     * @param lineNum specifies the starting line number on the dashboard to display sensor states.
      */
-    private void doSensorsTest()
+    private void doSensorsTest(int lineNum)
     {
-        int lineNum = 9;
         //
         // Read all sensors and display on the dashboard.
         // Drive the robot around to sample different locations of the field.
@@ -1062,17 +1074,19 @@ public class FtcTest extends FtcTeleOp
 
     /**
      * This method calls vision code to detect target objects and display their info.
+     *
+     * @param lineNum specifies the starting line number on the dashboard to display vision info.
      */
-    private void doVisionTest()
+    private void doVisionTest(int lineNum)
     {
         if (robot.vision != null)
         {
-            int lineNum = 9;
+            if (robot.vision.limelightVision != null)
-
-            if (robot.vision.vision != null)
             {
-                // displayExposureSettings is only available for VisionPortal.
+                robot.vision.getLimelightDetectedObject(
-                robot.vision.displayExposureSettings(lineNum++);
+                    robot.vision.limelightVision.getPipeline() == 0?
+                        FtcLimelightVision.ResultType.Fiducial: FtcLimelightVision.ResultType.Python,
+                    null, lineNum++);
             }
 
             if (robot.vision.rawColorBlobVision != null)
@@ -1087,17 +1101,18 @@ public class FtcTest extends FtcTeleOp
 
             if (robot.vision.redBlobVision != null)
             {
-                robot.vision.getDetectedColorBlob(Vision.ColorBlobType.RedBlob, lineNum++);
+                robot.vision.getDetectedColorBlob(Vision.ColorBlobType.RedBlob, 0.0, lineNum++);
             }
 
             if (robot.vision.blueBlobVision != null)
             {
-                robot.vision.getDetectedColorBlob(Vision.ColorBlobType.BlueBlob, lineNum++);
+                robot.vision.getDetectedColorBlob(Vision.ColorBlobType.BlueBlob, 0.0, lineNum++);
             }
 
-            if (robot.vision.limelightVision != null)
+            if (robot.vision.vision != null)
             {
-                robot.vision.getLimelightDetectedObject(FtcLimelightVision.ResultType.Fiducial, null, lineNum++);
+                // displayExposureSettings is only available for VisionPortal.
+                robot.vision.displayExposureSettings(lineNum++);
             }
         }
     }   //doVisionTest

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

@@ -31,11 +31,13 @@ import ftclib.robotcore.FtcOpMode;
 import ftclib.sensor.FtcRobotBattery;
 import teamcode.subsystems.LEDIndicator;
 import teamcode.subsystems.RobotBase;
+import teamcode.subsystems.RumbleIndicator;
 import teamcode.subsystems.Vision;
 import trclib.motor.TrcMotor;
 import trclib.motor.TrcServo;
 import trclib.pathdrive.TrcPose2D;
 import trclib.robotcore.TrcDbgTrace;
+import trclib.robotcore.TrcEvent;
 import trclib.robotcore.TrcRobot;
 import trclib.sensor.TrcDigitalInput;
 import trclib.timer.TrcTimer;
@@ -60,6 +62,8 @@ public class Robot
     public Vision vision;
     // Sensors and indicators.
     public LEDIndicator ledIndicator;
+    public RumbleIndicator driverRumble;
+    public RumbleIndicator operatorRumble;
     public FtcRobotBattery battery;
     // Subsystems.
     // Autotasks.
@@ -201,15 +205,21 @@ public class Robot
         if (vision != null)
         {
             vision.setCameraStreamEnabled(false);
-            if (vision.rawColorBlobVision != null)
+            if (vision.isRawColorBlobVisionEnabled())
             {
                 globalTracer.traceInfo(moduleName, "Disabling RawColorBlobVision.");
                 vision.setRawColorBlobVisionEnabled(false);
             }
 
-            if (vision.aprilTagVision != null)
+            if (vision.isLimelightVisionEnabled())
             {
-                globalTracer.traceInfo(moduleName, "Disabling AprilTagVision.");
+                globalTracer.traceInfo(moduleName, "Disabling LimelightVision.");
+                vision.setLimelightVisionEnabled(0, false);
+            }
+
+            if (vision.isAprilTagVisionEnabled())
+            {
+                globalTracer.traceInfo(moduleName, "Disabling Webcam AprilTagVision.");
                 vision.setAprilTagVisionEnabled(false);
             }
 
@@ -296,8 +306,9 @@ public class Robot
      * This method zero calibrates all subsystems.
      *
      * @param owner specifies the owner ID to check if the caller has ownership of the motor.
+     * @param event specifies the event to signal when the zero calibration is done.
      */
-    public void zeroCalibrate(String owner)
+    public void zeroCalibrate(String owner, TrcEvent event)
     {
     }   //zeroCalibrate
 
@@ -306,7 +317,7 @@ public class Robot
      */
     public void zeroCalibrate()
     {
-        zeroCalibrate(null);
+        zeroCalibrate(null, null);
     }   //zeroCalibrate
 
     /**
@@ -372,7 +383,7 @@ public class Robot
      */
     public TrcPose2D adjustPoseByAlliance(double x, double y, double heading, FtcAuto.Alliance alliance)
     {
-        return adjustPoseByAlliance(x, y, heading, alliance, true);
+        return adjustPoseByAlliance(x, y, heading, alliance, false);
     }   //adjustPoseByAlliance
 
     /**
@@ -391,13 +402,13 @@ public class Robot
     /**
      * This method adjusts the given pose in the red alliance to be the specified alliance.
      *
-     * @param pose specifies pose in the blue alliance in tile unit.
+     * @param pose specifies pose in the red alliance in tile unit.
      * @param alliance specifies the alliance to be converted to.
      * @return pose adjusted to be in the specified alliance in inches.
      */
     public TrcPose2D adjustPoseByAlliance(TrcPose2D pose, FtcAuto.Alliance alliance)
     {
-        return adjustPoseByAlliance(pose, alliance, true);
+        return adjustPoseByAlliance(pose, alliance, false);
     }   //adjustPoseByAlliance
 
     /**

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

@@ -30,7 +30,7 @@ import trclib.driverio.TrcGameController.DriveMode;
 import trclib.pathdrive.TrcPose2D;
 
 /**
- * This class contains robot and subsystem constants and parameters.
+ * This class contains robot constants and parameters.
  */
 public class RobotParams
 {
@@ -54,6 +54,7 @@ public class RobotParams
         public static final boolean showSubsystems              = true;
         public static final boolean useBlinkinLED               = false;
         public static final boolean useGobildaLED               = false;
+        public static final boolean useRumble                   = false;
         // Vision
         public static final boolean useVision                   = false;
         public static final boolean useWebCam                   = false;    // false to use Android phone camera.
@@ -63,7 +64,6 @@ public class RobotParams
         public static final boolean useCameraStreamProcessor    = false;
         public static final boolean useAprilTagVision           = false;
         public static final boolean useColorBlobVision          = false;
-        public static final boolean doWatershed                 = false;
         public static final boolean showVisionView              = !inCompetition;
         public static final boolean showVisionStat              = false;
         // Drive Base
@@ -110,6 +110,11 @@ public class RobotParams
             new TrcPose2D(0.0, 0.0, 0.0)    // TagId 4
         };
         // Robot start locations.
+        // Game elapsed times.
+        public static final double AUTO_PERIOD                      = 30.0;     // 30 seconds auto period
+        public static final double TELEOP_PERIOD                    = 120.0;    // 2 minutes teleop period
+        public static final double PARKING_TIME                     = 10.0;
+        public static final double ENDGAME_DEADLINE                 = TELEOP_PERIOD - PARKING_TIME;
     }   //class Game
 
     /**

TeamCode/src/main/java/teamcode/autocommands/CmdAuto.java

@@ -112,6 +112,8 @@ public class CmdAuto implements TrcRobot.RobotCommand
             switch (state)
             {
                 case START:
+                    // Set robot location according to auto choices.
+                    robot.setRobotStartPosition(autoChoices);
                     if (autoChoices.delay > 0.0)
                     {
                         robot.globalTracer.traceInfo(moduleName, "***** Do delay " + autoChoices.delay + "s.");

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

@@ -0,0 +1,68 @@
+/*
+ * Copyright (c) 2024 Titan Robotics Club (http://www.titanrobotics.com)
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package teamcode.subsystems;
+
+import ftclib.driverio.FtcGamepad;
+import ftclib.driverio.FtcGamepadRumble;
+import trclib.driverio.TrcPriorityIndicator;
+
+/**
+ * This class encapsulates the gamepad rumble to provide a priority indicator showing the status of the robot.
+ */
+public class RumbleIndicator
+{
+    // Rumble pattern names.
+    public static final String ENDGAME_DEADLINE = "EndGameDeadline";
+    private final TrcPriorityIndicator<?> indicator;
+
+    /**
+     * Constructor: Create an instance of the object.
+     *
+     * @param indicatorName specifies the indicator hardware name.
+     * @param gamepad specifies the gamepad to use for the rumble.
+     */
+    public RumbleIndicator(String indicatorName, FtcGamepad gamepad)
+    {
+        final FtcGamepadRumble.Pattern[] rumblePatternPriorities = {
+            // Highest priority.
+            new FtcGamepadRumble.Pattern(ENDGAME_DEADLINE, 3)
+            // Lowest priority.
+        };
+        indicator = new FtcGamepadRumble(indicatorName, gamepad.gamepad);
+        ((FtcGamepadRumble) indicator).setPatternPriorities(rumblePatternPriorities);
+    }   //RumbleIndicator
+
+    /**
+     * This method sets the rumble pattern ON for a period of time and turns off automatically afterwards.
+     *
+     * @param patternName specifies the name of the rumble pattern to turn on.
+     */
+    public void setRumblePattern(String patternName)
+    {
+        if (indicator != null)
+        {
+            indicator.setPatternState(patternName, true, 1.0);
+        }
+    }   //setRumblePattern
+
+}   //class RumbleIndicator

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

@@ -47,6 +47,7 @@ import ftclib.vision.FtcVisionAprilTag;
 import ftclib.vision.FtcVisionEocvColorBlob;
 import teamcode.Robot;
 import teamcode.RobotParams;
+import trclib.dataprocessor.TrcUtil;
 import trclib.pathdrive.TrcPose2D;
 import trclib.pathdrive.TrcPose3D;
 import trclib.robotcore.TrcDbgTrace;
@@ -74,25 +75,25 @@ public class Vision
             camName = "Webcam 1";
             camImageWidth = 640;
             camImageHeight = 480;
-            camXOffset = 0.0;                   // Inches to the right from robot center
+            camXOffset = -4.25;                 // Inches to the right from robot center
-            camYOffset = 2.0;                   // Inches forward from robot center
+            camYOffset = 5.5;                   // Inches forward from robot center
-            camZOffset = 9.75;                  // Inches up from the floor
+            camZOffset = 10.608;                // Inches up from the floor
-            camPitch = 15.0;                    // degrees down from horizontal
+            camYaw = -2.0;                      // degrees clockwise from robot forward
-            camYaw = 0.0;                       // degrees clockwise from robot front
+            camPitch = -32.346629699;           // degrees up from horizontal
             camRoll = 0.0;
             camPose = new TrcPose3D(camXOffset, camYOffset, camZOffset, camYaw, camPitch, camRoll);
             camOrientation = OpenCvCameraRotation.UPRIGHT;
             // Homography: cameraRect in pixels, worldRect in inches
             cameraRect = new TrcHomographyMapper.Rectangle(
-                0.0, 120.0,                                             // Camera Top Left
+                14.0, 28.0,                     // Camera Top Left
-                camImageWidth -1, 120.0,                                // Camera Top Right
+                612.0, 33.0,                    // Camera Top Right
-                0.0, camImageHeight - 1,                                // Camera Bottom Left
+                56.0, 448.0,                    // Camera Bottom Left
-                camImageWidth - 1, camImageHeight - 1);                 // Camera Bottom Right
+                581.0, 430.5);                  // Camera Bottom Right
             worldRect = new TrcHomographyMapper.Rectangle(
-                -12.5626, 48.0 - RobotParams.Robot.ROBOT_LENGTH/2.0 - camYOffset,   // World Top Left
+                -19.0, 37.5,                    // World Top Left
-                11.4375, 44.75 - RobotParams.Robot.ROBOT_LENGTH/2.0 - camYOffset,   // World Top Right
+                24.0, 37.5,                     // World Top Right
-                -2.5625, 21.0 - RobotParams.Robot.ROBOT_LENGTH/2.0 - camYOffset,    // World Bottom Left
+                -4.75, 9.0,                     // World Bottom Left
-                2.5626, 21.0 - RobotParams.Robot.ROBOT_LENGTH/2.0 - camYOffset);    // World Bottom Right
+                6.25, 9.0);                     // World Bottom Right
         }   //FrontCamParams
     }   //class FrontCamParams
 
@@ -109,8 +110,8 @@ public class Vision
             camXOffset = 0.0;                   // Inches to the right from robot center
             camYOffset = 2.0;                   // Inches forward from robot center
             camZOffset = 9.75;                  // Inches up from the floor
-            camPitch = 15.0;                    // degrees down from horizontal
             camYaw = 0.0;                       // degrees clockwise from robot front
+            camPitch = 15.0;                    // degrees down from horizontal
             camRoll = 0.0;
             camPose = new TrcPose3D(camXOffset, camYOffset, camZOffset, camYaw, camPitch, camRoll);
             camOrientation = OpenCvCameraRotation.UPRIGHT;
@@ -133,16 +134,20 @@ public class Vision
      */
     public static class LimelightParams extends FtcRobotDrive.VisionInfo
     {
+        public static final int NUM_PIPELINES = 4;
+
         public LimelightParams()
         {
             camName = "Limelight3a";
             camImageWidth = 640;
             camImageHeight = 480;
-            camXOffset = 0.0;                   // Inches to the right from robot center
+            camHFov = 80.0;                             // in degrees
-            camYOffset = 2.0;                   // Inches forward from robot center
+            camVFov = 56.0;                             // in degrees
-            camZOffset = 9.75;                  // Inches up from the floor
+            camXOffset = 135.47*TrcUtil.INCHES_PER_MM;  // Inches to the right from robot center
-            camPitch = 15.0;                    // degrees down from horizontal
+            camYOffset = 2.073;                         // Inches forward from robot center
-            camYaw = 0.0;                       // degrees clockwise from robot front
+            camZOffset = 10.758;                        // Inches up from the floor
+            camYaw = -3.438;                            // degrees clockwise from robot front
+            camPitch = 0.0;                             // degrees down from horizontal
             camRoll = 0.0;
             camPose = new TrcPose3D(camXOffset, camYOffset, camZOffset, camYaw, camPitch, camRoll);
         }   //LimelightParams
@@ -160,8 +165,8 @@ public class Vision
     //
     // YCrCb Color Space.
     private static final int colorConversion = Imgproc.COLOR_RGB2YCrCb;
-    private static final double[] redBlobColorThresholds = {20.0, 120.0, 170.0, 220.0, 80.0, 120.0};
+    private static final double[] redBlobColorThresholds = {10.0, 180.0, 170.0, 240.0, 80.0, 120.0};
-    private static final double[] blueBlobColorThresholds = {20.0, 60.0, 100.0, 150.0, 150.0, 180.0};
+    private static final double[] blueBlobColorThresholds = {0.0, 180.0, 80.0, 150.0, 150.0, 200.0};
     private static final TrcOpenCvColorBlobPipeline.FilterContourParams colorBlobFilterContourParams =
         new TrcOpenCvColorBlobPipeline.FilterContourParams()
             .setMinArea(500.0)
@@ -171,6 +176,15 @@ public class Vision
             .setSolidityRange(0.0, 100.0)
             .setVerticesRange(0.0, 1000.0)
             .setAspectRatioRange(0.5, 2.5);
+    private static final TrcOpenCvColorBlobPipeline.FilterContourParams tuneFilterContourParams =
+        new TrcOpenCvColorBlobPipeline.FilterContourParams()
+            .setMinArea(10.0)
+            .setMinPerimeter(12.0)
+            .setWidthRange(0.0, 1000.0)
+            .setHeightRange(0.0, 1000.0)
+            .setSolidityRange(0.0, 100.0)
+            .setVerticesRange(0.0, 1000.0)
+            .setAspectRatioRange(0.5, 2.5);
 
     private final TrcDbgTrace tracer;
     private final Robot robot;
@@ -178,7 +192,7 @@ public class Vision
     private FtcRawEocvColorBlobPipeline rawColorBlobPipeline;
     public FtcRawEocvVision rawColorBlobVision;
     public FtcLimelightVision limelightVision;
-    public FtcCameraStreamProcessor cameraStreamProcessor;
+    private FtcCameraStreamProcessor cameraStreamProcessor;
     public FtcVisionAprilTag aprilTagVision;
     private AprilTagProcessor aprilTagProcessor;
     public FtcVisionEocvColorBlob redBlobVision;
@@ -204,9 +218,9 @@ public class Vision
 
         this.tracer = new TrcDbgTrace();
         this.robot = robot;
-        this.webcam1 = robot.robotInfo.webCam1 != null?
+        webcam1 = robot.robotInfo.webCam1 != null?
             opMode.hardwareMap.get(WebcamName.class, robot.robotInfo.webCam1.camName): null;
-        this.webcam2 = robot.robotInfo.webCam2 != null?
+        webcam2 = robot.robotInfo.webCam2 != null?
             opMode.hardwareMap.get(WebcamName.class, robot.robotInfo.webCam2.camName): null;
         if (RobotParams.Preferences.tuneColorBlobVision && webcam1 != null)
         {
@@ -230,8 +244,7 @@ public class Vision
 
             tracer.traceInfo(moduleName, "Starting RawEocvColorBlobVision...");
             rawColorBlobPipeline = new FtcRawEocvColorBlobPipeline(
-                "rawColorBlobPipeline", colorConversion, redBlobColorThresholds, colorBlobFilterContourParams,
+                "rawColorBlobPipeline", colorConversion, redBlobColorThresholds, tuneFilterContourParams, true);
-                true, false);
             // By default, display original Mat.
             rawColorBlobPipeline.setVideoOutput(0);
             rawColorBlobPipeline.setAnnotateEnabled(true);
@@ -262,7 +275,7 @@ public class Vision
 
             if (RobotParams.Preferences.useAprilTagVision)
             {
-                tracer.traceInfo(moduleName, "Starting AprilTagVision...");
+                tracer.traceInfo(moduleName, "Starting Webcam AprilTagVision...");
                 FtcVisionAprilTag.Parameters aprilTagParams = new FtcVisionAprilTag.Parameters()
                     .setDrawTagIdEnabled(true)
                     .setDrawTagOutlineEnabled(true)
@@ -276,16 +289,16 @@ public class Vision
 
             if (RobotParams.Preferences.useColorBlobVision && robot.robotInfo.webCam1 != null)
             {
-                tracer.traceInfo(moduleName, "Starting ColorBlobVision...");
+                tracer.traceInfo(moduleName, "Starting Webcam ColorBlobVision...");
 
                 redBlobVision = new FtcVisionEocvColorBlob(
-                    "RedBlob", colorConversion, redBlobColorThresholds, colorBlobFilterContourParams, true, false,
+                    "RedBlob", colorConversion, redBlobColorThresholds, colorBlobFilterContourParams, true,
                     robot.robotInfo.webCam1.cameraRect, robot.robotInfo.webCam1.worldRect, true);
                 redBlobProcessor = redBlobVision.getVisionProcessor();
                 visionProcessorsList.add(redBlobProcessor);
 
                 blueBlobVision = new FtcVisionEocvColorBlob(
-                    "BlueBlob", colorConversion, blueBlobColorThresholds, colorBlobFilterContourParams, true, false,
+                    "BlueBlob", colorConversion, blueBlobColorThresholds, colorBlobFilterContourParams, true,
                     robot.robotInfo.webCam1.cameraRect, robot.robotInfo.webCam1.worldRect, true);
                 blueBlobProcessor = blueBlobVision.getVisionProcessor();
                 visionProcessorsList.add(blueBlobProcessor);
@@ -300,7 +313,8 @@ public class Vision
                     // Use USB webcams.
                     vision = new FtcVision(
                         webcam1, webcam2, robot.robotInfo.webCam1.camImageWidth, robot.robotInfo.webCam1.camImageHeight,
-                        RobotParams.Preferences.showVisionView, RobotParams.Preferences.showVisionStat, visionProcessors);
+                        RobotParams.Preferences.showVisionView, RobotParams.Preferences.showVisionStat,
+                        visionProcessors);
                 }
                 else
                 {
@@ -309,7 +323,8 @@ public class Vision
                         RobotParams.Preferences.useBuiltinCamBack?
                             BuiltinCameraDirection.BACK: BuiltinCameraDirection.FRONT,
                         robot.robotInfo.webCam1.camImageWidth, robot.robotInfo.webCam1.camImageHeight,
-                        RobotParams.Preferences.showVisionView, RobotParams.Preferences.showVisionStat, visionProcessors);
+                        RobotParams.Preferences.showVisionView, RobotParams.Preferences.showVisionStat,
+                        visionProcessors);
                 }
 
                 // Disable all vision until they are needed.
@@ -498,6 +513,8 @@ public class Vision
                 limelightVision.setPipeline(pipelineIndex);
             }
             limelightVision.setVisionEnabled(enabled);
+            tracer.traceInfo(moduleName, "Pipeline %d is %s: running=%s",
+                             pipelineIndex, enabled? "enabled": "disabled", limelightVision.limelight.isRunning());
         }
     }   //setLimelightVisionEnabled
 
@@ -523,26 +540,47 @@ public class Vision
         FtcLimelightVision.ResultType resultType, String label, int lineNum)
     {
         TrcVisionTargetInfo<FtcLimelightVision.DetectedObject> limelightInfo = null;
-        String objectName = null;
 
         if (limelightVision != null)
         {
-            limelightInfo = limelightVision.getBestDetectedTargetInfo(resultType, label, null);
+            String objectName = null;
+            int pipelineIndex = -1;
+            Double robotHeading = robot.robotDrive != null? robot.robotDrive.driveBase.getHeading(): null;
+
+            limelightInfo = limelightVision.getBestDetectedTargetInfo(resultType, label, robotHeading, null);
             if (limelightInfo != null)
             {
-                objectName = limelightInfo.detectedObj.label;
+                pipelineIndex = limelightVision.getPipeline();
+                switch (pipelineIndex)
+                {
+                    case 0:
+                        objectName = LEDIndicator.APRIL_TAG;
+                        break;
+
+                    case 1:
+                        objectName = LEDIndicator.RED_BLOB;
+                        break;
+
+                    case 2:
+                        objectName = LEDIndicator.BLUE_BLOB;
+                        break;
+
+                    default:
+                        break;
+                }
             }
-        }
 
-        if (objectName != null && robot.ledIndicator != null)
+            if (objectName != null && robot.ledIndicator != null)
-        {
+            {
-            robot.ledIndicator.setDetectedPattern(objectName);
+                robot.ledIndicator.setDetectedPattern(objectName);
-        }
+            }
 
-        if (lineNum != -1)
+            if (lineNum != -1)
-        {
+            {
-            robot.dashboard.displayPrintf(
+                robot.dashboard.displayPrintf(
-                lineNum, "%s: %s", objectName, limelightInfo != null? limelightInfo: "Not found.");
+                    lineNum, "%s(%d): %s",
+                    objectName, pipelineIndex, limelightInfo != null? limelightInfo: "Not found.");
+            }
         }
 
         return limelightInfo;
@@ -770,11 +808,12 @@ public class Vision
      * This method calls ColorBlob vision to detect the specified color blob object.
      *
      * @param colorBlobType specifies the color blob type to be detected.
+     * @param groundOffset specifies the ground offset of the detected sample.
      * @param lineNum specifies the dashboard line number to display the detected object info, -1 to disable printing.
      * @return detected color blob object info.
      */
     public TrcVisionTargetInfo<TrcOpenCvColorBlobPipeline.DetectedObject> getDetectedColorBlob(
-        ColorBlobType colorBlobType, int lineNum)
+        ColorBlobType colorBlobType, double groundOffset, int lineNum)
     {
         TrcVisionTargetInfo<TrcOpenCvColorBlobPipeline.DetectedObject> colorBlobInfo = null;
 
@@ -782,12 +821,12 @@ public class Vision
         {
             case RedBlob:
                 colorBlobInfo = redBlobVision != null? redBlobVision.getBestDetectedTargetInfo(
-                    null, this::compareDistance, 0.0, 0.0): null;
+                    null, this::compareDistance, groundOffset, robot.robotInfo.webCam1.camZOffset): null;
                 break;
 
             case BlueBlob:
                 colorBlobInfo = blueBlobVision != null? blueBlobVision.getBestDetectedTargetInfo(
-                    null, this::compareDistance, 0.0, 0.0): null;
+                    null, this::compareDistance, groundOffset, robot.robotInfo.webCam1.camZOffset): null;
                 break;
 
             case AnyColorBlob:
@@ -795,14 +834,14 @@ public class Vision
                     new ArrayList<>();
 
                 colorBlobInfo = redBlobVision != null ? redBlobVision.getBestDetectedTargetInfo(
-                    null, this::compareDistance, 0.0, 0.0) : null;
+                    null, this::compareDistance, groundOffset, robot.robotInfo.webCam1.camZOffset) : null;
                 if (colorBlobInfo != null)
                 {
                     colorBlobList.add(colorBlobInfo);
                 }
 
                 colorBlobInfo = blueBlobVision != null ? blueBlobVision.getBestDetectedTargetInfo(
-                    null, this::compareDistance, 0.0, 0.0) : null;
+                    null, this::compareDistance, groundOffset, robot.robotInfo.webCam1.camZOffset) : null;
                 if (colorBlobInfo != null)
                 {
                     colorBlobList.add(colorBlobInfo);
@@ -835,7 +874,7 @@ public class Vision
             if (colorBlobInfo != null)
             {
                 robot.dashboard.displayPrintf(
-                    lineNum, "%s: %s (rotatedAngle=%.1f",
+                    lineNum, "%s: %s (rotatedAngle=%.1f)",
                     colorBlobInfo.detectedObj.label, colorBlobInfo, colorBlobInfo.detectedObj.rotatedRect.angle);
             }
             else
@@ -861,6 +900,10 @@ public class Vision
         {
             offset = 5.75;
         }
+        else if ( resultType == FtcLimelightVision.ResultType.Python)
+        {
+            offset = 10.0;
+        }
 
         return offset;
     }   //getTargetGroundOffset