WPILibC++ 2025.2.1
Loading...
Searching...
No Matches
SwerveControllerCommand.h
Go to the documentation of this file.
1// Copyright (c) FIRST and other WPILib contributors.
2// Open Source Software; you can modify and/or share it under the terms of
3// the WPILib BSD license file in the root directory of this project.
4
5#include <functional>
6#include <memory>
7#include <utility>
8
9#include <frc/Timer.h>
13#include <frc/geometry/Pose2d.h>
18#include <units/length.h>
19#include <units/time.h>
20#include <units/voltage.h>
21
25
26#pragma once
27
28namespace frc2 {
29
30/**
31 * A command that uses two PID controllers (PIDController) and a profiled PID
32 * controller (ProfiledPIDController) to follow a trajectory (Trajectory) with a
33 * swerve drive.
34 *
35 * <p>The command handles trajectory-following, Velocity PID calculations, and
36 * feedforwards internally. This is intended to be a more-or-less "complete
37 * solution" that can be used by teams without a great deal of controls
38 * expertise.
39 *
40 * <p>Advanced teams seeking more flexibility (for example, those who wish to
41 * use the onboard PID functionality of a "smart" motor controller) may use the
42 * secondary constructor that omits the PID and feedforward functionality,
43 * returning only the raw module states from the position PID controllers.
44 *
45 * <p>The robot angle controller does not follow the angle given by
46 * the trajectory but rather goes to the angle given in the final state of the
47 * trajectory.
48 *
49 * This class is provided by the NewCommands VendorDep
50 */
51template <size_t NumModules>
53 : public CommandHelper<Command, SwerveControllerCommand<NumModules>> {
54 using voltsecondspermeter =
55 units::compound_unit<units::voltage::volt, units::second,
57 using voltsecondssquaredpermeter =
60
61 public:
62 /**
63 * Constructs a new SwerveControllerCommand that when executed will follow the
64 * provided trajectory. This command will not return output voltages but
65 * rather raw module states from the position controllers which need to be put
66 * into a velocity PID.
67 *
68 * <p>Note: The controllers will *not* set the outputVolts to zero upon
69 * completion of the path- this is left to the user, since it is not
70 * appropriate for paths with nonstationary endstates.
71 *
72 * @param trajectory The trajectory to follow.
73 * @param pose A function that supplies the robot pose,
74 * provided by the odometry class.
75 * @param kinematics The kinematics for the robot drivetrain.
76 * @param xController The Trajectory Tracker PID controller
77 * for the robot's x position.
78 * @param yController The Trajectory Tracker PID controller
79 * for the robot's y position.
80 * @param thetaController The Trajectory Tracker PID controller
81 * for angle for the robot.
82 * @param desiredRotation The angle that the drivetrain should be
83 * facing. This is sampled at each time step.
84 * @param output The raw output module states from the
85 * position controllers.
86 * @param requirements The subsystems to require.
87 */
89 frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
91 frc::PIDController xController, frc::PIDController yController,
93 std::function<frc::Rotation2d()> desiredRotation,
94 std::function<void(std::array<frc::SwerveModuleState, NumModules>)>
95 output,
96 Requirements requirements = {})
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);
104 }
105
106 /**
107 * Constructs a new SwerveControllerCommand that when executed will follow the
108 * provided trajectory. This command will not return output voltages but
109 * rather raw module states from the position controllers which need to be put
110 * into a velocity PID.
111 *
112 * <p>Note: The controllers will *not* set the outputVolts to zero upon
113 * completion of the path- this is left to the user, since it is not
114 * appropriate for paths with nonstationary endstates.
115 *
116 * <p>Note 2: The final rotation of the robot will be set to the rotation of
117 * the final pose in the trajectory. The robot will not follow the rotations
118 * from the poses at each timestep. If alternate rotation behavior is desired,
119 * the other constructor with a supplier for rotation should be used.
120 *
121 * @param trajectory The trajectory to follow.
122 * @param pose A function that supplies the robot pose,
123 * provided by the odometry class.
124 * @param kinematics The kinematics for the robot drivetrain.
125 * @param xController The Trajectory Tracker PID controller
126 * for the robot's x position.
127 * @param yController The Trajectory Tracker PID controller
128 * for the robot's y position.
129 * @param thetaController The Trajectory Tracker PID controller
130 * for angle for the robot.
131 * @param output The raw output module states from the
132 * position controllers.
133 * @param requirements The subsystems to require.
134 */
136 frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
138 frc::PIDController xController, frc::PIDController yController,
140 std::function<void(std::array<frc::SwerveModuleState, NumModules>)>
141 output,
142 Requirements requirements = {})
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);
149 }
150
151 /**
152 * Constructs a new SwerveControllerCommand that when executed will follow the
153 * provided trajectory. This command will not return output voltages but
154 * rather raw module states from the position controllers which need to be put
155 * into a velocity PID.
156 *
157 * <p>Note: The controllers will *not* set the outputVolts to zero upon
158 * completion of the path- this is left to the user, since it is not
159 * appropriate for paths with nonstationary endstates.
160 *
161 * @param trajectory The trajectory to follow.
162 * @param pose A function that supplies the robot pose,
163 * provided by the odometry class.
164 * @param kinematics The kinematics for the robot drivetrain.
165 * @param controller The HolonomicDriveController for the drivetrain.
166 * @param desiredRotation The angle that the drivetrain should be
167 * facing. This is sampled at each time step.
168 * @param output The raw output module states from the
169 * position controllers.
170 * @param requirements The subsystems to require.
171 */
173 frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
176 std::function<frc::Rotation2d()> desiredRotation,
177 std::function<void(std::array<frc::SwerveModuleState, NumModules>)>
178 output,
179 Requirements requirements = {})
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);
187 }
188
189 /**
190 * Constructs a new SwerveControllerCommand that when executed will follow the
191 * provided trajectory. This command will not return output voltages but
192 * rather raw module states from the position controllers which need to be put
193 * into a velocity PID.
194 *
195 * <p>Note: The controllers will *not* set the outputVolts to zero upon
196 * completion of the path- this is left to the user, since it is not
197 * appropriate for paths with nonstationary endstates.
198 *
199 * <p>Note 2: The final rotation of the robot will be set to the rotation of
200 * the final pose in the trajectory. The robot will not follow the rotations
201 * from the poses at each timestep. If alternate rotation behavior is desired,
202 * the other constructor with a supplier for rotation should be used.
203 *
204 * @param trajectory The trajectory to follow.
205 * @param pose A function that supplies the robot pose,
206 * provided by the odometry class.
207 * @param kinematics The kinematics for the robot drivetrain.
208 * @param controller The HolonomicDriveController for the drivetrain.
209 * @param output The raw output module states from the
210 * position controllers.
211 * @param requirements The subsystems to require.
212 */
214 frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
217 std::function<void(std::array<frc::SwerveModuleState, NumModules>)>
218 output,
219 Requirements requirements = {})
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);
226 }
227
228 void Initialize() override {
229 if (m_desiredRotation == nullptr) {
230 m_desiredRotation = [&] {
231 return m_trajectory.States().back().pose.Rotation();
232 };
233 }
234 m_timer.Restart();
235 }
236
237 void Execute() override {
238 auto curTime = m_timer.Get();
239 auto m_desiredState = m_trajectory.Sample(curTime);
240
241 auto targetChassisSpeeds =
242 m_controller.Calculate(m_pose(), m_desiredState, m_desiredRotation());
243 auto targetModuleStates =
244 m_kinematics.ToSwerveModuleStates(targetChassisSpeeds);
245
246 m_outputStates(targetModuleStates);
247 }
248
249 void End(bool interrupted) override { m_timer.Stop(); }
250
251 bool IsFinished() override {
252 return m_timer.HasElapsed(m_trajectory.TotalTime());
253 }
254
255 private:
256 frc::Trajectory m_trajectory;
257 std::function<frc::Pose2d()> m_pose;
260 std::function<void(std::array<frc::SwerveModuleState, NumModules>)>
261 m_outputStates;
262
263 std::function<frc::Rotation2d()> m_desiredRotation;
264
265 frc::Timer m_timer;
266 units::second_t m_prevTime;
267 frc::Rotation2d m_finalRotation;
268};
269
270} // namespace frc2
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 &currentPose, 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