Motion Profiling in Command-based
הערה
For a description of the WPILib motion profiling features used by these command-based wrappers, see Trapezoidal Motion Profiles in WPILib.
הערה
The TrapezoidProfile
class, used on its own, is most useful when composed with external controllers, such as a ”smart“ motor controller with a built-in PID functionality. For combining trapezoidal motion profiling with WPILib’s PIDController
, see Combining Motion Profiling and PID in Command-Based.
When controlling a mechanism, is often desirable to move it smoothly between two positions, rather than to abruptly change its setpoint. This is called ”motion-profiling,“ and is supported in WPILib through the TrapezoidProfile
class (Java, C++).
הערה
In C++, the TrapezoidProfile
class is templated on the unit type used for distance measurements, which may be angular or linear. The passed-in values must have units consistent with the distance units, or a compile-time error will be thrown. For more information on C++ units, see The C++ Units Library.
The following examples are taken from the DriveDistanceOffboard example project (Java, C++):
5package edu.wpi.first.wpilibj.examples.drivedistanceoffboard.subsystems;
6
7import static edu.wpi.first.units.Units.MetersPerSecond;
8import static edu.wpi.first.units.Units.Volts;
9
10import edu.wpi.first.math.controller.SimpleMotorFeedforward;
11import edu.wpi.first.math.trajectory.TrapezoidProfile;
12import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
13import edu.wpi.first.util.sendable.SendableRegistry;
14import edu.wpi.first.wpilibj.RobotController;
15import edu.wpi.first.wpilibj.Timer;
16import edu.wpi.first.wpilibj.drive.DifferentialDrive;
17import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants;
18import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.ExampleSmartMotorController;
19import edu.wpi.first.wpilibj2.command.Command;
20import edu.wpi.first.wpilibj2.command.SubsystemBase;
21
22public class DriveSubsystem extends SubsystemBase {
23 // The motors on the left side of the drive.
24 private final ExampleSmartMotorController m_leftLeader =
25 new ExampleSmartMotorController(DriveConstants.kLeftMotor1Port);
26
27 private final ExampleSmartMotorController m_leftFollower =
28 new ExampleSmartMotorController(DriveConstants.kLeftMotor2Port);
29
30 // The motors on the right side of the drive.
31 private final ExampleSmartMotorController m_rightLeader =
32 new ExampleSmartMotorController(DriveConstants.kRightMotor1Port);
33
34 private final ExampleSmartMotorController m_rightFollower =
35 new ExampleSmartMotorController(DriveConstants.kRightMotor2Port);
36
37 // The feedforward controller.
38 private final SimpleMotorFeedforward m_feedforward =
39 new SimpleMotorFeedforward(
40 DriveConstants.ksVolts,
41 DriveConstants.kvVoltSecondsPerMeter,
42 DriveConstants.kaVoltSecondsSquaredPerMeter);
43
44 // The robot's drive
45 private final DifferentialDrive m_drive =
46 new DifferentialDrive(m_leftLeader::set, m_rightLeader::set);
47
48 // The trapezoid profile
49 private final TrapezoidProfile m_profile =
50 new TrapezoidProfile(
51 new TrapezoidProfile.Constraints(
52 DriveConstants.kMaxSpeedMetersPerSecond,
53 DriveConstants.kMaxAccelerationMetersPerSecondSquared));
54
55 // The timer
56 private final Timer m_timer = new Timer();
57
58 /** Creates a new DriveSubsystem. */
59 public DriveSubsystem() {
60 SendableRegistry.addChild(m_drive, m_leftLeader);
61 SendableRegistry.addChild(m_drive, m_rightLeader);
62
63 // We need to invert one side of the drivetrain so that positive voltages
64 // result in both sides moving forward. Depending on how your robot's
65 // gearbox is constructed, you might have to invert the left side instead.
66 m_rightLeader.setInverted(true);
67
68 m_leftFollower.follow(m_leftLeader);
69 m_rightFollower.follow(m_rightLeader);
70
71 m_leftLeader.setPID(DriveConstants.kp, 0, 0);
72 m_rightLeader.setPID(DriveConstants.kp, 0, 0);
73 }
74
75 /**
76 * Drives the robot using arcade controls.
77 *
78 * @param fwd the commanded forward movement
79 * @param rot the commanded rotation
80 */
81 public void arcadeDrive(double fwd, double rot) {
82 m_drive.arcadeDrive(fwd, rot);
83 }
84
85 /**
86 * Attempts to follow the given drive states using offboard PID.
87 *
88 * @param currentLeft The current left wheel state.
89 * @param currentRight The current right wheel state.
90 * @param nextLeft The next left wheel state.
91 * @param nextRight The next right wheel state.
92 */
93 public void setDriveStates(
94 TrapezoidProfile.State currentLeft,
95 TrapezoidProfile.State currentRight,
96 TrapezoidProfile.State nextLeft,
97 TrapezoidProfile.State nextRight) {
98 // Feedforward is divided by battery voltage to normalize it to [-1, 1]
99 m_leftLeader.setSetpoint(
100 ExampleSmartMotorController.PIDMode.kPosition,
101 currentLeft.position,
102 m_feedforward
103 .calculate(
104 MetersPerSecond.of(currentLeft.velocity), MetersPerSecond.of(nextLeft.velocity))
105 .in(Volts)
106 / RobotController.getBatteryVoltage());
107 m_rightLeader.setSetpoint(
108 ExampleSmartMotorController.PIDMode.kPosition,
109 currentRight.position,
110 m_feedforward
111 .calculate(
112 MetersPerSecond.of(currentLeft.velocity), MetersPerSecond.of(nextLeft.velocity))
113 .in(Volts)
114 / RobotController.getBatteryVoltage());
115 }
116
117 /**
118 * Returns the left encoder distance.
119 *
120 * @return the left encoder distance
121 */
122 public double getLeftEncoderDistance() {
123 return m_leftLeader.getEncoderDistance();
124 }
125
126 /**
127 * Returns the right encoder distance.
128 *
129 * @return the right encoder distance
130 */
131 public double getRightEncoderDistance() {
132 return m_rightLeader.getEncoderDistance();
133 }
134
135 /** Resets the drive encoders. */
136 public void resetEncoders() {
137 m_leftLeader.resetEncoder();
138 m_rightLeader.resetEncoder();
139 }
140
141 /**
142 * Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
143 *
144 * @param maxOutput the maximum output to which the drive will be constrained
145 */
146 public void setMaxOutput(double maxOutput) {
147 m_drive.setMaxOutput(maxOutput);
148 }
149
150 /**
151 * Creates a command to drive forward a specified distance using a motion profile.
152 *
153 * @param distance The distance to drive forward.
154 * @return A command.
155 */
156 public Command profiledDriveDistance(double distance) {
157 return startRun(
158 () -> {
159 // Restart timer so profile setpoints start at the beginning
160 m_timer.restart();
161 resetEncoders();
162 },
163 () -> {
164 // Current state never changes, so we need to use a timer to get the setpoints we need
165 // to be at
166 var currentTime = m_timer.get();
167 var currentSetpoint =
168 m_profile.calculate(currentTime, new State(), new State(distance, 0));
169 var nextSetpoint =
170 m_profile.calculate(
171 currentTime + DriveConstants.kDt, new State(), new State(distance, 0));
172 setDriveStates(currentSetpoint, currentSetpoint, nextSetpoint, nextSetpoint);
173 })
174 .until(() -> m_profile.isFinished(0));
175 }
176
177 private double m_initialLeftDistance;
178 private double m_initialRightDistance;
179
180 /**
181 * Creates a command to drive forward a specified distance using a motion profile without
182 * resetting the encoders.
183 *
184 * @param distance The distance to drive forward.
185 * @return A command.
186 */
187 public Command dynamicProfiledDriveDistance(double distance) {
188 return startRun(
189 () -> {
190 // Restart timer so profile setpoints start at the beginning
191 m_timer.restart();
192 // Store distance so we know the target distance for each encoder
193 m_initialLeftDistance = getLeftEncoderDistance();
194 m_initialRightDistance = getRightEncoderDistance();
195 },
196 () -> {
197 // Current state never changes for the duration of the command, so we need to use a
198 // timer to get the setpoints we need to be at
199 var currentTime = m_timer.get();
200 var currentLeftSetpoint =
201 m_profile.calculate(
202 currentTime,
203 new State(m_initialLeftDistance, 0),
204 new State(m_initialLeftDistance + distance, 0));
205 var currentRightSetpoint =
206 m_profile.calculate(
207 currentTime,
208 new State(m_initialRightDistance, 0),
209 new State(m_initialRightDistance + distance, 0));
210 var nextLeftSetpoint =
211 m_profile.calculate(
212 currentTime + DriveConstants.kDt,
213 new State(m_initialLeftDistance, 0),
214 new State(m_initialLeftDistance + distance, 0));
215 var nextRightSetpoint =
216 m_profile.calculate(
217 currentTime + DriveConstants.kDt,
218 new State(m_initialRightDistance, 0),
219 new State(m_initialRightDistance + distance, 0));
220 setDriveStates(
221 currentLeftSetpoint, currentRightSetpoint, nextLeftSetpoint, nextRightSetpoint);
222 })
223 .until(() -> m_profile.isFinished(0));
224 }
225}
5#pragma once
6
7#include <frc/Encoder.h>
8#include <frc/Timer.h>
9#include <frc/controller/SimpleMotorFeedforward.h>
10#include <frc/drive/DifferentialDrive.h>
11#include <frc/trajectory/TrapezoidProfile.h>
12#include <frc2/command/CommandPtr.h>
13#include <frc2/command/SubsystemBase.h>
14#include <units/length.h>
15
16#include "Constants.h"
17#include "ExampleSmartMotorController.h"
18
19class DriveSubsystem : public frc2::SubsystemBase {
20 public:
21 DriveSubsystem();
22
23 /**
24 * Will be called periodically whenever the CommandScheduler runs.
25 */
26 void Periodic() override;
27
28 // Subsystem methods go here.
29
30 /**
31 * Attempts to follow the given drive states using offboard PID.
32 *
33 * @param currentLeft The current left wheel state.
34 * @param currentRight The current right wheel state.
35 * @param nextLeft The next left wheel state.
36 * @param nextRight The next right wheel state.
37 */
38 void SetDriveStates(frc::TrapezoidProfile<units::meters>::State currentLeft,
39 frc::TrapezoidProfile<units::meters>::State currentRight,
40 frc::TrapezoidProfile<units::meters>::State nextLeft,
41 frc::TrapezoidProfile<units::meters>::State nextRight);
42
43 /**
44 * Drives the robot using arcade controls.
45 *
46 * @param fwd the commanded forward movement
47 * @param rot the commanded rotation
48 */
49 void ArcadeDrive(double fwd, double rot);
50
51 /**
52 * Resets the drive encoders to currently read a position of 0.
53 */
54 void ResetEncoders();
55
56 /**
57 * Gets the distance of the left encoder.
58 *
59 * @return the average of the TWO encoder readings
60 */
61 units::meter_t GetLeftEncoderDistance();
62
63 /**
64 * Gets the distance of the right encoder.
65 *
66 * @return the average of the TWO encoder readings
67 */
68 units::meter_t GetRightEncoderDistance();
69
70 /**
71 * Sets the max output of the drive. Useful for scaling the drive to drive
72 * more slowly.
73 *
74 * @param maxOutput the maximum output to which the drive will be constrained
75 */
76 void SetMaxOutput(double maxOutput);
77
78 /**
79 * Creates a command to drive forward a specified distance using a motion
80 * profile.
81 *
82 * @param distance The distance to drive forward.
83 * @return A command.
84 */
85 [[nodiscard]]
86 frc2::CommandPtr ProfiledDriveDistance(units::meter_t distance);
87
88 /**
89 * Creates a command to drive forward a specified distance using a motion
90 * profile without resetting the encoders.
91 *
92 * @param distance The distance to drive forward.
93 * @return A command.
94 */
95 [[nodiscard]]
96 frc2::CommandPtr DynamicProfiledDriveDistance(units::meter_t distance);
97
98 private:
99 frc::TrapezoidProfile<units::meters> m_profile{
100 {DriveConstants::kMaxSpeed, DriveConstants::kMaxAcceleration}};
101 frc::Timer m_timer;
102 units::meter_t m_initialLeftDistance;
103 units::meter_t m_initialRightDistance;
104 // Components (e.g. motor controllers and sensors) should generally be
105 // declared private and exposed only through public methods.
106
107 // The motor controllers
108 ExampleSmartMotorController m_leftLeader;
109 ExampleSmartMotorController m_leftFollower;
110 ExampleSmartMotorController m_rightLeader;
111 ExampleSmartMotorController m_rightFollower;
112
113 // A feedforward component for the drive
114 frc::SimpleMotorFeedforward<units::meters> m_feedforward;
115
116 // The robot's drive
117 frc::DifferentialDrive m_drive{
118 [&](double output) { m_leftLeader.Set(output); },
119 [&](double output) { m_rightLeader.Set(output); }};
120};
5#include "subsystems/DriveSubsystem.h"
6
7#include <frc/RobotController.h>
8
9using namespace DriveConstants;
10
11DriveSubsystem::DriveSubsystem()
12 : m_leftLeader{kLeftMotor1Port},
13 m_leftFollower{kLeftMotor2Port},
14 m_rightLeader{kRightMotor1Port},
15 m_rightFollower{kRightMotor2Port},
16 m_feedforward{ks, kv, ka} {
17 wpi::SendableRegistry::AddChild(&m_drive, &m_leftLeader);
18 wpi::SendableRegistry::AddChild(&m_drive, &m_rightLeader);
19
20 // We need to invert one side of the drivetrain so that positive voltages
21 // result in both sides moving forward. Depending on how your robot's
22 // gearbox is constructed, you might have to invert the left side instead.
23 m_rightLeader.SetInverted(true);
24
25 m_leftFollower.Follow(m_leftLeader);
26 m_rightFollower.Follow(m_rightLeader);
27
28 m_leftLeader.SetPID(kp, 0, 0);
29 m_rightLeader.SetPID(kp, 0, 0);
30}
31
32void DriveSubsystem::Periodic() {
33 // Implementation of subsystem periodic method goes here.
34}
35
36void DriveSubsystem::SetDriveStates(
37 frc::TrapezoidProfile<units::meters>::State currentLeft,
38 frc::TrapezoidProfile<units::meters>::State currentRight,
39 frc::TrapezoidProfile<units::meters>::State nextLeft,
40 frc::TrapezoidProfile<units::meters>::State nextRight) {
41 // Feedforward is divided by battery voltage to normalize it to [-1, 1]
42 m_leftLeader.SetSetpoint(
43 ExampleSmartMotorController::PIDMode::kPosition,
44 currentLeft.position.value(),
45 m_feedforward.Calculate(currentLeft.velocity, nextLeft.velocity) /
46 frc::RobotController::GetBatteryVoltage());
47 m_rightLeader.SetSetpoint(
48 ExampleSmartMotorController::PIDMode::kPosition,
49 currentRight.position.value(),
50 m_feedforward.Calculate(currentRight.velocity, nextRight.velocity) /
51 frc::RobotController::GetBatteryVoltage());
52}
53
54void DriveSubsystem::ArcadeDrive(double fwd, double rot) {
55 m_drive.ArcadeDrive(fwd, rot);
56}
57
58void DriveSubsystem::ResetEncoders() {
59 m_leftLeader.ResetEncoder();
60 m_rightLeader.ResetEncoder();
61}
62
63units::meter_t DriveSubsystem::GetLeftEncoderDistance() {
64 return units::meter_t{m_leftLeader.GetEncoderDistance()};
65}
66
67units::meter_t DriveSubsystem::GetRightEncoderDistance() {
68 return units::meter_t{m_rightLeader.GetEncoderDistance()};
69}
70
71void DriveSubsystem::SetMaxOutput(double maxOutput) {
72 m_drive.SetMaxOutput(maxOutput);
73}
74
75frc2::CommandPtr DriveSubsystem::ProfiledDriveDistance(
76 units::meter_t distance) {
77 return StartRun(
78 [&] {
79 // Restart timer so profile setpoints start at the beginning
80 m_timer.Restart();
81 ResetEncoders();
82 },
83 [&] {
84 // Current state never changes, so we need to use a timer to get
85 // the setpoints we need to be at
86 auto currentTime = m_timer.Get();
87 auto currentSetpoint =
88 m_profile.Calculate(currentTime, {}, {distance, 0_mps});
89 auto nextSetpoint = m_profile.Calculate(currentTime + kDt, {},
90 {distance, 0_mps});
91 SetDriveStates(currentSetpoint, currentSetpoint, nextSetpoint,
92 nextSetpoint);
93 })
94 .Until([&] { return m_profile.IsFinished(0_s); });
95}
96
97frc2::CommandPtr DriveSubsystem::DynamicProfiledDriveDistance(
98 units::meter_t distance) {
99 return StartRun(
100 [&] {
101 // Restart timer so profile setpoints start at the beginning
102 m_timer.Restart();
103 // Store distance so we know the target distance for each encoder
104 m_initialLeftDistance = GetLeftEncoderDistance();
105 m_initialRightDistance = GetRightEncoderDistance();
106 },
107 [&] {
108 // Current state never changes for the duration of the command,
109 // so we need to use a timer to get the setpoints we need to be
110 // at
111 auto currentTime = m_timer.Get();
112
113 auto currentLeftSetpoint = m_profile.Calculate(
114 currentTime, {m_initialLeftDistance, 0_mps},
115 {m_initialLeftDistance + distance, 0_mps});
116 auto currentRightSetpoint = m_profile.Calculate(
117 currentTime, {m_initialRightDistance, 0_mps},
118 {m_initialRightDistance + distance, 0_mps});
119
120 auto nextLeftSetpoint = m_profile.Calculate(
121 currentTime + kDt, {m_initialLeftDistance, 0_mps},
122 {m_initialLeftDistance + distance, 0_mps});
123 auto nextRightSetpoint = m_profile.Calculate(
124 currentTime + kDt, {m_initialRightDistance, 0_mps},
125 {m_initialRightDistance + distance, 0_mps});
126 SetDriveStates(currentLeftSetpoint, currentRightSetpoint,
127 nextLeftSetpoint, nextRightSetpoint);
128 })
129 .Until([&] { return m_profile.IsFinished(0_s); });
130}
There are two commands in this example. They function very similarly, with the main difference being that one resets encoders, and the other doesn’t, which allows encoder data to be preserved.
The subsystem contains a TrapezoidProfile
with a Timer
. The timer is used along with a kDt
constant of 0.02 seconds to calculate the current and next states from the TrapezoidProfile
. The current state is fed to the ”smart“ motor controller for PID control, while the current and next state are used to calculate feedforward outputs. Both commands end when isFinished(0)
returns true, which means that the profile has reached the goal state.