Initial commit

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

@@ -0,0 +1,116 @@
+/*
+ * 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.
+ */
+
+#include "Robot.h"
+
+#include <iostream>
+
+#include <frc/simulation/BatterySim.h>
+#include <frc/simulation/RoboRioSim.h>
+
+void Robot::RobotInit() {}
+
+void Robot::RobotPeriodic() {
+  launcher.periodic();
+  drivetrain.Periodic();
+
+  auto visionEst = vision.GetEstimatedGlobalPose();
+  if (visionEst.has_value()) {
+    auto est = visionEst.value();
+    auto estPose = est.estimatedPose.ToPose2d();
+    auto estStdDevs = vision.GetEstimationStdDevs(estPose);
+    drivetrain.AddVisionMeasurement(est.estimatedPose.ToPose2d(), est.timestamp,
+                                    estStdDevs);
+  }
+
+  drivetrain.Log();
+}
+
+void Robot::DisabledInit() {}
+
+void Robot::DisabledPeriodic() { drivetrain.Stop(); }
+
+void Robot::DisabledExit() {}
+
+void Robot::AutonomousInit() {}
+
+void Robot::AutonomousPeriodic() {}
+
+void Robot::AutonomousExit() {}
+
+void Robot::TeleopInit() {
+  frc::Pose2d pose{1_m, 1_m, frc::Rotation2d{}};
+  drivetrain.ResetPose(pose, true);
+}
+
+void Robot::TeleopPeriodic() {
+  // Calculate drivetrain commands from Joystick values
+  auto forward =
+      -1.0 * controller.GetLeftY() * constants::Swerve::kMaxLinearSpeed;
+  auto strafe =
+      -1.0 * controller.GetLeftX() * constants::Swerve::kMaxLinearSpeed;
+  auto turn =
+      -1.0 * controller.GetRightX() * constants::Swerve::kMaxAngularSpeed;
+
+  // Command drivetrain motors based on target speeds
+  drivetrain.Drive(forward, strafe, turn);
+
+  // Calculate whether the gamepiece launcher runs based on our global pose
+  // estimate.
+  frc::Pose2d curPose = drivetrain.GetPose();
+  bool shouldRun = (curPose.Y() > 2.0_m &&
+                    curPose.X() < 4.0_m);  // Close enough to blue speaker
+  launcher.setRunning(shouldRun);
+}
+
+void Robot::TeleopExit() {}
+
+void Robot::TestInit() {}
+
+void Robot::TestPeriodic() {}
+
+void Robot::TestExit() {}
+
+void Robot::SimulationPeriodic() {
+  launcher.simulationPeriodic();
+  drivetrain.SimulationPeriodic();
+  vision.SimPeriodic(drivetrain.GetSimPose());
+
+  frc::Field2d& debugField = vision.GetSimDebugField();
+  debugField.GetObject("EstimatedRobot")->SetPose(drivetrain.GetPose());
+  debugField.GetObject("EstimatedRobotModules")
+      ->SetPoses(drivetrain.GetModulePoses());
+
+  units::ampere_t totalCurrent = drivetrain.GetCurrentDraw();
+  units::volt_t loadedBattVolts =
+      frc::sim::BatterySim::Calculate({totalCurrent});
+
+  // Using max(0.1, voltage) here isn't a *physically correct* solution,
+  // but it avoids problems with battery voltage measuring 0.
+  frc::sim::RoboRioSim::SetVInVoltage(units::math::max(0.1_V, loadedBattVolts));
+}
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif

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

