Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

Template C++ TrapezoidProfile and ProfiledPIDController on units (#2109)

Browse Source
This commit is contained in:
Oblarg
2019-11-20 23:11:46 -05:00
committed by Peter Johnson
parent f62e23f1af
commit fa85fbfc1c
16 changed files with 320 additions and 448 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp
View File

@@ -26,7 +26,8 @@ SwerveModule::SwerveModule(const int driveMotorChannel,
// Limit the PID Controller's input range between -pi and pi and set the input
// to be continuous.
m_turningPIDController.EnableContinuousInput(-wpi::math::pi, wpi::math::pi);
m_turningPIDController.EnableContinuousInput(-units::radian_t(wpi::math::pi),
units::radian_t(wpi::math::pi));
}
frc::SwerveModuleState SwerveModule::GetState() const {
@@ -41,10 +42,7 @@ void SwerveModule::SetDesiredState(const frc::SwerveModuleState& state) {
// Calculate the turning motor output from the turning PID controller.
const auto turnOutput = m_turningPIDController.Calculate(
units::meter_t(m_turningEncoder.Get()),
// We have to convert to the meters type here because that's what
// ProfiledPIDController wants.
units::meter_t(state.angle.Radians().to<double>()));
units::radian_t(m_turningEncoder.Get()), state.angle.Radians());
// Set the motor outputs.
m_driveMotor.Set(driveOutput);

15
wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h
View File

@@ -24,15 +24,10 @@ class SwerveModule {
static constexpr double kWheelRadius = 0.0508;
static constexpr int kEncoderResolution = 4096;
// We have to use meters here instead of radians because of the fact that
// ProfiledPIDController's constraints only take in meters per second and
// meters per second squared.
static constexpr units::meters_per_second_t kModuleMaxAngularVelocity =
units::meters_per_second_t(wpi::math::pi); // radians per second
static constexpr units::meters_per_second_squared_t
kModuleMaxAngularAcceleration = units::meters_per_second_squared_t(
wpi::math::pi * 2.0); // radians per second squared
static constexpr auto kModuleMaxAngularVelocity =
wpi::math::pi * 1_rad_per_s; // radians per second
static constexpr auto kModuleMaxAngularAcceleration =
wpi::math::pi * 2_rad_per_s / 1_s; // radians per second^2
frc::PWMVictorSPX m_driveMotor;
frc::PWMVictorSPX m_turningMotor;
@@ -41,7 +36,7 @@ class SwerveModule {
frc::Encoder m_turningEncoder{2, 3};
frc2::PIDController m_drivePIDController{1.0, 0, 0};
frc::ProfiledPIDController m_turningPIDController{
frc::ProfiledPIDController<units::radians> m_turningPIDController{
1.0,
0.0,
0.0,