Combinaison du profilage de mouvement et du PID dans les commandes

Note

Pour une description des fonctionnalités de contrôle PID WPILib utilisées par ces wrappers basés sur des commandes, voir: Contrôle PID dans WPILib.

A common FRC® controls solution is to pair a trapezoidal motion profile for setpoint generation with a PID controller for setpoint tracking. To facilitate this, WPILib includes its own ProfiledPIDController class. The following example is from the RapidReactCommandBot example project (Java, C++) and shows how ProfiledPIDController can be used within the command-based framework to turn a drivetrain to a specified angle:

  5package edu.wpi.first.wpilibj.examples.rapidreactcommandbot.subsystems;
  6
  7import static edu.wpi.first.units.Units.DegreesPerSecond;
  8import static edu.wpi.first.units.Units.Volts;
  9
 10import edu.wpi.first.epilogue.Logged;
 11import edu.wpi.first.epilogue.NotLogged;
 12import edu.wpi.first.math.controller.ProfiledPIDController;
 13import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 14import edu.wpi.first.math.trajectory.TrapezoidProfile;
 15import edu.wpi.first.util.sendable.SendableRegistry;
 16import edu.wpi.first.wpilibj.ADXRS450_Gyro;
 17import edu.wpi.first.wpilibj.Encoder;
 18import edu.wpi.first.wpilibj.RobotController;
 19import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 20import edu.wpi.first.wpilibj.examples.rapidreactcommandbot.Constants.DriveConstants;
 21import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 22import edu.wpi.first.wpilibj2.command.Command;
 23import edu.wpi.first.wpilibj2.command.SubsystemBase;
 24import java.util.function.DoubleSupplier;
 25
 26@Logged
 27public class Drive extends SubsystemBase {
 28  // The motors on the left side of the drive.
 29  private final PWMSparkMax m_leftLeader = new PWMSparkMax(DriveConstants.kLeftMotor1Port);
 30  private final PWMSparkMax m_leftFollower = new PWMSparkMax(DriveConstants.kLeftMotor2Port);
 31
 32  // The motors on the right side of the drive.
 33  private final PWMSparkMax m_rightLeader = new PWMSparkMax(DriveConstants.kRightMotor1Port);
 34  private final PWMSparkMax m_rightFollower = new PWMSparkMax(DriveConstants.kRightMotor2Port);
 35
 36  // The robot's drive
 37  @NotLogged // Would duplicate motor data, there's no point sending it twice
 38  private final DifferentialDrive m_drive =
 39      new DifferentialDrive(m_leftLeader::set, m_rightLeader::set);
 40
 41  // The left-side drive encoder
 42  private final Encoder m_leftEncoder =
 43      new Encoder(
 44          DriveConstants.kLeftEncoderPorts[0],
 45          DriveConstants.kLeftEncoderPorts[1],
 46          DriveConstants.kLeftEncoderReversed);
 47
 48  // The right-side drive encoder
 49  private final Encoder m_rightEncoder =
 50      new Encoder(
 51          DriveConstants.kRightEncoderPorts[0],
 52          DriveConstants.kRightEncoderPorts[1],
 53          DriveConstants.kRightEncoderReversed);
 54
 55  private final ADXRS450_Gyro m_gyro = new ADXRS450_Gyro();
 56  private final ProfiledPIDController m_controller =
 57      new ProfiledPIDController(
 58          DriveConstants.kTurnP,
 59          DriveConstants.kTurnI,
 60          DriveConstants.kTurnD,
 61          new TrapezoidProfile.Constraints(
 62              DriveConstants.kMaxTurnRateDegPerS,
 63              DriveConstants.kMaxTurnAccelerationDegPerSSquared));
 64  private final SimpleMotorFeedforward m_feedforward =
 65      new SimpleMotorFeedforward(
 66          DriveConstants.ksVolts,
 67          DriveConstants.kvVoltSecondsPerDegree,
 68          DriveConstants.kaVoltSecondsSquaredPerDegree);
 69
 70  /** Creates a new Drive subsystem. */
 71  public Drive() {
 72    SendableRegistry.addChild(m_drive, m_leftLeader);
 73    SendableRegistry.addChild(m_drive, m_rightLeader);
 74
 75    m_leftLeader.addFollower(m_leftFollower);
 76    m_rightLeader.addFollower(m_rightFollower);
 77
 78    // We need to invert one side of the drivetrain so that positive voltages
 79    // result in both sides moving forward. Depending on how your robot's
 80    // gearbox is constructed, you might have to invert the left side instead.
 81    m_rightLeader.setInverted(true);
 82
 83    // Sets the distance per pulse for the encoders
 84    m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
 85    m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
 86
 87    // Set the controller to be continuous (because it is an angle controller)
 88    m_controller.enableContinuousInput(-180, 180);
 89    // Set the controller tolerance - the delta tolerance ensures the robot is stationary at the
 90    // setpoint before it is considered as having reached the reference
 91    m_controller.setTolerance(
 92        DriveConstants.kTurnToleranceDeg, DriveConstants.kTurnRateToleranceDegPerS);
 93  }
 94
 95  /**
 96   * Returns a command that drives the robot with arcade controls.
 97   *
 98   * @param fwd the commanded forward movement
 99   * @param rot the commanded rotation
100   */
101  public Command arcadeDriveCommand(DoubleSupplier fwd, DoubleSupplier rot) {
102    // A split-stick arcade command, with forward/backward controlled by the left
103    // hand, and turning controlled by the right.
104    return run(() -> m_drive.arcadeDrive(fwd.getAsDouble(), rot.getAsDouble()))
105        .withName("arcadeDrive");
106  }
107
108  /**
109   * Returns a command that drives the robot forward a specified distance at a specified speed.
110   *
111   * @param distanceMeters The distance to drive forward in meters
112   * @param speed The fraction of max speed at which to drive
113   */
114  public Command driveDistanceCommand(double distanceMeters, double speed) {
115    return runOnce(
116            () -> {
117              // Reset encoders at the start of the command
118              m_leftEncoder.reset();
119              m_rightEncoder.reset();
120            })
121        // Drive forward at specified speed
122        .andThen(run(() -> m_drive.arcadeDrive(speed, 0)))
123        // End command when we've traveled the specified distance
124        .until(
125            () ->
126                Math.max(m_leftEncoder.getDistance(), m_rightEncoder.getDistance())
127                    >= distanceMeters)
128        // Stop the drive when the command ends
129        .finallyDo(interrupted -> m_drive.stopMotor());
130  }
131
132  /**
133   * Returns a command that turns to robot to the specified angle using a motion profile and PID
134   * controller.
135   *
136   * @param angleDeg The angle to turn to
137   */
138  public Command turnToAngleCommand(double angleDeg) {
139    return startRun(
140            () -> m_controller.reset(m_gyro.getRotation2d().getDegrees()),
141            () ->
142                m_drive.arcadeDrive(
143                    0,
144                    m_controller.calculate(m_gyro.getRotation2d().getDegrees(), angleDeg)
145                        // Divide feedforward voltage by battery voltage to normalize it to [-1, 1]
146                        + m_feedforward
147                                .calculate(DegreesPerSecond.of(m_controller.getSetpoint().velocity))
148                                .in(Volts)
149                            / RobotController.getBatteryVoltage()))
150        .until(m_controller::atGoal)
151        .finallyDo(() -> m_drive.arcadeDrive(0, 0));
152  }
153}

 5#pragma once
 6
 7#include <functional>
 8
 9#include <frc/ADXRS450_Gyro.h>