@@ -0,0 +1,58 @@
+/*
+ * 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.
+ */
+
+#include "subsystems/GamepieceLauncher.h"  // Include the header file
+
+GamepieceLauncher::GamepieceLauncher() {
+  motor = new frc::PWMSparkMax(8);  // Create the motor object for PWM port 8
+  simulationInit();
+}
+
+void GamepieceLauncher::setRunning(bool shouldRun) {
+  curDesSpd = shouldRun ? LAUNCH_SPEED_RPM : 0.0;
+}
+
+void GamepieceLauncher::periodic() {
+  // Calculate the maximum RPM
+  double maxRPM =
+      units::radians_per_second_t(frc::DCMotor::Falcon500(1).freeSpeed)
+          .to<double>() *
+      60 / (2 * std::numbers::pi);
+  curMotorCmd = curDesSpd / maxRPM;
+  curMotorCmd = std::clamp(curMotorCmd, 0.0, 1.0);
+  motor->Set(curMotorCmd);
+
+  frc::SmartDashboard::PutNumber("GPLauncher Des Spd (RPM)", curDesSpd);
+}
+
+void GamepieceLauncher::simulationPeriodic() {
+  launcherSim.SetInputVoltage(curMotorCmd *
+                              frc::RobotController::GetBatteryVoltage());
+  launcherSim.Update(0.02_s);
+  auto spd = launcherSim.GetAngularVelocity().to<double>() * 60 /
+             (2 * std::numbers::pi);
+  frc::SmartDashboard::PutNumber("GPLauncher Act Spd (RPM)", spd);
+}
+
+void GamepieceLauncher::simulationInit() {}

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

