Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +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

6
wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp
View File

@@ -41,8 +41,10 @@ class Robot : public frc::TimedRobot {
// Create a PID controller whose setpoint's change is subject to maximum
// velocity and acceleration constraints.
frc::TrapezoidProfile::Constraints m_constraints{1.75_mps, 0.75_mps_sq};
frc::ProfiledPIDController m_controller{1.3, 0.0, 0.7, m_constraints, kDt};
frc::TrapezoidProfile<units::meters>::Constraints m_constraints{1.75_mps,
0.75_mps_sq};
frc::ProfiledPIDController<units::meters> m_controller{1.3, 0.0, 0.7,
m_constraints, kDt};
};
#ifndef RUNNING_FRC_TESTS

10
wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp
View File

@@ -31,7 +31,8 @@ class Robot : public frc::TimedRobot {
// Create a motion profile with the given maximum velocity and maximum
// acceleration constraints for the next setpoint, the desired goal, and the
// current setpoint.
frc::TrapezoidProfile profile{m_constraints, m_goal, m_setpoint};
frc::TrapezoidProfile<units::meter> profile{m_constraints, m_goal,
m_setpoint};
// Retrieve the profiled setpoint for the next timestep. This setpoint moves
// toward the goal while obeying the constraints.
@@ -49,9 +50,10 @@ class Robot : public frc::TimedRobot {
frc::PWMVictorSPX m_motor{1};
frc2::PIDController m_controller{1.3, 0.0, 0.7, kDt};
frc::TrapezoidProfile::Constraints m_constraints{1.75_mps, 0.75_mps_sq};
frc::TrapezoidProfile::State m_goal;
frc::TrapezoidProfile::State m_setpoint;
frc::TrapezoidProfile<units::meters>::Constraints m_constraints{1.75_mps,
0.75_mps_sq};
frc::TrapezoidProfile<units::meters>::State m_goal;
frc::TrapezoidProfile<units::meters>::State m_setpoint;
};
#ifndef RUNNING_FRC_TESTS

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,