10#include <frc/Encoder.h>
11#include <frc/controller/ProfiledPIDController.h>
12#include <frc/controller/SimpleMotorFeedforward.h>
13#include <frc/drive/DifferentialDrive.h>
14#include <frc/motorcontrol/PWMSparkMax.h>
15#include <frc2/command/CommandPtr.h>
16#include <frc2/command/SubsystemBase.h>
17#include <units/angle.h>
18#include <units/length.h>
19
20#include "Constants.h"
21
22class Drive : public frc2::SubsystemBase {
23 public:
24  Drive();
25  /**
26   * Returns a command that drives the robot with arcade controls.
27   *
28   * @param fwd the commanded forward movement
29   * @param rot the commanded rotation
30   */
31  [[nodiscard]]
32  frc2::CommandPtr ArcadeDriveCommand(std::function<double()> fwd,
33                                      std::function<double()> rot);
34
35  /**
36   * Returns a command that drives the robot forward a specified distance at a
37   * specified speed.
38   *
39   * @param distance The distance to drive forward in meters
40   * @param speed The fraction of max speed at which to drive
41   */
42  [[nodiscard]]
43  frc2::CommandPtr DriveDistanceCommand(units::meter_t distance, double speed);
44
45  /**
46   * Returns a command that turns to robot to the specified angle using a motion
47   * profile and PID controller.
48   *
49   * @param angle The angle to turn to
50   */
51  [[nodiscard]]
52  frc2::CommandPtr TurnToAngleCommand(units::degree_t angle);
53
54 private:
55  frc::PWMSparkMax m_leftLeader{DriveConstants::kLeftMotor1Port};
56  frc::PWMSparkMax m_leftFollower{DriveConstants::kLeftMotor2Port};
57  frc::PWMSparkMax m_rightLeader{DriveConstants::kRightMotor1Port};
58  frc::PWMSparkMax m_rightFollower{DriveConstants::kRightMotor2Port};
59
60  frc::DifferentialDrive m_drive{
61      [&](double output) { m_leftLeader.Set(output); },
62      [&](double output) { m_rightLeader.Set(output); }};
63
64  frc::Encoder m_leftEncoder{DriveConstants::kLeftEncoderPorts[0],
65                             DriveConstants::kLeftEncoderPorts[1],
66                             DriveConstants::kLeftEncoderReversed};
67  frc::Encoder m_rightEncoder{DriveConstants::kRightEncoderPorts[0],
68                              DriveConstants::kRightEncoderPorts[1],
69                              DriveConstants::kRightEncoderReversed};
70
71  frc::ADXRS450_Gyro m_gyro;
72
73  frc::ProfiledPIDController<units::radians> m_controller{
74      DriveConstants::kTurnP,
75      DriveConstants::kTurnI,
76      DriveConstants::kTurnD,
77      {DriveConstants::kMaxTurnRate, DriveConstants::kMaxTurnAcceleration}};
78  frc::SimpleMotorFeedforward<units::radians> m_feedforward{
79      DriveConstants::ks, DriveConstants::kv, DriveConstants::ka};
80};

 5#include "subsystems/Drive.h"
 6
 7#include <utility>
 8
 9#include <frc/RobotController.h>