@@ -0,0 +1,212 @@
+/*
+ * 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.
+ */
+
+#include "subsystems/SwerveDrive.h"
+
+#include <iostream>
+#include <string>
+
+#include <frc/TimedRobot.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+
+SwerveDrive::SwerveDrive()
+    : poseEstimator(kinematics, GetGyroYaw(), GetModulePositions(),
+                    frc::Pose2d{}, {0.1, 0.1, 0.1}, {1.0, 1.0, 1.0}),
+      gyroSim(gyro),
+      swerveDriveSim(constants::Swerve::kDriveFF, frc::DCMotor::Falcon500(1),
+                     constants::Swerve::kDriveGearRatio,
+                     constants::Swerve::kWheelDiameter / 2,
+                     constants::Swerve::kSteerFF, frc::DCMotor::Falcon500(1),
+                     constants::Swerve::kSteerGearRatio, kinematics) {}
+
+void SwerveDrive::Periodic() {
+  for (auto& currentModule : swerveMods) {
+    currentModule.Periodic();
+  }
+
+  poseEstimator.Update(GetGyroYaw(), GetModulePositions());
+}
+
+void SwerveDrive::Drive(units::meters_per_second_t vx,
+                        units::meters_per_second_t vy,
+                        units::radians_per_second_t omega) {
+  frc::ChassisSpeeds newChassisSpeeds =
+      frc::ChassisSpeeds::FromFieldRelativeSpeeds(vx, vy, omega, GetHeading());
+  SetChassisSpeeds(newChassisSpeeds, true, false);
+}
+
+void SwerveDrive::SetChassisSpeeds(const frc::ChassisSpeeds& newChassisSpeeds,
+                                   bool openLoop, bool steerInPlace) {
+  SetModuleStates(kinematics.ToSwerveModuleStates(newChassisSpeeds), true,
+                  steerInPlace);
+  this->targetChassisSpeeds = newChassisSpeeds;
+}
+
+void SwerveDrive::SetModuleStates(
+    const std::array<frc::SwerveModuleState, 4>& desiredStates, bool openLoop,
+    bool steerInPlace) {
+  std::array<frc::SwerveModuleState, 4> desaturatedStates = desiredStates;
+  frc::SwerveDriveKinematics<4>::DesaturateWheelSpeeds(
+      static_cast<wpi::array<frc::SwerveModuleState, 4>*>(&desaturatedStates),
+      constants::Swerve::kMaxLinearSpeed);
+  for (int i = 0; i < swerveMods.size(); i++) {
+    swerveMods[i].SetDesiredState(desaturatedStates[i], openLoop, steerInPlace);
+  }
+}
+
+void SwerveDrive::Stop() { Drive(0_mps, 0_mps, 0_rad_per_s); }
+
+void SwerveDrive::AddVisionMeasurement(const frc::Pose2d& visionMeasurement,
+                                       units::second_t timestamp) {
+  poseEstimator.AddVisionMeasurement(visionMeasurement, timestamp);
+}
+
+void SwerveDrive::AddVisionMeasurement(const frc::Pose2d& visionMeasurement,
+                                       units::second_t timestamp,
+                                       const Eigen::Vector3d& stdDevs) {
+  wpi::array<double, 3> newStdDevs{stdDevs(0), stdDevs(1), stdDevs(2)};
+  poseEstimator.AddVisionMeasurement(visionMeasurement, timestamp, newStdDevs);
+}
+
+void SwerveDrive::ResetPose(const frc::Pose2d& pose, bool resetSimPose) {
+  if (resetSimPose) {
+    swerveDriveSim.Reset(pose, false);
+    for (int i = 0; i < swerveMods.size(); i++) {
+      swerveMods[i].SimulationUpdate(0_m, 0_mps, 0_A, 0_rad, 0_rad_per_s, 0_A);
+    }
+    gyroSim.SetAngle(-pose.Rotation().Degrees());
+    gyroSim.SetRate(0_rad_per_s);
+  }
+
+  poseEstimator.ResetPosition(GetGyroYaw(), GetModulePositions(), pose);
+}
+
+frc::Pose2d SwerveDrive::GetPose() const {
+  return poseEstimator.GetEstimatedPosition();
+}
+
+frc::Rotation2d SwerveDrive::GetHeading() const { return GetPose().Rotation(); }
+
+frc::Rotation2d SwerveDrive::GetGyroYaw() const { return gyro.GetRotation2d(); }
+
+frc::ChassisSpeeds SwerveDrive::GetChassisSpeeds() const {
+  return kinematics.ToChassisSpeeds(GetModuleStates());
+}
+
+std::array<frc::SwerveModuleState, 4> SwerveDrive::GetModuleStates() const {
+  std::array<frc::SwerveModuleState, 4> moduleStates;
+  moduleStates[0] = swerveMods[0].GetState();
+  moduleStates[1] = swerveMods[1].GetState();
+  moduleStates[2] = swerveMods[2].GetState();
+  moduleStates[3] = swerveMods[3].GetState();
+  return moduleStates;
+}
+
+std::array<frc::SwerveModulePosition, 4> SwerveDrive::GetModulePositions()
+    const {
+  std::array<frc::SwerveModulePosition, 4> modulePositions;
+  modulePositions[0] = swerveMods[0].GetPosition();
+  modulePositions[1] = swerveMods[1].GetPosition();
+  modulePositions[2] = swerveMods[2].GetPosition();
+  modulePositions[3] = swerveMods[3].GetPosition();
+  return modulePositions;
+}
+
+std::array<frc::Pose2d, 4> SwerveDrive::GetModulePoses() const {
+  std::array<frc::Pose2d, 4> modulePoses;
+  for (int i = 0; i < swerveMods.size(); i++) {
+    const SwerveModule& module = swerveMods[i];
+    modulePoses[i] = GetPose().TransformBy(frc::Transform2d{
+        module.GetModuleConstants().centerOffset, module.GetAbsoluteHeading()});
+  }
+  return modulePoses;
+}
+
+void SwerveDrive::Log() {
+  std::string table = "Drive/";
+  frc::Pose2d pose = GetPose();
+  frc::SmartDashboard::PutNumber(table + "X", pose.X().to<double>());
+  frc::SmartDashboard::PutNumber(table + "Y", pose.Y().to<double>());
+  frc::SmartDashboard::PutNumber(table + "Heading",
+                                 pose.Rotation().Degrees().to<double>());
+  frc::ChassisSpeeds chassisSpeeds = GetChassisSpeeds();
+  frc::SmartDashboard::PutNumber(table + "VX", chassisSpeeds.vx.to<double>());
+  frc::SmartDashboard::PutNumber(table + "VY", chassisSpeeds.vy.to<double>());
+  frc::SmartDashboard::PutNumber(
+      table + "Omega Degrees",
+      chassisSpeeds.omega.convert<units::degrees_per_second>().to<double>());
+  frc::SmartDashboard::PutNumber(table + "Target VX",
+                                 targetChassisSpeeds.vx.to<double>());
+  frc::SmartDashboard::PutNumber(table + "Target VY",
+                                 targetChassisSpeeds.vy.to<double>());
+  frc::SmartDashboard::PutNumber(
+      table + "Target Omega Degrees",
+      targetChassisSpeeds.omega.convert<units::degrees_per_second>()
+          .to<double>());
+
+  for (auto& module : swerveMods) {
+    module.Log();
+  }
+}
+
+void SwerveDrive::SimulationPeriodic() {
+  std::array<units::volt_t, 4> driveInputs;
+  std::array<units::volt_t, 4> steerInputs;
+  for (int i = 0; i < swerveMods.size(); i++) {
+    driveInputs[i] = swerveMods[i].GetDriveVoltage();
+    steerInputs[i] = swerveMods[i].GetSteerVoltage();
+  }
+  swerveDriveSim.SetDriveInputs(driveInputs);
+  swerveDriveSim.SetSteerInputs(steerInputs);
+
+  swerveDriveSim.Update(frc::TimedRobot::kDefaultPeriod);
+
+  auto driveStates = swerveDriveSim.GetDriveStates();
+  auto steerStates = swerveDriveSim.GetSteerStates();
+  totalCurrentDraw = 0_A;
+  std::array<units::ampere_t, 4> driveCurrents =
+      swerveDriveSim.GetDriveCurrentDraw();
+  for (const auto& current : driveCurrents) {
+    totalCurrentDraw += current;
+  }
+  std::array<units::ampere_t, 4> steerCurrents =
+      swerveDriveSim.GetSteerCurrentDraw();
+  for (const auto& current : steerCurrents) {
+    totalCurrentDraw += current;
+  }
+  for (int i = 0; i < swerveMods.size(); i++) {
+    units::meter_t drivePos{driveStates[i](0, 0)};
+    units::meters_per_second_t driveRate{driveStates[i](1, 0)};
+    units::radian_t steerPos{steerStates[i](0, 0)};
+    units::radians_per_second_t steerRate{steerStates[i](1, 0)};
+    swerveMods[i].SimulationUpdate(drivePos, driveRate, driveCurrents[i],
+                                   steerPos, steerRate, steerCurrents[i]);
+  }
+  gyroSim.SetRate(-swerveDriveSim.GetOmega());
+  gyroSim.SetAngle(-swerveDriveSim.GetPose().Rotation().Degrees());
+}
+
+frc::Pose2d SwerveDrive::GetSimPose() const { return swerveDriveSim.GetPose(); }
+
+units::ampere_t SwerveDrive::GetCurrentDraw() const { return totalCurrentDraw; }

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

