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

90
photonlib-cpp-examples/aimattarget/src/main/cpp/subsystems/SwerveModule.cpp
View File

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