Examples Clean-Up (#1408)

photonlib-cpp-examples/poseest/src/main/include/Constants.h

@@ -0,0 +1,118 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <numbers>
+
+#include <frc/apriltag/AprilTagFieldLayout.h>
+#include <frc/apriltag/AprilTagFields.h>
+#include <frc/controller/SimpleMotorFeedforward.h>
+#include <frc/geometry/Transform3d.h>
+#include <frc/geometry/Translation2d.h>
+#include <units/length.h>
+
+namespace constants {
+namespace Vision {
+inline constexpr std::string_view kCameraName{"YOUR CAMERA NAME"};
+inline const frc::Transform3d kRobotToCam{
+    frc::Translation3d{0.5_m, 0.0_m, 0.5_m},
+    frc::Rotation3d{0_rad, 0_rad, 0_rad}};
+inline const frc::AprilTagFieldLayout kTagLayout{
+    frc::LoadAprilTagLayoutField(frc::AprilTagField::k2024Crescendo)};
+
+inline const Eigen::Matrix<double, 3, 1> kSingleTagStdDevs{4, 4, 8};
+inline const Eigen::Matrix<double, 3, 1> kMultiTagStdDevs{0.5, 0.5, 1};
+}  // namespace Vision
+namespace Swerve {
+
+inline constexpr units::meter_t kTrackWidth{18.5_in};
+inline constexpr units::meter_t kTrackLength{18.5_in};
+inline constexpr units::meter_t kRobotWidth{25_in + 3.25_in * 2};
+inline constexpr units::meter_t kRobotLength{25_in + 3.25_in * 2};
+inline constexpr units::meters_per_second_t kMaxLinearSpeed{15.5_fps};
+inline constexpr units::radians_per_second_t kMaxAngularSpeed{720_deg_per_s};
+inline constexpr units::meter_t kWheelDiameter{4_in};
+inline constexpr units::meter_t kWheelCircumference{kWheelDiameter *
+                                                    std::numbers::pi};
+
+inline constexpr double kDriveGearRatio = 6.75;
+inline constexpr double kSteerGearRatio = 12.8;
+
+inline constexpr units::meter_t kDriveDistPerPulse =
+    kWheelCircumference / 1024.0 / kDriveGearRatio;
+inline constexpr units::radian_t kSteerRadPerPulse =
+    units::radian_t{2 * std::numbers::pi} / 1024.0;
+
+inline constexpr double kDriveKP = 1.0;
+inline constexpr double kDriveKI = 0.0;
+inline constexpr double kDriveKD = 0.0;
+
+inline constexpr double kSteerKP = 20.0;
+inline constexpr double kSteerKI = 0.0;
+inline constexpr double kSteerKD = 0.25;
+
+inline const frc::SimpleMotorFeedforward<units::meters> kDriveFF{
+    0.25_V, 2.5_V / 1_mps, 0.3_V / 1_mps_sq};
+
+inline const frc::SimpleMotorFeedforward<units::radians> kSteerFF{
+    0.5_V, 0.25_V / 1_rad_per_s, 0.01_V / 1_rad_per_s_sq};
+
+struct ModuleConstants {
+ public:
+  const int moduleNum;
+  const int driveMotorId;
+  const int driveEncoderA;
+  const int driveEncoderB;
+  const int steerMotorId;
+  const int steerEncoderA;
+  const int steerEncoderB;
+  const double angleOffset;
+  const frc::Translation2d centerOffset;
+
+  ModuleConstants(int moduleNum, int driveMotorId, int driveEncoderA,
+                  int driveEncoderB, int steerMotorId, int steerEncoderA,
+                  int steerEncoderB, double angleOffset, units::meter_t xOffset,
+                  units::meter_t yOffset)
+      : moduleNum(moduleNum),
+        driveMotorId(driveMotorId),
+        driveEncoderA(driveEncoderA),
+        driveEncoderB(driveEncoderB),
+        steerMotorId(steerMotorId),
+        steerEncoderA(steerEncoderA),
+        steerEncoderB(steerEncoderB),
+        angleOffset(angleOffset),
+        centerOffset(frc::Translation2d{xOffset, yOffset}) {}
+};
+
+inline const ModuleConstants FL_CONSTANTS{
+    1, 0, 0, 1, 1, 2, 3, 0, kTrackLength / 2, kTrackWidth / 2};
+inline const ModuleConstants FR_CONSTANTS{
+    2, 2, 4, 5, 3, 6, 7, 0, kTrackLength / 2, -kTrackWidth / 2};
+inline const ModuleConstants BL_CONSTANTS{
+    3, 4, 8, 9, 5, 10, 11, 0, -kTrackLength / 2, kTrackWidth / 2};
+inline const ModuleConstants BR_CONSTANTS{
+    4, 6, 12, 13, 7, 14, 15, 0, -kTrackLength / 2, -kTrackWidth / 2};
+}  // namespace Swerve
+}  // namespace constants

photonlib-cpp-examples/poseest/src/main/include/Robot.h

@@ -0,0 +1,57 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <frc/TimedRobot.h>
+#include <frc/XboxController.h>
+
+#include "Vision.h"
+#include "subsystems/GamepieceLauncher.h"
+#include "subsystems/SwerveDrive.h"
+
+class Robot : public frc::TimedRobot {
+ public:
+  void RobotInit() override;
+  void RobotPeriodic() override;
+  void DisabledInit() override;
+  void DisabledPeriodic() override;
+  void DisabledExit() override;
+  void AutonomousInit() override;
+  void AutonomousPeriodic() override;
+  void AutonomousExit() override;
+  void TeleopInit() override;
+  void TeleopPeriodic() override;
+  void TeleopExit() override;
+  void TestInit() override;
+  void TestPeriodic() override;
+  void TestExit() override;
+  void SimulationPeriodic() override;
+
+ private:
+  SwerveDrive drivetrain{};
+  Vision vision{};
+  GamepieceLauncher launcher{};
+  frc::XboxController controller{0};
+};

photonlib-cpp-examples/poseest/src/main/include/Vision.h

@@ -0,0 +1,152 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <photon/PhotonCamera.h>
+#include <photon/PhotonPoseEstimator.h>
+#include <photon/estimation/VisionEstimation.h>
+#include <photon/simulation/VisionSystemSim.h>
+#include <photon/simulation/VisionTargetSim.h>
+#include <photon/targeting/PhotonPipelineResult.h>
+
+#include <limits>
+#include <memory>
+
+#include <frc/apriltag/AprilTagFieldLayout.h>
+#include <frc/apriltag/AprilTagFields.h>
+
+#include "Constants.h"
+
+class Vision {
+ public:
+  Vision() {
+    photonEstimator.SetMultiTagFallbackStrategy(
+        photon::PoseStrategy::LOWEST_AMBIGUITY);
+
+    if (frc::RobotBase::IsSimulation()) {
+      visionSim = std::make_unique<photon::VisionSystemSim>("main");
+
+      visionSim->AddAprilTags(constants::Vision::kTagLayout);
+
+      cameraProp = std::make_unique<photon::SimCameraProperties>();
+
+      cameraProp->SetCalibration(960, 720, frc::Rotation2d{90_deg});
+      cameraProp->SetCalibError(.35, .10);
+      cameraProp->SetFPS(15_Hz);
+      cameraProp->SetAvgLatency(50_ms);
+      cameraProp->SetLatencyStdDev(15_ms);
+
+      cameraSim =
+          std::make_shared<photon::PhotonCameraSim>(&camera, *cameraProp.get());
+
+      visionSim->AddCamera(cameraSim.get(), constants::Vision::kRobotToCam);
+      cameraSim->EnableDrawWireframe(true);
+    }
+  }
+
+  photon::PhotonPipelineResult GetLatestResult() { return m_latestResult; }
+
+  std::optional<photon::EstimatedRobotPose> GetEstimatedGlobalPose() {
+    std::optional<photon::EstimatedRobotPose> visionEst;
+
+    // Run each new pipeline result through our pose estimator
+    for (const auto& result : camera.GetAllUnreadResults()) {
+      // cache result and update pose estimator
+      auto visionEst = photonEstimator.Update(result);
+      m_latestResult = result;
+
+      // In sim only, add our vision estimate to the sim debug field
+      if (frc::RobotBase::IsSimulation()) {
+        if (visionEst) {
+          GetSimDebugField()
+              .GetObject("VisionEstimation")
+              ->SetPose(visionEst->estimatedPose.ToPose2d());
+        } else {
+          GetSimDebugField().GetObject("VisionEstimation")->SetPoses({});
+        }
+      }
+    }
+
+    return visionEst;
+  }
+
+  Eigen::Matrix<double, 3, 1> GetEstimationStdDevs(frc::Pose2d estimatedPose) {
+    Eigen::Matrix<double, 3, 1> estStdDevs =
+        constants::Vision::kSingleTagStdDevs;
+    auto targets = GetLatestResult().GetTargets();
+    int numTags = 0;
+    units::meter_t avgDist = 0_m;
+    for (const auto& tgt : targets) {
+      auto tagPose =
+          photonEstimator.GetFieldLayout().GetTagPose(tgt.GetFiducialId());
+      if (tagPose) {
+        numTags++;
+        avgDist += tagPose->ToPose2d().Translation().Distance(
+            estimatedPose.Translation());
+      }
+    }
+    if (numTags == 0) {
+      return estStdDevs;
+    }
+    avgDist /= numTags;
+    if (numTags > 1) {
+      estStdDevs = constants::Vision::kMultiTagStdDevs;
+    }
+    if (numTags == 1 && avgDist > 4_m) {
+      estStdDevs = (Eigen::MatrixXd(3, 1) << std::numeric_limits<double>::max(),
+                    std::numeric_limits<double>::max(),
+                    std::numeric_limits<double>::max())
+                       .finished();
+    } else {
+      estStdDevs = estStdDevs * (1 + (avgDist.value() * avgDist.value() / 30));
+    }
+    return estStdDevs;
+  }
+
+  void SimPeriodic(frc::Pose2d robotSimPose) {
+    visionSim->Update(robotSimPose);
+  }
+
+  void ResetSimPose(frc::Pose2d pose) {
+    if (frc::RobotBase::IsSimulation()) {
+      visionSim->ResetRobotPose(pose);
+    }
+  }
+
+  frc::Field2d& GetSimDebugField() { return visionSim->GetDebugField(); }
+
+ private:
+  photon::PhotonPoseEstimator photonEstimator{
+      constants::Vision::kTagLayout,
+      photon::PoseStrategy::MULTI_TAG_PNP_ON_COPROCESSOR,
+      constants::Vision::kRobotToCam};
+  photon::PhotonCamera camera{constants::Vision::kCameraName};
+  std::unique_ptr<photon::VisionSystemSim> visionSim;
+  std::unique_ptr<photon::SimCameraProperties> cameraProp;
+  std::shared_ptr<photon::PhotonCameraSim> cameraSim;
+
+  // The most recent result, cached for calculating std devs
+  photon::PhotonPipelineResult m_latestResult;
+};

photonlib-cpp-examples/poseest/src/main/include/subsystems/GamepieceLauncher.h

@@ -0,0 +1,66 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * 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.
+ */
+
+#ifndef PHOTONVISION_PHOTONLIB_CPP_EXAMPLES_POSEEST_SRC_MAIN_INCLUDE_SUBSYSTEMS_GAMEPIECELAUNCHER_H_
+#define PHOTONVISION_PHOTONLIB_CPP_EXAMPLES_POSEEST_SRC_MAIN_INCLUDE_SUBSYSTEMS_GAMEPIECELAUNCHER_H_
+
+#include <cmath>
+#include <numbers>
+
+#include <frc/RobotController.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
+#include <frc/simulation/FlywheelSim.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <frc/system/plant/DCMotor.h>
+#include <frc/system/plant/LinearSystemId.h>
+#include <units/angle.h>
+#include <units/moment_of_inertia.h>
+
+class GamepieceLauncher {
+ public:
+  GamepieceLauncher();              // Constructor
+  void setRunning(bool shouldRun);  // Method to start/stop the launcher
+  void periodic();                  // Method to handle periodic updates
+  void simulationPeriodic();        // Method to handle simulation updates
+
+ private:
+  frc::PWMSparkMax* motor;  // Motor controller
+
+  const double LAUNCH_SPEED_RPM = 2500;
+  double curDesSpd = 0.0;
+  double curMotorCmd = 0.0;
+
+  static constexpr units::kilogram_square_meter_t kFlywheelMomentOfInertia =
+      0.5 * 1.5_lb * 4_in * 4_in;
+
+  frc::DCMotor m_gearbox = frc::DCMotor::Falcon500(1);
+  frc::LinearSystem<1, 1, 1> m_plant{frc::LinearSystemId::FlywheelSystem(
+      m_gearbox, kFlywheelMomentOfInertia, 1.0)};
+
+  frc::sim::FlywheelSim launcherSim{m_plant, m_gearbox, 1.0};
+
+  void simulationInit();  // Method to initialize simulation components
+};
+
+#endif  // PHOTONVISION_PHOTONLIB_CPP_EXAMPLES_POSEEST_SRC_MAIN_INCLUDE_SUBSYSTEMS_GAMEPIECELAUNCHER_H_

photonlib-cpp-examples/poseest/src/main/include/subsystems/SwerveDrive.h

@@ -0,0 +1,85 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <frc/ADXRS450_Gyro.h>
+#include <frc/SPI.h>
+#include <frc/estimator/SwerveDrivePoseEstimator.h>
+#include <frc/kinematics/SwerveDriveKinematics.h>
+#include <frc/simulation/ADXRS450_GyroSim.h>
+
+#include "SwerveDriveSim.h"
+#include "SwerveModule.h"
+
+class SwerveDrive {
+ public:
+  SwerveDrive();
+  void Periodic();
+  void Drive(units::meters_per_second_t vx, units::meters_per_second_t vy,
+             units::radians_per_second_t omega);
+  void SetChassisSpeeds(const frc::ChassisSpeeds& targetChassisSpeeds,
+                        bool openLoop, bool steerInPlace);
+  void SetModuleStates(
+      const std::array<frc::SwerveModuleState, 4>& desiredStates, bool openLoop,
+      bool steerInPlace);
+  void Stop();
+  void AddVisionMeasurement(const frc::Pose2d& visionMeasurement,
+                            units::second_t timestamp);
+  void AddVisionMeasurement(const frc::Pose2d& visionMeasurement,
+                            units::second_t timestamp,
+                            const Eigen::Vector3d& stdDevs);
+  void ResetPose(const frc::Pose2d& pose, bool resetSimPose);
+  frc::Pose2d GetPose() const;
+  frc::Rotation2d GetHeading() const;
+  frc::Rotation2d GetGyroYaw() const;
+  frc::ChassisSpeeds GetChassisSpeeds() const;
+  std::array<frc::SwerveModuleState, 4> GetModuleStates() const;
+  std::array<frc::SwerveModulePosition, 4> GetModulePositions() const;
+  std::array<frc::Pose2d, 4> GetModulePoses() const;
+  void Log();
+  void SimulationPeriodic();
+  frc::Pose2d GetSimPose() const;
+  units::ampere_t GetCurrentDraw() const;
+
+ private:
+  std::array<SwerveModule, 4> swerveMods{
+      SwerveModule{constants::Swerve::FL_CONSTANTS},
+      SwerveModule{constants::Swerve::FR_CONSTANTS},
+      SwerveModule{constants::Swerve::BL_CONSTANTS},
+      SwerveModule{constants::Swerve::BR_CONSTANTS}};
+  frc::SwerveDriveKinematics<4> kinematics{
+      swerveMods[0].GetModuleConstants().centerOffset,
+      swerveMods[1].GetModuleConstants().centerOffset,
+      swerveMods[2].GetModuleConstants().centerOffset,
+      swerveMods[3].GetModuleConstants().centerOffset,
+  };
+  frc::ADXRS450_Gyro gyro{frc::SPI::Port::kOnboardCS0};
+  frc::SwerveDrivePoseEstimator<4> poseEstimator;
+  frc::ChassisSpeeds targetChassisSpeeds{};
+
+  frc::sim::ADXRS450_GyroSim gyroSim;
+  SwerveDriveSim swerveDriveSim;
+  units::ampere_t totalCurrentDraw{0};
+};

photonlib-cpp-examples/poseest/src/main/include/subsystems/SwerveDriveSim.h

@@ -0,0 +1,102 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <random>
+
+#include <frc/controller/SimpleMotorFeedforward.h>
+#include <frc/kinematics/SwerveDriveKinematics.h>
+#include <frc/system/LinearSystem.h>
+#include <frc/system/plant/DCMotor.h>
+#include <units/voltage.h>
+
+static constexpr int numModules{4};
+
+class SwerveDriveSim {
+ public:
+  SwerveDriveSim(const frc::SimpleMotorFeedforward<units::meters>& driveFF,
+                 const frc::DCMotor& driveMotor, double driveGearing,
+                 units::meter_t driveWheelRadius,
+                 const frc::SimpleMotorFeedforward<units::radians>& steerFF,
+                 const frc::DCMotor& steerMotor, double steerGearing,
+                 const frc::SwerveDriveKinematics<numModules>& kinematics);
+  SwerveDriveSim(const frc::LinearSystem<2, 1, 2>& drivePlant,
+                 units::volt_t driveKs, const frc::DCMotor& driveMotor,
+                 double driveGearing, units::meter_t driveWheelRadius,
+                 const frc::LinearSystem<2, 1, 2>& steerPlant,
+                 units::volt_t steerKs, const frc::DCMotor& steerMotor,
+                 double steerGearing,
+                 const frc::SwerveDriveKinematics<numModules>& kinematics);
+  void SetDriveInputs(const std::array<units::volt_t, numModules>& inputs);
+  void SetSteerInputs(const std::array<units::volt_t, numModules>& inputs);
+  static Eigen::Matrix<double, 2, 1> CalculateX(
+      const Eigen::Matrix<double, 2, 2>& discA,
+      const Eigen::Matrix<double, 2, 1>& discB,
+      const Eigen::Matrix<double, 2, 1>& x, units::volt_t input,
+      units::volt_t kS);
+  void Update(units::second_t dt);
+  void Reset(const frc::Pose2d& pose, bool preserveMotion);
+  void Reset(const frc::Pose2d& pose,
+             const std::array<Eigen::Matrix<double, 2, 1>, numModules>&
+                 moduleDriveStates,
+             const std::array<Eigen::Matrix<double, 2, 1>, numModules>&
+                 moduleSteerStates);
+  frc::Pose2d GetPose() const;
+  std::array<frc::SwerveModulePosition, numModules> GetModulePositions() const;
+  std::array<frc::SwerveModulePosition, numModules> GetNoisyModulePositions(
+      units::meter_t driveStdDev, units::radian_t steerStdDev);
+  std::array<frc::SwerveModuleState, numModules> GetModuleStates();
+  std::array<Eigen::Matrix<double, 2, 1>, numModules> GetDriveStates() const;
+  std::array<Eigen::Matrix<double, 2, 1>, numModules> GetSteerStates() const;
+  units::radians_per_second_t GetOmega() const;
+  units::ampere_t GetCurrentDraw(const frc::DCMotor& motor,
+                                 units::radians_per_second_t velocity,
+                                 units::volt_t inputVolts,
+                                 units::volt_t batteryVolts) const;
+  std::array<units::ampere_t, numModules> GetDriveCurrentDraw() const;
+  std::array<units::ampere_t, numModules> GetSteerCurrentDraw() const;
+  units::ampere_t GetTotalCurrentDraw() const;
+
+ private:
+  std::random_device rd{};
+  std::mt19937 generator{rd()};
+  std::normal_distribution<double> randDist{0.0, 1.0};
+  const frc::LinearSystem<2, 1, 2> drivePlant;
+  const units::volt_t driveKs;
+  const frc::DCMotor driveMotor;
+  const double driveGearing;
+  const units::meter_t driveWheelRadius;
+  const frc::LinearSystem<2, 1, 2> steerPlant;
+  const units::volt_t steerKs;
+  const frc::DCMotor steerMotor;
+  const double steerGearing;
+  const frc::SwerveDriveKinematics<numModules> kinematics;
+  std::array<units::volt_t, numModules> driveInputs{};
+  std::array<Eigen::Matrix<double, 2, 1>, numModules> driveStates{};
+  std::array<units::volt_t, numModules> steerInputs{};
+  std::array<Eigen::Matrix<double, 2, 1>, numModules> steerStates{};
+  frc::Pose2d pose{frc::Pose2d{}};
+  units::radians_per_second_t omega{0};
+};

photonlib-cpp-examples/poseest/src/main/include/subsystems/SwerveModule.h

@@ -0,0 +1,81 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <frc/Encoder.h>
+#include <frc/controller/PIDController.h>
+#include <frc/kinematics/SwerveModulePosition.h>
+#include <frc/kinematics/SwerveModuleState.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
+#include <frc/simulation/EncoderSim.h>
+#include <units/current.h>
+
+#include "Constants.h"
+
+class SwerveModule {
+ public:
+  explicit SwerveModule(const constants::Swerve::ModuleConstants& consts);
+  void Periodic();
+  void SetDesiredState(const frc::SwerveModuleState& newState,
+                       bool shouldBeOpenLoop, bool steerInPlace);
+  frc::Rotation2d GetAbsoluteHeading() const;
+  frc::SwerveModuleState GetState() const;
+  frc::SwerveModulePosition GetPosition() const;
+  units::volt_t GetDriveVoltage() const;
+  units::volt_t GetSteerVoltage() const;
+  units::ampere_t GetDriveCurrentSim() const;
+  units::ampere_t GetSteerCurrentSim() const;
+  constants::Swerve::ModuleConstants GetModuleConstants() const;
+  void Log();
+  void SimulationUpdate(units::meter_t driveEncoderDist,
+                        units::meters_per_second_t driveEncoderRate,
+                        units::ampere_t driveCurrent,
+                        units::radian_t steerEncoderDist,
+                        units::radians_per_second_t steerEncoderRate,
+                        units::ampere_t steerCurrent);
+
+ private:
+  const constants::Swerve::ModuleConstants moduleConstants;
+
+  frc::PWMSparkMax driveMotor;
+  frc::Encoder driveEncoder;
+  frc::PWMSparkMax steerMotor;
+  frc::Encoder steerEncoder;
+
+  frc::SwerveModuleState desiredState{};
+  bool openLoop{false};
+
+  frc::PIDController drivePIDController{constants::Swerve::kDriveKP,
+                                        constants::Swerve::kDriveKI,
+                                        constants::Swerve::kDriveKD};
+  frc::PIDController steerPIDController{constants::Swerve::kSteerKP,
+                                        constants::Swerve::kSteerKI,
+                                        constants::Swerve::kSteerKD};
+
+  frc::sim::EncoderSim driveEncoderSim;
+  units::ampere_t driveCurrentSim{0};
+  frc::sim::EncoderSim steerEncoderSim;
+  units::ampere_t steerCurrentSim{0};
+};