Étape 3 : Création d’un sous-système de déplacement

Maintenant que la base pilotable est caractérisée, c’est le moment de commencer à écrire notre code robot proprement dit. Comme mentionné précédemment, nous utiliserons le cadre de développement command-based, pour ce faire. Aussi, notre première étape consiste à créer une classe sous-système de déplacement appropriée subsystem.

La classe de déplacement complète du projet d’exemple RamseteCommand (Java, C++) peut être vu ci-dessous. Le reste de l’article décrira les étapes nécessaires dans l’écriture de cette classe.

  5package edu.wpi.first.wpilibj.examples.ramsetecommand.subsystems;
  6
  7import edu.wpi.first.math.geometry.Pose2d;
  8import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
  9import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
 10import edu.wpi.first.util.sendable.SendableRegistry;
 11import edu.wpi.first.wpilibj.ADXRS450_Gyro;
 12import edu.wpi.first.wpilibj.Encoder;
 13import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 14import edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants;
 15import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 16import edu.wpi.first.wpilibj2.command.SubsystemBase;
 17
 18public class DriveSubsystem extends SubsystemBase {
 19  // The motors on the left side of the drive.
 20  private final PWMSparkMax m_leftLeader = new PWMSparkMax(DriveConstants.kLeftMotor1Port);
 21  private final PWMSparkMax m_leftFollower = new PWMSparkMax(DriveConstants.kLeftMotor2Port);
 22
 23  // The motors on the right side of the drive.
 24  private final PWMSparkMax m_rightLeader = new PWMSparkMax(DriveConstants.kRightMotor1Port);
 25  private final PWMSparkMax m_rightFollower = new PWMSparkMax(DriveConstants.kRightMotor2Port);
 26
 27  // The robot's drive
 28  private final DifferentialDrive m_drive =
 29      new DifferentialDrive(m_leftLeader::set, m_rightLeader::set);
 30
 31  // The left-side drive encoder
 32  private final Encoder m_leftEncoder =
 33      new Encoder(
 34          DriveConstants.kLeftEncoderPorts[0],
 35          DriveConstants.kLeftEncoderPorts[1],
 36          DriveConstants.kLeftEncoderReversed);
 37
 38  // The right-side drive encoder
 39  private final Encoder m_rightEncoder =
 40      new Encoder(
 41          DriveConstants.kRightEncoderPorts[0],
 42          DriveConstants.kRightEncoderPorts[1],
 43          DriveConstants.kRightEncoderReversed);
 44
 45  // The gyro sensor
 46  private final ADXRS450_Gyro m_gyro = new ADXRS450_Gyro();
 47
 48  // Odometry class for tracking robot pose
 49  private final DifferentialDriveOdometry m_odometry;
 50
 51  /** Creates a new DriveSubsystem. */
 52  public DriveSubsystem() {
 53    SendableRegistry.addChild(m_drive, m_leftLeader);
 54    SendableRegistry.addChild(m_drive, m_rightLeader);
 55
 56    m_leftLeader.addFollower(m_leftFollower);
 57    m_rightLeader.addFollower(m_rightFollower);
 58
 59    // We need to invert one side of the drivetrain so that positive voltages
 60    // result in both sides moving forward. Depending on how your robot's
 61    // gearbox is constructed, you might have to invert the left side instead.
 62    m_rightLeader.setInverted(true);
 63
 64    // Sets the distance per pulse for the encoders
 65    m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
 66    m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
 67
 68    resetEncoders();
 69    m_odometry =
 70        new DifferentialDriveOdometry(
 71            m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
 72  }
 73
 74  @Override
 75  public void periodic() {
 76    // Update the odometry in the periodic block
 77    m_odometry.update(
 78        m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
 79  }
 80
 81  /**
 82   * Returns the currently-estimated pose of the robot.
 83   *
 84   * @return The pose.
 85   */
 86  public Pose2d getPose() {
 87    return m_odometry.getPoseMeters();
 88  }
 89
 90  /**
 91   * Returns the current wheel speeds of the robot.
 92   *
 93   * @return The current wheel speeds.
 94   */
 95  public DifferentialDriveWheelSpeeds getWheelSpeeds() {
 96    return new DifferentialDriveWheelSpeeds(m_leftEncoder.getRate(), m_rightEncoder.getRate());
 97  }
 98
 99  /**
100   * Resets the odometry to the specified pose.
101   *
102   * @param pose The pose to which to set the odometry.
103   */
104  public void resetOdometry(Pose2d pose) {
105    m_odometry.resetPosition(
106        m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance(), pose);
107  }
108
109  /**
110   * Drives the robot using arcade controls.
111   *
112   * @param fwd the commanded forward movement
113   * @param rot the commanded rotation
114   */
115  public void arcadeDrive(double fwd, double rot) {
116    m_drive.arcadeDrive(fwd, rot);
117  }
118
119  /**
120   * Controls the left and right sides of the drive directly with voltages.
121   *
122   * @param leftVolts the commanded left output
123   * @param rightVolts the commanded right output
124   */
125  public void tankDriveVolts(double leftVolts, double rightVolts) {
126    m_leftLeader.setVoltage(leftVolts);
127    m_rightLeader.setVoltage(rightVolts);
128    m_drive.feed();
129  }
130
131  /** Resets the drive encoders to currently read a position of 0. */
132  public void resetEncoders() {
133    m_leftEncoder.reset();
134    m_rightEncoder.reset();
135  }
136
137  /**
138   * Gets the average distance of the two encoders.
139   *
140   * @return the average of the two encoder readings
141   */
142  public double getAverageEncoderDistance() {
143    return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.0;
144  }
145
146  /**
147   * Gets the left drive encoder.
148   *
149   * @return the left drive encoder
150   */
151  public Encoder getLeftEncoder() {
152    return m_leftEncoder;
153  }
154
155  /**
156   * Gets the right drive encoder.
157   *
158   * @return the right drive encoder
159   */
160  public Encoder getRightEncoder() {
161    return m_rightEncoder;
162  }
163
164  /**
165   * Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
166   *
167   * @param maxOutput the maximum output to which the drive will be constrained
168   */
169  public void setMaxOutput(double maxOutput) {
170    m_drive.setMaxOutput(maxOutput);
171  }
172
173  /** Zeroes the heading of the robot. */
174  public void zeroHeading() {
175    m_gyro.reset();
176  }
177
178  /**
179   * Returns the heading of the robot.
180   *
181   * @return the robot's heading in degrees, from -180 to 180
182   */
183  public double getHeading() {
184    return m_gyro.getRotation2d().getDegrees();
185  }
186
187  /**
188   * Returns the turn rate of the robot.
189   *
190   * @return The turn rate of the robot, in degrees per second
191   */
192  public double getTurnRate() {
193    return -m_gyro.getRate();
194  }
195}

  5#pragma once
  6
  7#include <frc/ADXRS450_Gyro.h>
  8#include <frc/Encoder.h>
  9#include <frc/drive/DifferentialDrive.h>
 10#include <frc/geometry/Pose2d.h>
 11#include <frc/kinematics/DifferentialDriveOdometry.h>
 12#include <frc/motorcontrol/PWMSparkMax.h>
 13#include <frc2/command/SubsystemBase.h>
 14#include <units/voltage.h>
 15
 16#include "Constants.h"
 17
 18class DriveSubsystem : public frc2::SubsystemBase {
 19 public:
 20  DriveSubsystem();
 21
 22  /**
 23   * Will be called periodically whenever the CommandScheduler runs.
 24   */
 25  void Periodic() override;
 26
 27  // Subsystem methods go here.
 28
 29  /**
 30   * Drives the robot using arcade controls.
 31   *
 32   * @param fwd the commanded forward movement
 33   * @param rot the commanded rotation
 34   */
 35  void ArcadeDrive(double fwd, double rot);
 36
 37  /**
 38   * Controls each side of the drive directly with a voltage.
 39   *
 40   * @param left the commanded left output
 41   * @param right the commanded right output
 42   */
 43  void TankDriveVolts(units::volt_t left, units::volt_t right);
 44
 45  /**
 46   * Resets the drive encoders to currently read a position of 0.
 47   */
 48  void ResetEncoders();
 49
 50  /**
 51   * Gets the average distance of the TWO encoders.
 52   *
 53   * @return the average of the TWO encoder readings
 54   */
 55  double GetAverageEncoderDistance();
 56
 57  /**
 58   * Gets the left drive encoder.
 59   *
 60   * @return the left drive encoder
 61   */
 62  frc::Encoder& GetLeftEncoder();
 63
 64  /**
 65   * Gets the right drive encoder.
 66   *
 67   * @return the right drive encoder
 68   */
 69  frc::Encoder& GetRightEncoder();
 70
 71  /**
 72   * Sets the max output of the drive.  Useful for scaling the drive to drive
 73   * more slowly.
 74   *
 75   * @param maxOutput the maximum output to which the drive will be constrained
 76   */
 77  void SetMaxOutput(double maxOutput);
 78
 79  /**
 80   * Returns the heading of the robot.
 81   *
 82   * @return the robot's heading in degrees, from -180 to 180
 83   */
 84  units::degree_t GetHeading() const;
 85
 86  /**
 87   * Returns the turn rate of the robot.
 88   *
 89   * @return The turn rate of the robot, in degrees per second
 90   */
 91  double GetTurnRate();
 92
 93  /**
 94   * Returns the currently-estimated pose of the robot.
 95   *
 96   * @return The pose.
 97   */
 98  frc::Pose2d GetPose();
 99
100  /**
101   * Returns the current wheel speeds of the robot.
102   *
103   * @return The current wheel speeds.
104   */
105  frc::DifferentialDriveWheelSpeeds GetWheelSpeeds();
106
107  /**
108   * Resets the odometry to the specified pose.
109   *
110   * @param pose The pose to which to set the odometry.
111   */
112  void ResetOdometry(frc::Pose2d pose);
113
114 private:
115  // Components (e.g. motor controllers and sensors) should generally be
116  // declared private and exposed only through public methods.
117
118  // The motor controllers
119  frc::PWMSparkMax m_left1;
120  frc::PWMSparkMax m_left2;
121  frc::PWMSparkMax m_right1;
122  frc::PWMSparkMax m_right2;
123
124  // The robot's drive
125  frc::DifferentialDrive m_drive{[&](double output) { m_left1.Set(output); },
126                                 [&](double output) { m_right1.Set(output); }};
127
128  // The left-side drive encoder
129  frc::Encoder m_leftEncoder;
130
131  // The right-side drive encoder
132  frc::Encoder m_rightEncoder;
133
134  // The gyro sensor
135  frc::ADXRS450_Gyro m_gyro;
136
137  // Odometry class for tracking robot pose
138  frc::DifferentialDriveOdometry m_odometry;
139};

 5#include "subsystems/DriveSubsystem.h"
 6
 7#include <frc/geometry/Rotation2d.h>
 8#include <frc/kinematics/DifferentialDriveWheelSpeeds.h>
 9
10using namespace DriveConstants;
11
12DriveSubsystem::DriveSubsystem()
13    : m_left1{kLeftMotor1Port},
14      m_left2{kLeftMotor2Port},
15      m_right1{kRightMotor1Port},
16      m_right2{kRightMotor2Port},
17      m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]},
18      m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]},
19      m_odometry{m_gyro.GetRotation2d(), units::meter_t{0}, units::meter_t{0}} {
20  wpi::SendableRegistry::AddChild(&m_drive, &m_left1);
21  wpi::SendableRegistry::AddChild(&m_drive, &m_right1);
22
23  m_left1.AddFollower(m_left2);
24  m_right1.AddFollower(m_right2);
25
26  // We need to invert one side of the drivetrain so that positive voltages
27  // result in both sides moving forward. Depending on how your robot's
28  // gearbox is constructed, you might have to invert the left side instead.
29  m_right1.SetInverted(true);
30
31  // Set the distance per pulse for the encoders
32  m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse.value());
33  m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse.value());
34
35  ResetEncoders();
36}
37
38void DriveSubsystem::Periodic() {
39  // Implementation of subsystem periodic method goes here.
40  m_odometry.Update(m_gyro.GetRotation2d(),
41                    units::meter_t{m_leftEncoder.GetDistance()},
42                    units::meter_t{m_rightEncoder.GetDistance()});
43}
44
45void DriveSubsystem::ArcadeDrive(double fwd, double rot) {
46  m_drive.ArcadeDrive(fwd, rot);
47}
48
49void DriveSubsystem::TankDriveVolts(units::volt_t left, units::volt_t right) {
50  m_left1.SetVoltage(left);
51  m_right1.SetVoltage(right);
52  m_drive.Feed();
53}
54
55void DriveSubsystem::ResetEncoders() {
56  m_leftEncoder.Reset();
57  m_rightEncoder.Reset();
58}
59
60double DriveSubsystem::GetAverageEncoderDistance() {
61  return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.0;
62}
63
64frc::Encoder& DriveSubsystem::GetLeftEncoder() {
65  return m_leftEncoder;
66}
67
68frc::Encoder& DriveSubsystem::GetRightEncoder() {
69  return m_rightEncoder;
70}
71
72void DriveSubsystem::SetMaxOutput(double maxOutput) {
73  m_drive.SetMaxOutput(maxOutput);
74}
75
76units::degree_t DriveSubsystem::GetHeading() const {
77  return m_gyro.GetRotation2d().Degrees();
78}
79
80double DriveSubsystem::GetTurnRate() {
81  return -m_gyro.GetRate();
82}
83
84frc::Pose2d DriveSubsystem::GetPose() {
85  return m_odometry.GetPose();
86}
87
88frc::DifferentialDriveWheelSpeeds DriveSubsystem::GetWheelSpeeds() {
89  return {units::meters_per_second_t{m_leftEncoder.GetRate()},
90          units::meters_per_second_t{m_rightEncoder.GetRate()}};
91}
92
93void DriveSubsystem::ResetOdometry(frc::Pose2d pose) {
94  m_odometry.ResetPosition(m_gyro.GetRotation2d(),
95                           units::meter_t{m_leftEncoder.GetDistance()},
96                           units::meter_t{m_rightEncoder.GetDistance()}, pose);
97}

