Added BradenTestOp to test vision

Also added a backup localizer for the vision

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousOpMode.java

@@ -33,7 +33,7 @@ public void runOpMode() throws InterruptedException {
         vision.init();
 
         drive = DriveFactory.getDrive(hardwareMap);
-        drive.setVisionLocalizer(vision.getVuforia());
+        // drive.setVisionLocalizer(vision.getVuforia());
 
        Carosel carosel = new Carosel(hardwareMap);
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BradenTestAutoOp.java

@@ -0,0 +1,39 @@
+package org.firstinspires.ftc.teamcode;
+
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.drive.BaseBotMecanumDrive;
+import org.firstinspires.ftc.teamcode.drive.BotMecanumDrive;
+import org.firstinspires.ftc.teamcode.drive.DriveFactory;
+import org.firstinspires.ftc.teamcode.drive.VisionLocalizer;
+import org.firstinspires.ftc.teamcode.subsystems.Carosel;
+import org.firstinspires.ftc.teamcode.subsystems.Vision;
+
+@Autonomous(name = "BradenTestAutoOp")
+public class BradenTestAutoOp extends LinearOpMode {
+
+    Vision vision = null;
+    Carosel carousel = null;
+    BaseBotMecanumDrive drive = null;
+
+    private Trajectory testTrajectory;
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+
+        vision = new Vision(this, hardwareMap, telemetry);
+        vision.init();
+
+        drive = DriveFactory.getDrive(hardwareMap);
+        // drive.setVisionLocalizer(vision.getVuforia());
+        // VisionLocalizer visionLocalizer = new VisionLocalizer(FtcDashboard.getInstance().getTelemetry(), vision.getVuforia());
+
+        waitForStart();
+
+        sleep(30000);
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/BotMecanumDrive.java

@@ -156,11 +156,12 @@ public BotMecanumDrive(HardwareMap hardwareMap) {
         leftRear.setDirection(DcMotorSimple.Direction.REVERSE);
 
         // TODO: if desired, use setLocalizer() to change the localization method
+        setLocalizer(new MecanumLocalizer(this));
     }
 
     @Override
     public void setVisionLocalizer(VuforiaLocalizer vuforia) {
-        setLocalizer(new VisionLocalizer(dashboard.getTelemetry(), vuforia));
+        setLocalizer(new VisionLocalizer(dashboard.getTelemetry(), vuforia, this));
     }
 
     @Override

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/VisionLocalizer.java

@@ -8,6 +8,9 @@
 import androidx.annotation.NonNull;
 import androidx.annotation.Nullable;
 
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
+import com.acmerobotics.roadrunner.drive.MecanumDrive;
 import com.acmerobotics.roadrunner.geometry.Pose2d;
 import com.acmerobotics.roadrunner.localization.Localizer;
 
@@ -40,15 +43,19 @@ public class VisionLocalizer implements Localizer {
     private VuforiaLocalizer vuforia; // Vuforia tells the robot where it is via vision
     private final VuforiaTrackables targets; // These are the targets we are looking for
 
+    MecanumDrive.MecanumLocalizer backupLocalizer;
+
     private boolean targetVisible = false;
     private Pose2d poseEstimate;
     // private Pose2d _poseEstimate;
 
-    public VisionLocalizer(Telemetry telemetry, VuforiaLocalizer vuforia) {
+    public VisionLocalizer(Telemetry telemetry, VuforiaLocalizer vuforia, BotMecanumDrive drive) {
 
         this.telemetry = telemetry;
         this.vuforia = vuforia;
 
+        backupLocalizer = new MecanumDrive.MecanumLocalizer(drive);
+
         // Targets
 
         targets = this.vuforia.loadTrackablesFromAsset("FreightFrenzy");
@@ -79,13 +86,14 @@ public VisionLocalizer(Telemetry telemetry, VuforiaLocalizer vuforia) {
 
     public void update() {
         targets.activate();
-
+        // TelemetryPacket packet = new TelemetryPacket();
         // check all the trackable targets to see which one (if any) is visible.
         targetVisible = false;
         for (VuforiaTrackable trackable : allTrackables) {
             if (((VuforiaTrackableDefaultListener) trackable.getListener()).isVisible()) {
                 telemetry.addData("Visible Target", trackable.getName());
                 targetVisible = true;
+                // packet.put("Target", trackable.getName());
 
                 // getUpdatedRobotLocation() will return null if no new information is available since
                 // the last time that call was made, or if the trackable is not currently visible.
@@ -101,6 +109,7 @@ public void update() {
         if (targetVisible) {
             // express position (translation) of robot in inches.
             VectorF translation = lastLocation.getTranslation();
+
             telemetry.addData("Pos (inches)", "{X, Y, Z} = %.1f, %.1f, %.1f",
                     translation.get(0) / mmPerInch, translation.get(1) / mmPerInch, translation.get(2) / mmPerInch);
 
@@ -112,8 +121,14 @@ public void update() {
             poseEstimate = new Pose2d(translation.get(0) / mmPerInch, translation.get(1) / mmPerInch, rotation.thirdAngle);
         } else {
             telemetry.addData("Visible Target", "none");
+            backupLocalizer.update();
+            poseEstimate = backupLocalizer.getPoseEstimate();
+            // change pose estimate to what ever the
         }
+        // FtcDashboard.getInstance().sendTelemetryPacket(packet);
+        backupLocalizer.setPoseEstimate(poseEstimate);
         telemetry.update();
+
         targets.deactivate();
     }