@@ -0,0 +1,287 @@
+/*
+ * 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.
+ */
+
+#include "subsystems/SwerveDriveSim.h"
+
+#include <iostream>
+
+#include <frc/RobotController.h>
+#include <frc/system/Discretization.h>
+
+template <typename T>
+int sgn(T val) {
+  return (T(0) < val) - (val < T(0));
+}
+
+SwerveDriveSim::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(
+          frc::LinearSystem<2, 1, 2>{
+              (Eigen::MatrixXd(2, 2) << 0.0, 1.0, 0.0,
+               -driveFF.GetKv().to<double>() / driveFF.GetKa().to<double>())
+                  .finished(),
+              Eigen::Matrix<double, 2, 1>{0.0,
+                                          1.0 / driveFF.GetKa().to<double>()},
+              (Eigen::MatrixXd(2, 2) << 1.0, 0.0, 0.0, 1.0).finished(),
+              Eigen::Matrix<double, 2, 1>{0.0, 0.0}},
+          driveFF.GetKs(), driveMotor, driveGearing, driveWheelRadius,
+          frc::LinearSystem<2, 1, 2>{
+              (Eigen::MatrixXd(2, 2) << 0.0, 1.0, 0.0,
+               -steerFF.GetKv().to<double>() / steerFF.GetKa().to<double>())
+                  .finished(),
+              Eigen::Matrix<double, 2, 1>{0.0,
+                                          1.0 / steerFF.GetKa().to<double>()},
+              (Eigen::MatrixXd(2, 2) << 1.0, 0.0, 0.0, 1.0).finished(),
+              Eigen::Matrix<double, 2, 1>{0.0, 0.0}},
+          steerFF.GetKs(), steerMotor, steerGearing, kinematics) {}
+
+SwerveDriveSim::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)
+    : drivePlant(drivePlant),
+      driveKs(driveKs),
+      driveMotor(driveMotor),
+      driveGearing(driveGearing),
+      driveWheelRadius(driveWheelRadius),
+      steerPlant(steerPlant),
+      steerKs(steerKs),
+      steerMotor(steerMotor),
+      steerGearing(steerGearing),
+      kinematics(kinematics) {}
+
+void SwerveDriveSim::SetDriveInputs(
+    const std::array<units::volt_t, numModules>& inputs) {
+  units::volt_t battVoltage = frc::RobotController::GetBatteryVoltage();
+  for (int i = 0; i < driveInputs.size(); i++) {
+    units::volt_t input = inputs[i];
+    driveInputs[i] = std::clamp(input, -battVoltage, battVoltage);
+  }
+}
+
+void SwerveDriveSim::SetSteerInputs(
+    const std::array<units::volt_t, numModules>& inputs) {
+  units::volt_t battVoltage = frc::RobotController::GetBatteryVoltage();
+  for (int i = 0; i < steerInputs.size(); i++) {
+    units::volt_t input = inputs[i];
+    steerInputs[i] = std::clamp(input, -battVoltage, battVoltage);
+  }
+}
+
+Eigen::Matrix<double, 2, 1> SwerveDriveSim::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) {
+  auto Ax = discA * x;
+  double nextStateVel = Ax(1, 0);
+  double inputToStop = nextStateVel / -discB(1, 0);
+  double ksSystemEffect =
+      std::clamp(inputToStop, -kS.to<double>(), kS.to<double>());
+
+  nextStateVel += discB(1, 0) * ksSystemEffect;
+  inputToStop = nextStateVel / -discB(1, 0);
+  double signToStop = sgn(inputToStop);
+  double inputSign = sgn(input.to<double>());
+  double ksInputEffect = 0;
+
+  if (std::abs(ksSystemEffect) < kS.to<double>()) {
+    double absInput = std::abs(input.to<double>());
+    ksInputEffect =
+        -std::clamp(kS.to<double>() * inputSign, -absInput, absInput);
+  } else if ((input.to<double>() * signToStop) > (inputToStop * signToStop)) {
+    double absInput = std::abs(input.to<double>() - inputToStop);
+    ksInputEffect =
+        -std::clamp(kS.to<double>() * inputSign, -absInput, absInput);
+  }
+
+  auto sF = Eigen::Matrix<double, 1, 1>{input.to<double>() + ksSystemEffect +
+                                        ksInputEffect};
+  auto Bu = discB * sF;
+  auto retVal = Ax + Bu;
+  return retVal;
+}
+
+void SwerveDriveSim::Update(units::second_t dt) {
+  Eigen::Matrix<double, 2, 2> driveDiscA;
+  Eigen::Matrix<double, 2, 1> driveDiscB;
+  frc::DiscretizeAB<2, 1>(drivePlant.A(), drivePlant.B(), dt, &driveDiscA,
+                          &driveDiscB);
+
+  Eigen::Matrix<double, 2, 2> steerDiscA;
+  Eigen::Matrix<double, 2, 1> steerDiscB;
+  frc::DiscretizeAB<2, 1>(steerPlant.A(), steerPlant.B(), dt, &steerDiscA,
+                          &steerDiscB);
+
+  std::array<frc::SwerveModulePosition, 4> moduleDeltas;
+
+  for (int i = 0; i < numModules; i++) {
+    double prevDriveStatePos = driveStates[i](0, 0);
+    driveStates[i] = CalculateX(driveDiscA, driveDiscB, driveStates[i],
+                                driveInputs[i], driveKs);
+    double currentDriveStatePos = driveStates[i](0, 0);
+    steerStates[i] = CalculateX(steerDiscA, steerDiscB, steerStates[i],
+                                steerInputs[i], steerKs);
+    double currentSteerStatePos = steerStates[i](0, 0);
+    moduleDeltas[i] = frc::SwerveModulePosition{
+        units::meter_t{currentDriveStatePos - prevDriveStatePos},
+        frc::Rotation2d{units::radian_t{currentSteerStatePos}}};
+  }
+
+  frc::Twist2d twist = kinematics.ToTwist2d(moduleDeltas);
+  pose = pose.Exp(twist);
+  omega = twist.dtheta / dt;
+}
+
+void SwerveDriveSim::Reset(const frc::Pose2d& pose, bool preserveMotion) {
+  this->pose = pose;
+  if (!preserveMotion) {
+    for (int i = 0; i < numModules; i++) {
+      driveStates[i] = Eigen::Matrix<double, 2, 1>{0, 0};
+      steerStates[i] = Eigen::Matrix<double, 2, 1>{0, 0};
+    }
+    omega = 0_rad_per_s;
+  }
+}
+
+void SwerveDriveSim::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) {
+  this->pose = pose;
+  driveStates = moduleDriveStates;
+  steerStates = moduleSteerStates;
+  omega = kinematics.ToChassisSpeeds(GetModuleStates()).omega;
+}
+
+frc::Pose2d SwerveDriveSim::GetPose() const { return pose; }
+
+std::array<frc::SwerveModulePosition, numModules>
+SwerveDriveSim::GetModulePositions() const {
+  std::array<frc::SwerveModulePosition, numModules> positions;
+  for (int i = 0; i < numModules; i++) {
+    positions[i] = frc::SwerveModulePosition{
+        units::meter_t{driveStates[i](0, 0)},
+        frc::Rotation2d{units::radian_t{steerStates[i](0, 0)}}};
+  }
+  return positions;
+}
+
+std::array<frc::SwerveModulePosition, numModules>
+SwerveDriveSim::GetNoisyModulePositions(units::meter_t driveStdDev,
+                                        units::radian_t steerStdDev) {
+  std::array<frc::SwerveModulePosition, numModules> positions;
+  for (int i = 0; i < numModules; i++) {
+    positions[i] = frc::SwerveModulePosition{
+        units::meter_t{driveStates[i](0, 0)} +
+            randDist(generator) * driveStdDev,
+        frc::Rotation2d{units::radian_t{steerStates[i](0, 0)} +
+                        randDist(generator) * steerStdDev}};
+  }
+  return positions;
+}
+
+std::array<frc::SwerveModuleState, numModules>
+SwerveDriveSim::GetModuleStates() {
+  std::array<frc::SwerveModuleState, numModules> states;
+  for (int i = 0; i < numModules; i++) {
+    states[i] = frc::SwerveModuleState{
+        units::meters_per_second_t{driveStates[i](1, 0)},
+        frc::Rotation2d{units::radian_t{steerStates[i](0, 0)}}};
+  }
+  return states;
+}
+
+std::array<Eigen::Matrix<double, 2, 1>, numModules>
+SwerveDriveSim::GetDriveStates() const {
+  return driveStates;
+}
+
+std::array<Eigen::Matrix<double, 2, 1>, numModules>
+SwerveDriveSim::GetSteerStates() const {
+  return steerStates;
+}
+
+units::radians_per_second_t SwerveDriveSim::GetOmega() const { return omega; }
+
+units::ampere_t SwerveDriveSim::GetCurrentDraw(
+    const frc::DCMotor& motor, units::radians_per_second_t velocity,
+    units::volt_t inputVolts, units::volt_t batteryVolts) const {
+  units::volt_t effVolts = inputVolts - velocity / motor.Kv;
+  if (inputVolts >= 0_V) {
+    effVolts = std::clamp(effVolts, 0_V, inputVolts);
+  } else {
+    effVolts = std::clamp(effVolts, inputVolts, 0_V);
+  }
+  auto retVal = (inputVolts / batteryVolts) * (effVolts / motor.R);
+  return retVal;
+}
+
+std::array<units::ampere_t, numModules> SwerveDriveSim::GetDriveCurrentDraw()
+    const {
+  std::array<units::ampere_t, numModules> currents;
+  for (int i = 0; i < numModules; i++) {
+    units::radians_per_second_t speed =
+        units::radians_per_second_t{driveStates[i](1, 0)} * driveGearing /
+        driveWheelRadius.to<double>();
+    currents[i] = GetCurrentDraw(driveMotor, speed, driveInputs[i],
+                                 frc::RobotController::GetBatteryVoltage());
+  }
+  return currents;
+}
+
+std::array<units::ampere_t, numModules> SwerveDriveSim::GetSteerCurrentDraw()
+    const {
+  std::array<units::ampere_t, numModules> currents;
+  for (int i = 0; i < numModules; i++) {
+    units::radians_per_second_t speed =
+        units::radians_per_second_t{steerStates[i](1, 0) * steerGearing};
+    // TODO: If uncommented we get huge current values.. Not sure how to fix
+    // atm. :(
+    currents[i] = 20_A;
+    // currents[i] = GetCurrentDraw(steerMotor, speed, steerInputs[i],
+    // frc::RobotController::GetBatteryVoltage());
+  }
+  return currents;
+}
+
+units::ampere_t SwerveDriveSim::GetTotalCurrentDraw() const {
+  units::ampere_t total{0};
+  for (const auto& val : GetDriveCurrentDraw()) {
+    total += val;
+  }
+  for (const auto& val : GetSteerCurrentDraw()) {
+    total += val;
+  }
+  return total;
+}

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

