revert more changes

photonlib-java-examples/aimandrange/.wpilib/wpilib_preferences.json

@@ -2,5 +2,5 @@
     "enableCppIntellisense": false,
     "currentLanguage": "java",
     "projectYear": "2025",
-    "teamNumber": 1736
+    "teamNumber": 4512
 }

photonlib-java-examples/aimandrange/simgui.json

@@ -1,11 +1,4 @@
 {
-  "HALProvider": {
-    "Other Devices": {
-      "window": {
-        "visible": false
-      }
-    }
-  },
   "NTProvider": {
     "types": {
       "/FMSInfo": "FMSInfo",
@@ -114,38 +107,20 @@
         },
         "open": true
       },
-      "Shuffleboard": {
-        "open": true
-      },
       "SmartDashboard": {
-        "Drive": {
-          "open": true
-        },
         "VisionSystemSim-main": {
           "open": true
         },
         "open": true
-      },
-      "out": {
-        "open": true
       }
     }
   },
   "NetworkTables Info": {
-    "Clients": {
-      "open": true
-    },
-    "Connections": {
-      "open": true
-    },
-    "Server": {
-      "Publishers": {
-        "open": true
-      },
-      "open": true
-    },
     "visible": true
   },
+  "NetworkTables View": {
+    "visible": false
+  },
   "Plot": {
     "Plot <0>": {
       "plots": [
@@ -161,7 +136,7 @@
             0.0,
             0.8500000238418579
           ],
-          "height": 0,
+          "height": 338,
           "series": [
             {
               "color": [

photonlib-java-examples/aimandrange/src/main/java/frc/robot/Constants.java

@@ -42,7 +42,7 @@ public static class Vision {
         public static final String kCameraName = "YOUR CAMERA NAME";
         // Cam mounted facing forward, half a meter forward of center, half a meter up from center,
         // pitched upward.
-        private static final double camPitch = Units.degreesToRadians(10.0);
+        private static final double camPitch = Units.degreesToRadians(30.0);
         public static final Transform3d kRobotToCam =
                 new Transform3d(new Translation3d(0.5, 0.0, 0.5), new Rotation3d(0, -camPitch, 0));
 

photonlib-java-examples/aimandrange/src/main/java/frc/robot/Robot.java

@@ -26,139 +26,52 @@
 
 import static frc.robot.Constants.Vision.*;
 
-import edu.wpi.first.apriltag.AprilTagFieldLayout;
-import edu.wpi.first.apriltag.AprilTagFields;
-import edu.wpi.first.cscore.OpenCvLoader;
 import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.networktables.DoublePublisher;
-import edu.wpi.first.networktables.NetworkTableInstance;
-import edu.wpi.first.networktables.StructPublisher;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.simulation.BatterySim;
 import edu.wpi.first.wpilibj.simulation.RoboRioSim;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.subsystems.drivetrain.SwerveDrive;
-import java.util.Optional;
-import org.photonvision.EstimatedRobotPose;
 import org.photonvision.PhotonCamera;
-import org.photonvision.PhotonPoseEstimator;
-import org.photonvision.PhotonPoseEstimator.PoseStrategy;
+import org.photonvision.PhotonUtils;
 
 public class Robot extends TimedRobot {
-    PhotonPoseEstimator casadiEstimator =
-            new PhotonPoseEstimator(
-                    AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo),
-                    PoseStrategy.CASADI_MEME,
-                    kRobotToCam);
-    PhotonPoseEstimator oldEstimator =
-            new PhotonPoseEstimator(
-                    AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo),
-                    PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
-                    kRobotToCam);
-
-    StructPublisher<Pose3d> poseOldPub;
-    StructPublisher<Pose3d> poseNewPub;
-
-    DoublePublisher oldDtPub;
-    DoublePublisher newDtPub;
-
     private SwerveDrive drivetrain;
     private VisionSim visionSim;
     private PhotonCamera camera;
 
-    private XboxController controller;
+    private final double VISION_TURN_kP = 0.01;
+    private final double VISION_DES_ANGLE_deg = 0.0;
+    private final double VISION_STRAFE_kP = 0.5;
+    private final double VISION_DES_RANGE_m = 1.25;
 
-    private boolean FORCE_LOCAL = false;
+    private XboxController controller;
 
     @Override
     public void robotInit() {
-        if (isSimulation()) {
-            drivetrain = new SwerveDrive();
-        }
+        drivetrain = new SwerveDrive();
         camera = new PhotonCamera(kCameraName);
-        visionSim = new VisionSim(camera);
-        controller = new XboxController(0);
-
-        if (isReal() || FORCE_LOCAL) {
-            poseOldPub =
-                    NetworkTableInstance.getDefault()
-                            .getStructTopic("/out/pose_legacy", Pose3d.struct)
-                            .publish();
-            poseNewPub =
-                    NetworkTableInstance.getDefault()
-                            .getStructTopic("/out/pose_constrained", Pose3d.struct)
-                            .publish();
 
-            newDtPub = NetworkTableInstance.getDefault().getDoubleTopic("/out/dt_constrained").publish();
-
-            OpenCvLoader.forceStaticLoad();
-        }
+        visionSim = new VisionSim(camera);
 
-        if (isSimulation() && !FORCE_LOCAL) {
-            NetworkTableInstance.getDefault().stopServer();
-            NetworkTableInstance.getDefault().setServer("10.17.36.2");
-            NetworkTableInstance.getDefault().startClient4("matt-laptop");
-        }
+        controller = new XboxController(0);
     }
 
     @Override
     public void robotPeriodic() {
-        if (isSimulation()) {
-            // Update drivetrain subsystem
-            drivetrain.periodic();
-
-            // Log values to the dashboard
-            drivetrain.log();
-        }
-
-        if (isReal() || FORCE_LOCAL) {
-            // System.out.println("I shouldn't be here");
-            for (var result : camera.getAllUnreadResults()) {
-                Optional<EstimatedRobotPose> oldPoseEst =
-                        oldEstimator.update(result, camera.getCameraMatrix(), camera.getDistCoeffs(), null);
-                if (oldPoseEst.isPresent()) {
-                    Pose3d pose = oldPoseEst.get().estimatedPose;
-                    poseOldPub.set(pose);
-                }
+        // Update drivetrain subsystem
+        drivetrain.periodic();
 
-                var headingArr =
-                        SmartDashboard.getNumberArray("VisionSystemSim-main/Sim Field/Robot", new double[0]);
-                if (headingArr.length != 3) break;
-                var heading = Rotation2d.fromDegrees(headingArr[2]);
-                System.out.println(heading.getDegrees());
-
-                // some drift lol
-                var offset = Rotation2d.fromDegrees((Math.random() - 0.5));
-                var t1 = Timer.getFPGATimestamp();
-                Optional<EstimatedRobotPose> newPoseEst =
-                        casadiEstimator.update(
-                                result,
-                                camera.getCameraMatrix(),
-                                camera.getDistCoeffs(),
-                                new PhotonPoseEstimator.ConstrainedSolvepnpParams(true, heading.plus(offset), 100));
-                var t2 = Timer.getFPGATimestamp();
-                newDtPub.set(t2 - t1);
-                if (newPoseEst.isPresent() && newPoseEst.get().strategy == PoseStrategy.CASADI_MEME) {
-                    Pose3d pose = newPoseEst.get().estimatedPose;
-                    poseNewPub.set(pose);
-                    SmartDashboard.putNumber("yaw_offset", offset.getDegrees());
-                    SmartDashboard.putNumber(
-                            "yaw_error_deg",
-                            heading
-                                    .minus(newPoseEst.get().estimatedPose.getRotation().toRotation2d())
-                                    .getDegrees());
-                }
-            }
-        }
+        // Log values to the dashboard
+        drivetrain.log();
     }
 
     @Override
     public void disabledPeriodic() {
-        if (drivetrain != null) drivetrain.stop();
+        drivetrain.stop();
     }
 
     @Override
@@ -173,8 +86,52 @@ public void teleopPeriodic() {
         double strafe = -controller.getLeftX() * Constants.Swerve.kMaxLinearSpeed;
         double turn = -controller.getRightX() * Constants.Swerve.kMaxAngularSpeed;
 
+        // Read in relevant data from the Camera
+        boolean targetVisible = false;
+        double targetYaw = 0.0;
+        double targetRange = 0.0;
+        var results = camera.getAllUnreadResults();
+        if (!results.isEmpty()) {
+            // Camera processed a new frame since last
+            // Get the last one in the list.
+            var result = results.get(results.size() - 1);
+            if (result.hasTargets()) {
+                // At least one AprilTag was seen by the camera
+                for (var target : result.getTargets()) {
+                    if (target.getFiducialId() == 7) {
+                        // Found Tag 7, record its information
+                        targetYaw = target.getYaw();
+                        targetRange =
+                                PhotonUtils.calculateDistanceToTargetMeters(
+                                        0.5, // Measured with a tape measure, or in CAD.
+                                        1.435, // From 2024 game manual for ID 7
+                                        Units.degreesToRadians(-30.0), // Measured with a protractor, or in CAD.
+                                        Units.degreesToRadians(target.getPitch()));
+
+                        targetVisible = true;
+                    }
+                }
+            }
+        }
+
+        // Auto-align when requested
+        if (controller.getAButton() && targetVisible) {
+            // Driver wants auto-alignment to tag 7
+            // And, tag 7 is in sight, so we can turn toward it.
+            // Override the driver's turn and fwd/rev command with an automatic one
+            // That turns toward the tag, and gets the range right.
+            turn =
+                    (VISION_DES_ANGLE_deg - targetYaw) * VISION_TURN_kP * Constants.Swerve.kMaxAngularSpeed;
+            forward =
+                    (VISION_DES_RANGE_m - targetRange) * VISION_STRAFE_kP * Constants.Swerve.kMaxLinearSpeed;
+        }
+
         // Command drivetrain motors based on target speeds
         drivetrain.drive(forward, strafe, turn);
+
+        // Put debug information to the dashboard
+        SmartDashboard.putBoolean("Vision Target Visible", targetVisible);
+        SmartDashboard.putNumber("Vision Target Range (m)", targetRange);
     }
 
     @Override

photonlib-java-examples/aimandrange/src/main/java/frc/robot/VisionSim.java

@@ -48,7 +48,7 @@ public VisionSim(PhotonCamera cam_in) {
             visionSim.addAprilTags(kTagLayout);
             // Create simulated camera properties. These can be set to mimic your actual camera.
             var cameraProp = new SimCameraProperties();
-            cameraProp.setCalibration(960, 720, Rotation2d.fromDegrees(90));
+            cameraProp.setCalibration(320, 240, Rotation2d.fromDegrees(90));
             cameraProp.setCalibError(0.35, 0.10);
             cameraProp.setFPS(70);
             cameraProp.setAvgLatencyMs(30);