Configuration des encodeurs du sous-système de déplacement

Les encodeurs du sous-système de déplacement mesurent la rotation des roues de chaque côté de la base pilotable. Pour configurer correctement ces encodeurs, nous devons spécifier deux choses : les ports dans lesquels les encodeurs sont branchés et la distance par impulsion d’encodeur. Ensuite, nous devons écrire des méthodes permettant d’obtention de valeurs d’encodeur à partir du code qui utilise le sous-système.

Ports des encodeurs

Les ports d’encodeur sont spécifiés dans le constructeur de l’encodeur de la manière suivante :

31  // The left-side drive encoder
32  private final Encoder m_leftEncoder =
33      new Encoder(
34          DriveConstants.kLeftEncoderPorts[0],
35          DriveConstants.kLeftEncoderPorts[1],
36          DriveConstants.kLeftEncoderReversed);
37
38  // The right-side drive encoder
39  private final Encoder m_rightEncoder =
40      new Encoder(
41          DriveConstants.kRightEncoderPorts[0],
42          DriveConstants.kRightEncoderPorts[1],
43          DriveConstants.kRightEncoderReversed);

17      m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]},
18      m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]},

Distance par impulsion d’encodeur

La distance par impulsion est spécifiée en appelant la méthode setDistancePerPulse de l’encodeur. Notez que pour la classe WPILib Encoder, « impulsion » fait référence à un cycle d’encodeur complet (c’est-à-dire quatre fronts), et sera donc 1/4 de la valeur qui a été spécifiée dans la configuration SysId. N’oubliez pas non plus que la distance doit être mesurée en mètres !

