10 #include <initializer_list>
13 #include <frc/controller/PIDController.h>
14 #include <frc/controller/ProfiledPIDController.h>
15 #include <frc/geometry/Pose2d.h>
16 #include <frc/kinematics/ChassisSpeeds.h>
17 #include <frc/kinematics/SwerveDriveKinematics.h>
18 #include <frc/kinematics/SwerveModuleState.h>
19 #include <frc/trajectory/Trajectory.h>
20 #include <units/units.h>
21 #include <wpi/ArrayRef.h>
23 #include "CommandBase.h"
24 #include "CommandHelper.h"
25 #include "frc2/Timer.h"
50 template <
size_t NumModules>
52 :
public CommandHelper<CommandBase, SwerveControllerCommand<NumModules>> {
53 using voltsecondspermeter =
54 units::compound_unit<units::voltage::volt, units::second,
55 units::inverse<units::meter>>;
56 using voltsecondssquaredpermeter =
57 units::compound_unit<units::voltage::volt, units::squared<units::second>,
58 units::inverse<units::meter>>;
93 std::function<
void(std::array<frc::SwerveModuleState, NumModules>)>
95 std::initializer_list<Subsystem*> requirements);
129 std::function<
void(std::array<frc::SwerveModuleState, NumModules>)>
137 void End(
bool interrupted)
override;
145 std::unique_ptr<frc2::PIDController> m_xController;
146 std::unique_ptr<frc2::PIDController> m_yController;
147 std::unique_ptr<frc::ProfiledPIDController<units::radians>> m_thetaController;
148 std::function<void(std::array<frc::SwerveModuleState, NumModules>)>
152 units::second_t m_prevTime;
157 #include "SwerveControllerCommand.inc"
ArrayRef - Represent a constant reference to an array (0 or more elements consecutively in memory),...
Definition: ArrayRef.h:42
A wrapper for the frc::Timer class that returns unit-typed values.
Definition: Timer.h:40
SwerveControllerCommand(frc::Trajectory trajectory, std::function< frc::Pose2d()> pose, frc::SwerveDriveKinematics< NumModules > kinematics, frc2::PIDController xController, frc2::PIDController yController, frc::ProfiledPIDController< units::radians > thetaController, std::function< void(std::array< frc::SwerveModuleState, NumModules >)> output, std::initializer_list< Subsystem * > requirements)
Constructs a new SwerveControllerCommand that when executed will follow the provided trajectory.
Definition: SwerveControllerCommand.inc:15
Represents a 2d pose containing translational and rotational elements.
Definition: Pose2d.h:23
Implements a PID control loop whose setpoint is constrained by a trapezoid profile.
Definition: ProfiledPIDController.h:33
void Execute() override
The main body of a command.
Definition: SwerveControllerCommand.inc:63
Helper class that converts a chassis velocity (dx, dy, and dtheta components) into individual module ...
Definition: SwerveDriveKinematics.h:47
void End(bool interrupted) override
The action to take when the command ends.
Definition: SwerveControllerCommand.inc:100
CRTP implementation to allow polymorphic decorator functions in Command.
Definition: CommandHelper.h:26
Represents a time-parameterized trajectory.
Definition: Trajectory.h:34
void Initialize() override
The initial subroutine of a command.
Definition: SwerveControllerCommand.inc:55
Implements a PID control loop.
Definition: PIDController.h:23
bool IsFinished() override
Whether the command has finished.
Definition: SwerveControllerCommand.inc:105
A command that uses two PID controllers (PIDController) and a ProfiledPIDController (ProfiledPIDContr...
Definition: SwerveControllerCommand.h:51