changes needed to get building in 2025 (minus changes to singleaxis) …

lib/cpp/subzero/motor/PidMotorController.cpp

@@ -74,7 +74,7 @@ void PidMotorController<
   m_absolutePositionEnabled = false;
   frc::SmartDashboard::PutNumber(m_name + "commanded rpm", rpm.value());
   m_controller.SetReference(rpm.value(),
-                            rev::CANSparkBase::ControlType::kVelocity);
+                            rev::spark::SparkLowLevel::ControlType::kVelocity);
 }
 
 template <typename TMotor, typename TController, typename TRelativeEncoder,

lib/include/subzero/autonomous/AutoFactory.h

@@ -4,7 +4,7 @@
 #include <frc/Filesystem.h>
 #include <frc2/command/CommandPtr.h>
 #include <frc2/command/WaitCommand.h>
-#include <pathplanner/lib/commands/FollowPathHolonomic.h>
+#include <pathplanner/lib/commands/PathPlannerAuto.h>
 #include <wpi/MemoryBuffer.h>
 
 #include <filesystem>
@@ -34,9 +34,31 @@ template <typename T> class AutoFactory {
 private:
   const std::map<T, std::string> &m_autos;
 
-  bool AutoFileExists(const std::string fileName);
+  bool AutoFileExists(const std::string fileName) {
+    const std::string filePath = frc::filesystem::GetDeployDirectory() +
+                                 "/pathplanner/autos/" + fileName + ".auto";
 
-  frc2::CommandPtr PathPlannerPathFromName(const std::string autoName);
+    // Use wpi::MemoryBuffer::GetFile and handle the wpi::expected result
+    auto fileBufferResult = wpi::MemoryBuffer::GetFile(filePath);
+
+    if (!fileBufferResult) {
+      // fileBufferResult is an error; the file does not exist
+      return false;
+    }
+
+    // fileBufferResult contains a valid std::unique_ptr<wpi::MemoryBuffer>
+    return true;
+  }
+
+  frc2::CommandPtr PathPlannerPathFromName(const std::string autoName) {
+    if (!AutoFileExists(autoName)) {
+      ConsoleWriter.logError("Auto Factory",
+                             "AUTO '%s' DOES NOT EXIST HELP US EVAN",
+                             autoName.c_str());
+      return EmptyCommand().ToPtr();
+    }
+    return pathplanner::PathPlannerAuto(autoName).ToPtr();
+  }
 
 public:
   /**
@@ -45,6 +67,18 @@ template <typename T> class AutoFactory {
    * @param type
    * @return frc2::CommandPtr The schedulable auto command
    */
-  frc2::CommandPtr GetAuto(T type);
+  frc2::CommandPtr GetAuto(T type) {
+    if (!m_autos.contains(type)) {
+      ConsoleWriter.logWarning(
+          "Auto Factory",
+          "Auto type %d does not exist, defaulting to empty "
+          "auto",
+          static_cast<int>(type));
+      return EmptyCommand().ToPtr();
+    }
+
+    return PathPlannerPathFromName(m_autos.at(type));
+  }
 };
-} // namespace subzero
+
+} // namespace subzero

lib/include/subzero/motor/PidMotorController.h

@@ -2,8 +2,7 @@
 
 #include <frc/controller/PIDController.h>
 #include <frc/smartdashboard/SmartDashboard.h>
-#include <rev/CANSparkBase.h>
-#include <rev/CANSparkMax.h>
+#include <rev/SparkMax.h>
 #include <units/angle.h>
 #include <units/angular_velocity.h>
 
@@ -205,7 +204,8 @@ class PidMotorControllerTuner {
 };
 
 class RevPidMotorController
-    : public PidMotorController<rev::CANSparkMax, rev::SparkPIDController,
-                                rev::SparkRelativeEncoder,
-                                rev::SparkAbsoluteEncoder> {};
+    : public PidMotorController<
+          rev::spark::SparkMax, rev::spark::SparkClosedLoopController,
+          rev::spark::SparkRelativeEncoder, rev::spark::SparkAbsoluteEncoder> {
+};
 } // namespace subzero

lib/include/subzero/singleaxis/BaseSingleAxisSubsystem.h

@@ -9,9 +9,9 @@
 #include <frc2/command/FunctionalCommand.h>
 #include <frc2/command/InstantCommand.h>
 #include <frc2/command/TrapezoidProfileSubsystem.h>
-#include <rev/CANSparkFlex.h>
-#include <rev/CANSparkMax.h>
-#include <rev/SparkMaxPIDController.h>
+#include <rev/SparkClosedLoopController.h>
+#include <rev/SparkFlex.h>
+#include <rev/SparkMax.h>
 #include <units/acceleration.h>
 #include <units/angle.h>
 #include <units/angular_acceleration.h>

lib/include/subzero/vision/PhotonVisionEstimators.h