@@ -0,0 +1,146 @@
+/*
+ * 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.
+ */
+
+#include "subsystems/SwerveModule.h"
+
+#include <iostream>
+#include <string>
+
+#include <frc/MathUtil.h>
+#include <frc/RobotController.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+
+SwerveModule::SwerveModule(const constants::Swerve::ModuleConstants& consts)
+    : moduleConstants(consts),
+      driveMotor(frc::PWMSparkMax{moduleConstants.driveMotorId}),
+      driveEncoder(frc::Encoder{moduleConstants.driveEncoderA,
+                                moduleConstants.driveEncoderB}),
+      steerMotor(frc::PWMSparkMax{moduleConstants.steerMotorId}),
+      steerEncoder(frc::Encoder{moduleConstants.steerEncoderA,
+                                moduleConstants.steerEncoderB}),
+      driveEncoderSim(driveEncoder),
+      steerEncoderSim(steerEncoder) {
+  driveEncoder.SetDistancePerPulse(
+      constants::Swerve::kDriveDistPerPulse.to<double>());
+  steerEncoder.SetDistancePerPulse(
+      constants::Swerve::kSteerRadPerPulse.to<double>());
+  steerPIDController.EnableContinuousInput(-std::numbers::pi, std::numbers::pi);
+}
+
+void SwerveModule::Periodic() {
+  units::volt_t steerPID = units::volt_t{
+      steerPIDController.Calculate(GetAbsoluteHeading().Radians().to<double>(),
+                                   desiredState.angle.Radians().to<double>())};
+  steerMotor.SetVoltage(steerPID);
+
+  units::volt_t driveFF =
+      constants::Swerve::kDriveFF.Calculate(desiredState.speed);
+  units::volt_t drivePID{0};
+  if (!openLoop) {
+    drivePID = units::volt_t{drivePIDController.Calculate(
+        driveEncoder.GetRate(), desiredState.speed.to<double>())};
+  }
+  driveMotor.SetVoltage(driveFF + drivePID);
+}
+
+void SwerveModule::SetDesiredState(frc::SwerveModuleState newState,
+                                   bool shouldBeOpenLoop, bool steerInPlace) {
+  frc::Rotation2d currentRotation = GetAbsoluteHeading();
+  newState.Optimize(currentRotation);
+  desiredState = newState;
+}
+
+frc::Rotation2d SwerveModule::GetAbsoluteHeading() const {
+  return frc::Rotation2d{units::radian_t{steerEncoder.GetDistance()}};
+}
+
+frc::SwerveModuleState SwerveModule::GetState() const {
+  return frc::SwerveModuleState{driveEncoder.GetRate() * 1_mps,
+                                GetAbsoluteHeading()};
+}
+
+frc::SwerveModulePosition SwerveModule::GetPosition() const {
+  return frc::SwerveModulePosition{driveEncoder.GetDistance() * 1_m,
+                                   GetAbsoluteHeading()};
+}
+
+units::volt_t SwerveModule::GetDriveVoltage() const {
+  return driveMotor.Get() * frc::RobotController::GetBatteryVoltage();
+}
+
+units::volt_t SwerveModule::GetSteerVoltage() const {
+  return steerMotor.Get() * frc::RobotController::GetBatteryVoltage();
+}
+
+units::ampere_t SwerveModule::GetDriveCurrentSim() const {
+  return driveCurrentSim;
+}
+
+units::ampere_t SwerveModule::GetSteerCurrentSim() const {
+  return steerCurrentSim;
+}
+
+constants::Swerve::ModuleConstants SwerveModule::GetModuleConstants() const {
+  return moduleConstants;
+}
+
+void SwerveModule::Log() {
+  frc::SwerveModuleState state = GetState();
+
+  std::string table =
+      "Module " + std::to_string(moduleConstants.moduleNum) + "/";
+  frc::SmartDashboard::PutNumber(table + "Steer Degrees",
+                                 frc::AngleModulus(state.angle.Radians())
+                                     .convert<units::degrees>()
+                                     .to<double>());
+  frc::SmartDashboard::PutNumber(
+      table + "Steer Target Degrees",
+      units::radian_t{steerPIDController.GetSetpoint()}
+          .convert<units::degrees>()
+          .to<double>());
+  frc::SmartDashboard::PutNumber(
+      table + "Drive Velocity Feet",
+      state.speed.convert<units::feet_per_second>().to<double>());
+  frc::SmartDashboard::PutNumber(
+      table + "Drive Velocity Target Feet",
+      desiredState.speed.convert<units::feet_per_second>().to<double>());
+  frc::SmartDashboard::PutNumber(table + "Drive Current",
+                                 driveCurrentSim.to<double>());
+  frc::SmartDashboard::PutNumber(table + "Steer Current",
+                                 steerCurrentSim.to<double>());
+}
+
+void SwerveModule::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) {
+  driveEncoderSim.SetDistance(driveEncoderDist.to<double>());
+  driveEncoderSim.SetRate(driveEncoderRate.to<double>());
+  driveCurrentSim = driveCurrent;
+  steerEncoderSim.SetDistance(steerEncoderDist.to<double>());
+  steerEncoderSim.SetRate(steerEncoderRate.to<double>());
+  steerCurrentSim = steerCurrent;
+}