65    m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
66    m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);

32  m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse.value());
33  m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse.value());

Méthode d’obtention d’encodeur

Pour accéder aux valeurs mesurées par les encodeurs, nous incluons la méthode suivante :

Important

Les vitesses renvoyées ** doivent ** être en mètres! Parce que nous avons configuré la distance par impulsion sur les encodeurs ci-dessus, l’appel de getRate() appliquera automatiquement le facteur de conversion des unités d’encodeur aux mètres. Si vous n’utilisez pas la classe Encoder de WPILib, vous devez effectuer cette conversion soit via l’API du fournisseur respectif, soit en multipliant manuellement par un facteur de conversion.

90  /**
91   * Returns the current wheel speeds of the robot.
92   *
93   * @return The current wheel speeds.
94   */
95  public DifferentialDriveWheelSpeeds getWheelSpeeds() {
96    return new DifferentialDriveWheelSpeeds(m_leftEncoder.getRate(), m_rightEncoder.getRate());
97  }

88frc::DifferentialDriveWheelSpeeds DriveSubsystem::GetWheelSpeeds() {
89  return {units::meters_per_second_t{m_leftEncoder.GetRate()},
90          units::meters_per_second_t{m_rightEncoder.GetRate()}};
91}

Nous enveloppons les valeurs d’encodeur mesurées dans un objet DifferentialDriveWheelSpeeds pour faciliter l’intégration, plus tard, avec la classe RamseteCommand.