@@ -14,7 +14,7 @@
 namespace subzero {
 
 /**
- * @brief Combines estimated poses from an aritrary number of PhotonVision
+ * @brief Combines estimated poses from an arbitrary number of PhotonVision
  * cameras and applies them to a Holonomic pose estimator
  *
  */
@@ -26,13 +26,12 @@ class PhotonVisionEstimators {
    */
   class PhotonCameraEstimator {
   public:
-    explicit PhotonCameraEstimator(photon::PhotonPoseEstimator &est)
-        : estimator{est} {
-      camera = estimator.GetCamera();
-    }
+    explicit PhotonCameraEstimator(photon::PhotonPoseEstimator &est,
+                                   photon::PhotonCamera &cam)
+        : estimator{est}, camera{cam} {}
 
     photon::PhotonPoseEstimator &estimator;
-    std::shared_ptr<photon::PhotonCamera> camera;
+    photon::PhotonCamera &camera; // Changed to reference
   };
 
   explicit PhotonVisionEstimators(std::vector<PhotonCameraEstimator> &estms,
@@ -46,34 +45,39 @@ class PhotonVisionEstimators {
     }
   }
 
-  std::optional<photon::EstimatedRobotPose>
-  GetPoseFromCamera(frc::Pose3d prevPose, photon::PhotonPoseEstimator &est,
-                    photon::PhotonCamera &camera, double maxAbmiguity = 0.2) {
+  std::vector<photon::EstimatedRobotPose>
+  GetPosesFromCamera(frc::Pose3d prevPose, photon::PhotonPoseEstimator &est,
+                     photon::PhotonCamera &camera, double maxAmbiguity = 0.2) {
     est.SetReferencePose(prevPose);
-    auto camResult = camera.GetLatestResult();
-
-    std::optional<photon::EstimatedRobotPose> visionEst;
 
-    if (camResult.HasTargets() &&
-        (camResult.targets.size() > 1 ||
-         camResult.targets[0].GetPoseAmbiguity() <= maxAbmiguity)) {
-      visionEst = est.Update(camResult);
-    }
-
-    if (visionEst.has_value()) {
-      auto estimatedPose = visionEst.value().estimatedPose;
-      // Replace 100_m with actual field dimensions
-      if (estimatedPose.X() > 0_m && estimatedPose.X() <= 100_m &&
-          estimatedPose.Y() > 0_m && estimatedPose.Y() <= 100_m) {
-        return visionEst;
+    std::vector<photon::EstimatedRobotPose> validPoses;
+
+    // Photon now returns all of the poses that we haven't integrated
+    // rather than just the latest, so we must return all that are valid
+    for (const auto &result : camera.GetAllUnreadResults()) {
+      // Check if the result has valid targets
+      if (result.HasTargets() &&
+          (result.targets.size() > 1 ||
+           result.targets[0].GetPoseAmbiguity() <= maxAmbiguity)) {
+        // Attempt to update the pose estimator with the result
+        std::optional<photon::EstimatedRobotPose> visionEst =
+            est.Update(result);
+
+        // If a valid pose is produced, check its boundaries
+        if (visionEst.has_value()) {
+          auto estimatedPose = visionEst.value().estimatedPose;
+          if (estimatedPose.X() > 0_m && estimatedPose.X() <= 100_m &&
+              estimatedPose.Y() > 0_m && estimatedPose.Y() <= 100_m) {
+            // Add the valid pose to the vector
+            validPoses.push_back(visionEst.value());
+          }
+        }
       }
     }
 
-    return std::nullopt;
+    return validPoses;
   }
 
-  // See:
-  // https://github.com/Hemlock5712/2023-Robot/blob/Joe-Test/src/main/java/frc/robot/subsystems/PoseEstimatorSubsystem.java
   /**
    * @brief Call in the drivetrain's Periodic method to update the estimated
    * robot's position
@@ -84,12 +88,18 @@ class PhotonVisionEstimators {
   void UpdateEstimatedGlobalPose(frc::SwerveDrivePoseEstimator<4U> &estimator,
                                  bool test) {
     for (auto &est : m_cameraEstimators) {
-      auto camPose =
-          GetPoseFromCamera(frc::Pose3d(estimator.GetEstimatedPosition()),
-                            est.estimator, *est.camera);
-      if (camPose.has_value()) {
-        auto pose = camPose.value();
-        AddVisionMeasurement(pose, estimator, est);
+      auto camPoses =
+          GetPosesFromCamera(frc::Pose3d(estimator.GetEstimatedPosition()),
+                             est.estimator, est.camera);
+      // if (camPose.has_value()) {
+      //   auto pose = camPose.value();
+      //   AddVisionMeasurement(pose, estimator, est);
+      // }
+
+      if (camPoses.size() > 0) {
+        for (auto &pose : camPoses) {
+          AddVisionMeasurement(pose, estimator, est);
+        }
       }
     }
   }
@@ -101,8 +111,6 @@ class PhotonVisionEstimators {
     Eigen::Matrix<double, 3, 1> estStdDevs = m_singleTagStdDevs;
     int numTags = 0;
     units::meter_t avgDist = 0_m;
-    // ConsoleWriter.logVerbose("Vision", "targets used length
-    // %d", pose.targetsUsed.size());
     for (const auto &tgt : pose.targetsUsed) {
       auto tagPose =
           photonEst.estimator.GetFieldLayout().GetTagPose(tgt.GetFiducialId());
@@ -119,7 +127,6 @@ class PhotonVisionEstimators {
 
     avgDist /= numTags;
     if (numTags > 1) {
-      // TODO:
       estStdDevs = m_multiTagStdDevs;
     }
 
@@ -151,4 +158,5 @@ class PhotonVisionEstimators {
 
   units::second_t lastEstTimestamp{0_s};
 };
+
 } // namespace subzero

lib/include/subzero/vision/TargetTracker.h

@@ -1,7 +1,7 @@
 #pragma once
 
 #include <frc/smartdashboard/Field2d.h>
-#include <pathplanner/lib/commands/FollowPathHolonomic.h>
+// #include <pathplanner/lib/commands/FollowPathHolonomic.h>
 
 #include <functional>
 #include <string>