51template <
size_t NumModules>
53 :
public CommandHelper<Command, SwerveControllerCommand<NumModules>> {
54 using voltsecondspermeter =
57 using voltsecondssquaredpermeter =
94 std::function<
void(std::array<frc::SwerveModuleState, NumModules>)>
97 : m_trajectory(
std::move(trajectory)),
98 m_pose(
std::move(pose)),
99 m_kinematics(kinematics),
100 m_controller(xController, yController, thetaController),
101 m_desiredRotation(
std::move(desiredRotation)),
102 m_outputStates(output) {
103 this->AddRequirements(requirements);
140 std::function<
void(std::array<frc::SwerveModuleState, NumModules>)>
143 : m_trajectory(
std::move(trajectory)),
144 m_pose(
std::move(pose)),
145 m_kinematics(kinematics),
146 m_controller(xController, yController, thetaController),
147 m_outputStates(output) {
148 this->AddRequirements(requirements);
177 std::function<
void(std::array<frc::SwerveModuleState, NumModules>)>
180 : m_trajectory(
std::move(trajectory)),
181 m_pose(
std::move(pose)),
182 m_kinematics(kinematics),
183 m_controller(
std::move(controller)),
184 m_desiredRotation(
std::move(desiredRotation)),
185 m_outputStates(output) {
186 this->AddRequirements(requirements);
217 std::function<
void(std::array<frc::SwerveModuleState, NumModules>)>
220 : m_trajectory(
std::move(trajectory)),
221 m_pose(
std::move(pose)),
222 m_kinematics(kinematics),
223 m_controller(
std::move(controller)),
224 m_outputStates(output) {
225 this->AddRequirements(requirements);
229 if (m_desiredRotation ==
nullptr) {
230 m_desiredRotation = [&] {
231 return m_trajectory.
States().back().pose.Rotation();
238 auto curTime = m_timer.
Get();
239 auto m_desiredState = m_trajectory.
Sample(curTime);
241 auto targetChassisSpeeds =
242 m_controller.
Calculate(m_pose(), m_desiredState, m_desiredRotation());
243 auto targetModuleStates =
244 m_kinematics.ToSwerveModuleStates(targetChassisSpeeds);
246 m_outputStates(targetModuleStates);
249 void End(
bool interrupted)
override { m_timer.
Stop(); }
260 std::function<void(std::array<frc::SwerveModuleState, NumModules>)>
266 units::second_t m_prevTime;
CRTP implementation to allow polymorphic decorator functions in Command.
Definition CommandHelper.h:25
Represents requirements for a command, which is a set of (pointers to) subsystems.
Definition Requirements.h:20
A command that uses two PID controllers (PIDController) and a profiled PID controller (ProfiledPIDCon...
Definition SwerveControllerCommand.h:53
SwerveControllerCommand(frc::Trajectory trajectory, std::function< frc::Pose2d()> pose, frc::SwerveDriveKinematics< NumModules > kinematics, frc::PIDController xController, frc::PIDController yController, frc::ProfiledPIDController< units::radians > thetaController, std::function< frc::Rotation2d()> desiredRotation, std::function< void(std::array< frc::SwerveModuleState, NumModules >)> output, Requirements requirements={})
Constructs a new SwerveControllerCommand that when executed will follow the provided trajectory.
Definition SwerveControllerCommand.h:88
void End(bool interrupted) override
Definition SwerveControllerCommand.h:249
SwerveControllerCommand(frc::Trajectory trajectory, std::function< frc::Pose2d()> pose, frc::SwerveDriveKinematics< NumModules > kinematics, frc::HolonomicDriveController controller, std::function< void(std::array< frc::SwerveModuleState, NumModules >)> output, Requirements requirements={})
Constructs a new SwerveControllerCommand that when executed will follow the provided trajectory.
Definition SwerveControllerCommand.h:213
void Execute() override
Definition SwerveControllerCommand.h:237
void Initialize() override
Definition SwerveControllerCommand.h:228
SwerveControllerCommand(frc::Trajectory trajectory, std::function< frc::Pose2d()> pose, frc::SwerveDriveKinematics< NumModules > kinematics, frc::PIDController xController, frc::PIDController yController, frc::ProfiledPIDController< units::radians > thetaController, std::function< void(std::array< frc::SwerveModuleState, NumModules >)> output, Requirements requirements={})
Constructs a new SwerveControllerCommand that when executed will follow the provided trajectory.
Definition SwerveControllerCommand.h:135
bool IsFinished() override
Definition SwerveControllerCommand.h:251
SwerveControllerCommand(frc::Trajectory trajectory, std::function< frc::Pose2d()> pose, frc::SwerveDriveKinematics< NumModules > kinematics, frc::HolonomicDriveController controller, std::function< frc::Rotation2d()> desiredRotation, std::function< void(std::array< frc::SwerveModuleState, NumModules >)> output, Requirements requirements={})
Constructs a new SwerveControllerCommand that when executed will follow the provided trajectory.
Definition SwerveControllerCommand.h:172
This holonomic drive controller can be used to follow trajectories using a holonomic drivetrain (i....
Definition HolonomicDriveController.h:35
constexpr ChassisSpeeds Calculate(const Pose2d ¤tPose, const Pose2d &trajectoryPose, units::meters_per_second_t desiredLinearVelocity, const Rotation2d &desiredHeading)
Returns the next output of the holonomic drive controller.
Definition HolonomicDriveController.h:97
Implements a PID control loop.
Definition PIDController.h:29
Represents a 2D pose containing translational and rotational elements.
Definition Pose2d.h:28
Implements a PID control loop whose setpoint is constrained by a trapezoid profile.
Definition ProfiledPIDController.h:34
A rotation in a 2D coordinate frame represented by a point on the unit circle (cosine and sine).
Definition Rotation2d.h:31
Helper class that converts a chassis velocity (dx, dy, and dtheta components) into individual module ...
Definition SwerveDriveKinematics.h:54
A timer class.
Definition Timer.h:36
void Restart()
Restart the timer by stopping the timer, if it is not already stopped, resetting the accumulated time...
units::second_t Get() const
Get the current time from the timer.
bool HasElapsed(units::second_t period) const
Check if the period specified has passed.
void Stop()
Stop the timer.
Represents a time-parameterized trajectory.
Definition Trajectory.h:29
units::second_t TotalTime() const
Returns the overall duration of the trajectory.
Definition Trajectory.h:125
State Sample(units::second_t t) const
Sample the trajectory at a point in time.
Definition Trajectory.h:141
const std::vector< State > & States() const
Return the states of the trajectory.
Definition Trajectory.h:132
typename units::detail::inverse_impl< U >::type inverse
represents the inverse unit type of class U.
Definition base.h:1138
typename units::detail::compound_impl< U, Us... >::type compound_unit
Represents a unit type made up from other units.
Definition base.h:1438
Definition FunctionalCommand.h:13
Implement std::hash so that hash_code can be used in STL containers.
Definition PointerIntPair.h:280