Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-19 00:41:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

Update to match new WPILib organization

Browse Source
This commit is contained in:
Gold856
2025-12-29 16:16:56 -05:00
committed by samfreund
parent c34c854583
commit 934eed21d2
264 changed files with 3440 additions and 3299 deletions
Download Patch File Download Diff File Expand all files Collapse all files

103
photonlib-cpp-examples/aimattarget/src/main/include/subsystems/SwerveDriveSim.h
View File

@@ -26,77 +26,80 @@
#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>
#include <wpi/math/controller/SimpleMotorFeedforward.hpp>
#include <wpi/math/kinematics/wpi/math/kinematics/SwerveDriveKinematics.hpppp>
#include <wpi/math/system/LinearSystem.hpp>
#include <wpi/math/system/plant/DCMotor.hpp>
#include <wpi/units/voltage.hpp>
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);
SwerveDriveSim(
const wpi::math::SimpleMotorFeedforward<wpi::units::meters>& driveFF,
const wpi::math::DCMotor& driveMotor, double driveGearing,
wpi::units::meter_t driveWheelRadius,
const wpi::math::SimpleMotorFeedforward<wpi::units::radians>& steerFF,
const wpi::math::DCMotor& steerMotor, double steerGearing,
const wpi::math::SwerveDriveKinematics<numModules>& kinematics);
SwerveDriveSim(
const wpi::math::LinearSystem<2, 1, 2>& drivePlant, wpi::units::volt_t driveKs,
const wpi::math::DCMotor& driveMotor, double driveGearing,
wpi::units::meter_t driveWheelRadius,
const wpi::math::LinearSystem<2, 1, 2>& steerPlant, wpi::units::volt_t steerKs,
const wpi::math::DCMotor& steerMotor, double steerGearing,
const wpi::math::SwerveDriveKinematics<numModules>& kinematics);
void SetDriveInputs(const std::array<wpi::units::volt_t, numModules>& inputs);
void SetSteerInputs(const std::array<wpi::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 Eigen::Matrix<double, 2, 1>& x, wpi::units::volt_t input,
wpi::units::volt_t kS);
void Update(wpi::units::second_t dt);
void Reset(const wpi::math::Pose2d& pose, bool preserveMotion);
void Reset(const wpi::math::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();
wpi::math::Pose2d GetPose() const;
std::array<wpi::math::SwerveModulePosition, numModules> GetModulePositions()
const;
std::array<wpi::math::SwerveModulePosition, numModules>
GetNoisyModulePositions(wpi::units::meter_t driveStdDev,
wpi::units::radian_t steerStdDev);
std::array<wpi::math::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;
wpi::units::radians_per_second_t GetOmega() const;
wpi::units::ampere_t GetCurrentDraw(const wpi::math::DCMotor& motor,
wpi::units::radians_per_second_t velocity,
wpi::units::volt_t inputVolts,
wpi::units::volt_t batteryVolts) const;
std::array<wpi::units::ampere_t, numModules> GetDriveCurrentDraw() const;
std::array<wpi::units::ampere_t, numModules> GetSteerCurrentDraw() const;
wpi::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 wpi::math::LinearSystem<2, 1, 2> drivePlant;
const wpi::units::volt_t driveKs;
const wpi::math::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 wpi::units::meter_t driveWheelRadius;
const wpi::math::LinearSystem<2, 1, 2> steerPlant;
const wpi::units::volt_t steerKs;
const wpi::math::DCMotor steerMotor;
const double steerGearing;
const frc::SwerveDriveKinematics<numModules> kinematics;
std::array<units::volt_t, numModules> driveInputs{};
const wpi::math::SwerveDriveKinematics<numModules> kinematics;
std::array<wpi::units::volt_t, numModules> driveInputs{};
std::array<Eigen::Matrix<double, 2, 1>, numModules> driveStates{};
std::array<units::volt_t, numModules> steerInputs{};
std::array<wpi::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};
wpi::math::Pose2d pose{wpi::math::Pose2d{}};
wpi::units::radians_per_second_t omega{0};
};