photonlib-cpp-examples/poseest/src/main/deploy/example.txt

@@ -0,0 +1,4 @@
+Files placed in this directory will be deployed to the RoboRIO into the
+  'deploy' directory in the home folder. Use the 'frc::filesystem::GetDeployDirectory'
+  function from the 'frc/Filesystem.h' header to get a proper path relative to the deploy
+  directory.

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

@@ -0,0 +1,125 @@
+/*
+ * 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/acceleration.h>
+#include <units/angular_acceleration.h>
+#include <units/angular_velocity.h>
+#include <units/length.h>
+#include <units/velocity.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, -30_deg, 0_rad}};
+inline const frc::AprilTagFieldLayout kTagLayout{
+    frc::AprilTagFieldLayout::LoadField(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 {
+using namespace units;
+
+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;
+
+using namespace units;
+
+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};
+
+  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(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};
+};

photonlib-cpp-examples/poseest/src/test/cpp/main.cpp

@@ -0,0 +1,35 @@
+/*
+ * 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.
+ */
+
+#include <hal/HAL.h>
+
+#include "gtest/gtest.h"
+
+int main(int argc, char** argv) {
+  HAL_Initialize(500, 0);
+  ::testing::InitGoogleTest(&argc, argv);
+  int ret = RUN_ALL_TESTS();
+  HAL_Shutdown();
+  return ret;
+}