Configuration du Gyroscope

The gyroscope measures the rate of change of the robot’s heading (which can then be integrated to provide a measurement of the robot’s heading relative to when it first turned on). In our example, we use the Analog Devices ADXRS450 FRC Gyro Board, which was included in the kit of parts for several years:

45  // The gyro sensor
46  private final ADXRS450_Gyro m_gyro = new ADXRS450_Gyro();

134  // The gyro sensor
135  frc::ADXRS450_Gyro m_gyro;

Méthode d’obtention du gyroscope

Pour accéder à l’orientation actuelle mesurée par le gyroscope, nous incluons la méthode suivante :

178  /**
179   * Returns the heading of the robot.
180   *
181   * @return the robot's heading in degrees, from -180 to 180
182   */
183  public double getHeading() {
184    return m_gyro.getRotation2d().getDegrees();
185  }

76units::degree_t DriveSubsystem::GetHeading() const {
77  return m_gyro.GetRotation2d().Degrees();
78}

Configuration de l’odométrie

Maintenant que nous avons nos encodeurs et gyroscope configurés, nous pouvons mettre en place notre sous-système de déplacement pour calculer automatiquement sa position à partir des lectures obtenues des encodeurs et de gyroscope.

Tout d’abord, nous créons une instance membre de la classe DifferentialDriveOdometry:

48  // Odometry class for tracking robot pose
49  private final DifferentialDriveOdometry m_odometry;

137  // Odometry class for tracking robot pose
138  frc::DifferentialDriveOdometry m_odometry;

Then we initialize the DifferentialDriveOdometry.

69    m_odometry =
70        new DifferentialDriveOdometry(
71            m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());

19      m_odometry{m_gyro.GetRotation2d(), units::meter_t{0}, units::meter_t{0}} {

Mise à jour de l’odométrie

La classe d’odométrie doit être régulièrement mise à jour pour incorporer de nouvelles lectures de l’encodeur et du gyroscope. Nous accomplissons cela à l’intérieur de la méthode periodic du sous-système, qui est automatiquement appelée une fois à chaque itération de boucle principale :

74  @Override
75  public void periodic() {
76    // Update the odometry in the periodic block
77    m_odometry.update(
78        m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
79  }

38void DriveSubsystem::Periodic() {
39  // Implementation of subsystem periodic method goes here.
40  m_odometry.Update(m_gyro.GetRotation2d(),
41                    units::meter_t{m_leftEncoder.GetDistance()},
42                    units::meter_t{m_rightEncoder.GetDistance()});
43}

Méthode d’obtention de l’odométrie

Pour accéder à la pose actuelle calculée du robot, nous incluons la méthode suivante :

81  /**
82   * Returns the currently-estimated pose of the robot.
83   *
84   * @return The pose.
85   */
86  public Pose2d getPose() {
87    return m_odometry.getPoseMeters();
88  }

84frc::Pose2d DriveSubsystem::GetPose() {
85  return m_odometry.GetPose();
86}

Important

Before running a RamseteCommand, teams are strongly encouraged to deploy and test the odometry code alone, with values sent to the SmartDashboard or Shuffleboard during the DriveSubsystem’s periodic(). This odometry must be correct for a RamseteCommand to successfully work, as sign or unit errors can cause a robot to move at high speeds in unpredictable directions.

Méthode du sous-système de déplacement en charge de la tension

Enfin, nous devons inclure une méthode supplémentaire - une méthode qui nous permet de régler la tension de chaque côté de la base pilotable en utilisant la méthode setVoltage() de l’interface MotorController. La classe du système d’entraînement WPILib par défaut n’inclut pas cette fonctionnalité, nous devons donc l’écrire nous-mêmes:

119  /**
120   * Controls the left and right sides of the drive directly with voltages.
121   *
122   * @param leftVolts the commanded left output
123   * @param rightVolts the commanded right output
124   */
125  public void tankDriveVolts(double leftVolts, double rightVolts) {
126    m_leftLeader.setVoltage(leftVolts);
127    m_rightLeader.setVoltage(rightVolts);
128    m_drive.feed();
129  }

49void DriveSubsystem::TankDriveVolts(units::volt_t left, units::volt_t right) {
50  m_left1.SetVoltage(left);
51  m_right1.SetVoltage(right);
52  m_drive.Feed();
53}

Il est très important d’utiliser la méthode setVoltage() plutôt que la méthode ordinaire set(), car cela compensera automatiquement la « baisse de tension » de la batterie pendant le fonctionnement. Étant donné que nos tensions d’anticipation ont une signification physique (car elles sont basées sur des données d’identification mesurées), cela est essentiel pour garantir leur exactitude.

Avertissement

RamseteCommand itself does not internally enforce any speed or acceleration limits before providing motor voltage parameters to this method. During initial code development, teams are strongly encouraged to apply both maximum and minimum bounds on the input variables before passing these values to setVoltage() while ensuring the trajectory velocity and acceleration are achievable. For example, generate a trajectory with a little less than half of the Robot’s maximum velocity and limit voltage to 6 volts.