10#include <frc2/command/Commands.h>
11
12Drive::Drive() {
13  wpi::SendableRegistry::AddChild(&m_drive, &m_leftLeader);
14  wpi::SendableRegistry::AddChild(&m_drive, &m_rightLeader);
15
16  m_leftLeader.AddFollower(m_leftFollower);
17  m_rightLeader.AddFollower(m_rightFollower);
18
19  // We need to invert one side of the drivetrain so that positive voltages
20  // result in both sides moving forward. Depending on how your robot's
21  // gearbox is constructed, you might have to invert the left side instead.
22  m_rightLeader.SetInverted(true);
23
24  // Sets the distance per pulse for the encoders
25  m_leftEncoder.SetDistancePerPulse(DriveConstants::kEncoderDistancePerPulse);
26  m_rightEncoder.SetDistancePerPulse(DriveConstants::kEncoderDistancePerPulse);
27
28  // Set the controller to be continuous (because it is an angle controller)
29  m_controller.EnableContinuousInput(-180_deg, 180_deg);
30  // Set the controller tolerance - the delta tolerance ensures the robot is
31  // stationary at the setpoint before it is considered as having reached the
32  // reference
33  m_controller.SetTolerance(DriveConstants::kTurnTolerance,
34                            DriveConstants::kTurnRateTolerance);
35}
36
37frc2::CommandPtr Drive::ArcadeDriveCommand(std::function<double()> fwd,
38                                           std::function<double()> rot) {
39  return Run([this, fwd = std::move(fwd), rot = std::move(rot)] {
40           m_drive.ArcadeDrive(fwd(), rot());
41         })
42      .WithName("ArcadeDrive");
43}
44
45frc2::CommandPtr Drive::DriveDistanceCommand(units::meter_t distance,
46                                             double speed) {
47  return RunOnce([this] {
48           // Reset encoders at the start of the command
49           m_leftEncoder.Reset();
50           m_rightEncoder.Reset();
51         })
52      // Drive forward at specified speed
53      .AndThen(Run([this, speed] { m_drive.ArcadeDrive(speed, 0.0); }))
54      .Until([this, distance] {
55        return units::math::max(units::meter_t(m_leftEncoder.GetDistance()),
56                                units::meter_t(m_rightEncoder.GetDistance())) >=
57               distance;
58      })
59      // Stop the drive when the command ends
60      .FinallyDo([this](bool interrupted) { m_drive.StopMotor(); });
61}
62
63frc2::CommandPtr Drive::TurnToAngleCommand(units::degree_t angle) {
64  return StartRun(
65             [this] { m_controller.Reset(m_gyro.GetRotation2d().Degrees()); },
66             [this, angle] {
67               m_drive.ArcadeDrive(
68                   0, m_controller.Calculate(m_gyro.GetRotation2d().Degrees(),
69                                             angle) +
70                          // Divide feedforward voltage by battery voltage to
71                          // normalize it to [-1, 1]
72                          m_feedforward.Calculate(
73                              m_controller.GetSetpoint().velocity) /
74                              frc::RobotController::GetBatteryVoltage());
75             })
76      .Until([this] { return m_controller.AtGoal(); })
77      .FinallyDo([this] { m_drive.ArcadeDrive(0, 0); });
78}

turnToAngleCommand uses a ProfiledPIDController to smoothly turn the drivetrain. The startRun command factory is used to reset the ProfiledPIDController when the command is scheduled to avoid unwanted behavior, and to calculate PID and feedforward outputs to pass into the arcadeDrive method in order to drive the robot. The command is decorated using the until decorator to end the command when the ProfiledPIDController is finished with the profile. To ensure the drivetrain stops when the command ends, the finallyDo decorator is used to stop the drivetrain by setting the speed to zero.