Sync'd latest changes.

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

@@ -162,10 +162,10 @@ public class FtcAuto extends FtcOpMode
                 break;
         }
 
-        if (robot.vision != null)
+//        if (robot.vision != null)
-        {
+//        {
-            // Enabling vision early so we can detect target before match starts if necessary.
+//            // Enabling vision early so we can detect target before match starts if necessary.
-            // Only enable the necessary vision for that purpose.
+//            // Only enable the necessary vision for that purpose.
 //            if (robot.vision.aprilTagVision != null)
 //            {
 //                robot.globalTracer.traceInfo(moduleName, "Enabling AprilTagVision.");
@@ -183,7 +183,7 @@ public class FtcAuto extends FtcOpMode
 //                robot.globalTracer.traceInfo(moduleName, "Enabling BlueBlobVision.");
 //                robot.vision.setBlueBlobVisionEnabled(true);
 //            }
-        }
+//        }
     }   //robotInit
 
     //
@@ -268,9 +268,9 @@ public class FtcAuto extends FtcOpMode
             robot.battery.setEnabled(false);
         }
 
-        printPerformanceMetrics();
         robot.globalTracer.traceInfo(
             moduleName, "***** Stopping autonomous: " + TrcTimer.getCurrentTimeString() + " *****");
+        printPerformanceMetrics();
 
         if (TrcDbgTrace.isTraceLogOpened())
         {

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

@@ -54,6 +54,7 @@ public class FtcTeleOp extends FtcOpMode
     private boolean statusUpdateOn = false;
     private boolean relocalizing = false;
     private TrcPose2D robotFieldPose = null;
+    private Integer savedLimelightPipeline = null;
 
     //
     // Implements FtcOpMode abstract method.
@@ -136,16 +137,16 @@ public class FtcTeleOp extends FtcOpMode
         //
         if (robot.vision != null)
         {
-            if (robot.vision.limelightVision != null)
+            if (robot.vision.aprilTagVision != null)
-            {
-                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);
             }
+            else if (robot.vision.limelightVision != null)
+            {
+                robot.globalTracer.traceInfo(moduleName, "Enabling Limelight AprilTagVision.");
+                robot.vision.setLimelightVisionEnabled(0, true);
+            }
         }
     }   //startMode
 
@@ -163,9 +164,9 @@ public class FtcTeleOp extends FtcOpMode
         // Tell robot object opmode is about to stop so it can do the necessary cleanup for the mode.
         //
         robot.stopMode(prevMode);
-        printPerformanceMetrics();
         robot.globalTracer.traceInfo(
             moduleName, "***** Stopping TeleOp: " + TrcTimer.getCurrentTimeString() + " *****");
+        printPerformanceMetrics();
 
         if (TrcDbgTrace.isTraceLogOpened())
         {
@@ -187,6 +188,7 @@ public class FtcTeleOp extends FtcOpMode
     {
         if (slowPeriodicLoop)
         {
+            int lineNum = 1;
             //
             // DriveBase subsystem.
             //
@@ -214,9 +216,13 @@ public class FtcTeleOp extends FtcOpMode
                     {
                         robot.robotDrive.driveBase.arcadeDrive(inputs[1], inputs[2]);
                     }
-                    robot.dashboard.displayPrintf(
+
-                        1, "RobotDrive: Power=(%.2f,y=%.2f,rot=%.2f),Mode:%s",
+                    if (RobotParams.Preferences.doStatusUpdate || statusUpdateOn)
-                        inputs[0], inputs[1], inputs[2], robot.robotDrive.driveBase.getDriveOrientation());
+                    {
+                        robot.dashboard.displayPrintf(
+                            lineNum++, "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)
@@ -242,7 +248,7 @@ public class FtcTeleOp extends FtcOpMode
             // Display subsystem status.
             if (RobotParams.Preferences.doStatusUpdate || statusUpdateOn)
             {
-                robot.updateStatus(2);
+                robot.updateStatus(lineNum);
             }
         }
     }   //periodic
@@ -370,7 +376,7 @@ public class FtcTeleOp extends FtcOpMode
                 {
                     robot.globalTracer.traceInfo(moduleName, ">>>>> ZeroCalibrating.");
                     robot.cancelAll();
-                    robot.zeroCalibrate();
+                    robot.zeroCalibrate(moduleName, null);
                     if (robot.robotDrive != null && robot.robotDrive instanceof FtcSwerveDrive)
                     {
                         // Drive base is a Swerve Drive, align all steering wheels forward.
@@ -381,27 +387,47 @@ public class FtcTeleOp extends FtcOpMode
                 break;
 
             case Start:
-                if (robot.vision != null &&
+                if (robot.vision != null && robot.robotDrive != null)
-                    (robot.vision.isLimelightVisionEnabled() || robot.vision.isAprilTagVisionEnabled()) &&
-                    robot.robotDrive != null)
                 {
-                    // On press of the button, we will start looking for AprilTag for re-localization.
+                    boolean hasAprilTagVision = robot.vision.isAprilTagVisionEnabled();
-                    // On release of the button, we will set the robot's field location if we found the AprilTag.
+
-                    relocalizing = pressed;
+                    if (!hasAprilTagVision && robot.vision.limelightVision != null)
-                    if (!pressed)
                     {
-                        if (robotFieldPose != null)
+                        hasAprilTagVision = true;
+                        if (pressed)
                         {
-                            // Vision found an AprilTag, set the new robot field location.
+                            // Webcam AprilTag vision is not enable, enable Limelight AprilTag pipeline instead.
-                            robot.globalTracer.traceInfo(
+                            savedLimelightPipeline = robot.vision.limelightVision.getPipeline();
-                                moduleName, ">>>>> Finish re-localizing: pose=" + robotFieldPose);
+                            robot.vision.setLimelightVisionEnabled(0, true);
-                            robot.robotDrive.driveBase.setFieldPosition(robotFieldPose, false);
-                            robotFieldPose = null;
                         }
                     }
-                    else
+
+                    if (hasAprilTagVision)
                     {
-                        robot.globalTracer.traceInfo(moduleName, ">>>>> Start re-localizing ...");
+                        // 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.
+                        relocalizing = pressed;
+                        if (!pressed)
+                        {
+                            if (robotFieldPose != null)
+                            {
+                                // Vision found an AprilTag, set the new robot field location.
+                                robot.globalTracer.traceInfo(
+                                    moduleName, ">>>>> Finish re-localizing: pose=" + robotFieldPose);
+                                robot.robotDrive.driveBase.setFieldPosition(robotFieldPose, false);
+                                robotFieldPose = null;
+                                if (savedLimelightPipeline != null)
+                                {
+                                    // Done with AprilTag re-localization, restore previous Limelight pipeline.
+                                    robot.vision.limelightVision.setPipeline(savedLimelightPipeline);
+                                    savedLimelightPipeline = null;
+                                }
+                            }
+                        }
+                        else
+                        {
+                            robot.globalTracer.traceInfo(moduleName, ">>>>> Start re-localizing ...");
+                        }
                     }
                 }
                 break;

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

@@ -89,7 +89,7 @@ public class Robot
         // Create and initialize vision subsystems.
         if (RobotParams.Preferences.useVision &&
             (RobotParams.Preferences.tuneColorBlobVision ||
-             RobotParams.Preferences.useAprilTagVision ||
+             RobotParams.Preferences.useWebcamAprilTagVision ||
              RobotParams.Preferences.useColorBlobVision ||
              RobotParams.Preferences.useLimelightVision))
         {
@@ -115,8 +115,13 @@ public class Robot
             if (RobotParams.Preferences.useSubsystems)
             {
                 // Create subsystems.
-                // Zero calibrate all subsystems only at init time.
+
-                zeroCalibrate();
+                // Zero calibrate all subsystems only in Auto or if TeleOp is run standalone without prior Auto.
+                // There is no reason to zero calibrate again if Auto was run right before TeleOp.
+                if (runMode == TrcRobot.RunMode.AUTO_MODE || FtcAuto.autoChoices.alliance == null)
+                {
+                    zeroCalibrate(null, null);
+                }
                 // Create autotasks.
             }
         }
@@ -312,14 +317,6 @@ public class Robot
     {
     }   //zeroCalibrate
 
-    /**
-     * This method zero calibrates all subsystems.
-     */
-    public void zeroCalibrate()
-    {
-        zeroCalibrate(null, null);
-    }   //zeroCalibrate
-
     /**
      * This method sets the robot's starting position according to the autonomous choices.
      *

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

@@ -62,7 +62,7 @@ public class RobotParams
         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 useWebcamAprilTagVision     = false;
         public static final boolean useColorBlobVision          = false;
         public static final boolean showVisionView              = !inCompetition;
         public static final boolean showVisionStat              = false;

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

@@ -273,7 +273,7 @@ public class Vision
 //                FtcDashboard.getInstance().startCameraStream(cameraStreamProcessor, 0);
             }
 
-            if (RobotParams.Preferences.useAprilTagVision)
+            if (RobotParams.Preferences.useWebcamAprilTagVision)
             {
                 tracer.traceInfo(moduleName, "Starting Webcam AprilTagVision...");
                 FtcVisionAprilTag.Parameters aprilTagParams = new FtcVisionAprilTag.Parameters()
@@ -292,14 +292,14 @@ public class Vision
                 tracer.traceInfo(moduleName, "Starting Webcam ColorBlobVision...");
 
                 redBlobVision = new FtcVisionEocvColorBlob(
-                    "RedBlob", colorConversion, redBlobColorThresholds, colorBlobFilterContourParams, true,
+                    LEDIndicator.RED_BLOB, colorConversion, redBlobColorThresholds, colorBlobFilterContourParams,
-                    robot.robotInfo.webCam1.cameraRect, robot.robotInfo.webCam1.worldRect, true);
+                    true, robot.robotInfo.webCam1.cameraRect, robot.robotInfo.webCam1.worldRect, true);
                 redBlobProcessor = redBlobVision.getVisionProcessor();
                 visionProcessorsList.add(redBlobProcessor);
 
                 blueBlobVision = new FtcVisionEocvColorBlob(
-                    "BlueBlob", colorConversion, blueBlobColorThresholds, colorBlobFilterContourParams, true,
+                    LEDIndicator.BLUE_BLOB, colorConversion, blueBlobColorThresholds, colorBlobFilterContourParams,
-                    robot.robotInfo.webCam1.cameraRect, robot.robotInfo.webCam1.worldRect, true);
+                    true, robot.robotInfo.webCam1.cameraRect, robot.robotInfo.webCam1.worldRect, true);
                 blueBlobProcessor = blueBlobVision.getVisionProcessor();
                 visionProcessorsList.add(blueBlobProcessor);
             }
@@ -487,6 +487,11 @@ public class Vision
                 colorBlobInfo.detectedObj.label, colorBlobInfo.detectedObj.getRotatedRectVertices());
         }
 
+        if (colorBlobInfo != null && robot.ledIndicator != null)
+        {
+            robot.ledIndicator.setDetectedPattern(colorBlobInfo.detectedObj.label);
+        }
+
         if (lineNum != -1)
         {
             robot.dashboard.displayPrintf(