É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.

  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
package edu.wpi.first.wpilibj.examples.ramsetecommand.subsystems;

import edu.wpi.first.wpilibj.ADXRS450_Gyro;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.PWMVictorSPX;
import edu.wpi.first.wpilibj.SpeedControllerGroup;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.interfaces.Gyro;
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry;
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
import edu.wpi.first.wpilibj2.command.SubsystemBase;

import edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants;

public class DriveSubsystem extends SubsystemBase {
  // The motors on the left side of the drive.
  private final SpeedControllerGroup m_leftMotors =
      new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kLeftMotor1Port),
                               new PWMVictorSPX(DriveConstants.kLeftMotor2Port));

  // The motors on the right side of the drive.
  private final SpeedControllerGroup m_rightMotors =
      new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kRightMotor1Port),
                               new PWMVictorSPX(DriveConstants.kRightMotor2Port));

  // The robot's drive
  private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);

  // The left-side drive encoder
  private final Encoder m_leftEncoder =
      new Encoder(DriveConstants.kLeftEncoderPorts[0], DriveConstants.kLeftEncoderPorts[1],
                  DriveConstants.kLeftEncoderReversed);

  // The right-side drive encoder
  private final Encoder m_rightEncoder =
      new Encoder(DriveConstants.kRightEncoderPorts[0], DriveConstants.kRightEncoderPorts[1],
                  DriveConstants.kRightEncoderReversed);

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

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

  /**
   * Creates a new DriveSubsystem.
   */
  public DriveSubsystem() {
    // Sets the distance per pulse for the encoders
    m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
    m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);

    resetEncoders();
    m_odometry = new DifferentialDriveOdometry(m_gyro.getRotation2d());
  }

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

  /**
   * Returns the currently-estimated pose of the robot.
   *
   * @return The pose.
   */
  public Pose2d getPose() {
    return m_odometry.getPoseMeters();
  }

  /**
   * Returns the current wheel speeds of the robot.
   *
   * @return The current wheel speeds.
   */
  public DifferentialDriveWheelSpeeds getWheelSpeeds() {
    return new DifferentialDriveWheelSpeeds(m_leftEncoder.getRate(), m_rightEncoder.getRate());
  }

  /**
   * Resets the odometry to the specified pose.
   *
   * @param pose The pose to which to set the odometry.
   */
  public void resetOdometry(Pose2d pose) {
    resetEncoders();
    m_odometry.resetPosition(pose, m_gyro.getRotation2d());
  }

  /**
   * Drives the robot using arcade controls.
   *
   * @param fwd the commanded forward movement
   * @param rot the commanded rotation
   */
  public void arcadeDrive(double fwd, double rot) {
    m_drive.arcadeDrive(fwd, rot);
  }

  /**
   * Controls the left and right sides of the drive directly with voltages.
   *
   * @param leftVolts  the commanded left output
   * @param rightVolts the commanded right output
   */
  public void tankDriveVolts(double leftVolts, double rightVolts) {
    m_leftMotors.setVoltage(leftVolts);
    m_rightMotors.setVoltage(-rightVolts);
    m_drive.feed();
  }

  /**
   * Resets the drive encoders to currently read a position of 0.
   */
  public void resetEncoders() {
    m_leftEncoder.reset();
    m_rightEncoder.reset();
  }

  /**
   * Gets the average distance of the two encoders.
   *
   * @return the average of the two encoder readings
   */
  public double getAverageEncoderDistance() {
    return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.0;
  }

  /**
   * Gets the left drive encoder.
   *
   * @return the left drive encoder
   */
  public Encoder getLeftEncoder() {
    return m_leftEncoder;
  }

  /**
   * Gets the right drive encoder.
   *
   * @return the right drive encoder
   */
  public Encoder getRightEncoder() {
    return m_rightEncoder;
  }

  /**
   * Sets the max output of the drive.  Useful for scaling the drive to drive more slowly.
   *
   * @param maxOutput the maximum output to which the drive will be constrained
   */
  public void setMaxOutput(double maxOutput) {
    m_drive.setMaxOutput(maxOutput);
  }

  /**
   * Zeroes the heading of the robot.
   */
  public void zeroHeading() {
    m_gyro.reset();
  }

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

  /**
   * Returns the turn rate of the robot.
   *
   * @return The turn rate of the robot, in degrees per second
   */
  public double getTurnRate() {
    return -m_gyro.getRate();
  }
}

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 :

38
39
40
41
42
43
44
45
  private final Encoder m_leftEncoder =
      new Encoder(DriveConstants.kLeftEncoderPorts[0], DriveConstants.kLeftEncoderPorts[1],
                  DriveConstants.kLeftEncoderReversed);

  // The right-side drive encoder
  private final Encoder m_rightEncoder =
      new Encoder(DriveConstants.kRightEncoderPorts[0], DriveConstants.kRightEncoderPorts[1],
                  DriveConstants.kRightEncoderReversed);

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, « pulse » fait référence à un cycle d’encodeur complet (c.-à-d. quatre fronts), et sera donc 1/4 la valeur spécifiée dans la configuration FRC-Caractérisation. Rappelez-vous, aussi, que la distance doit être mesurée en mètres!

58
59
    m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
    m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);

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.

82
83
84
85
86
87
88
89
  /**
   * Returns the current wheel speeds of the robot.
   *
   * @return The current wheel speeds.
   */
  public DifferentialDriveWheelSpeeds getWheelSpeeds() {
    return new DifferentialDriveWheelSpeeds(m_leftEncoder.getRate(), m_rightEncoder.getRate());
  }

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

Le gyroscope mesure le taux de changement de position angulaire du robot (qui peut ensuite être intégré pour fournir une mesure de l’angle du robot par rapport au moment où il s’est allumé pour la première fois). Dans notre exemple, nous utilisons le Gyro Analog Devices ADXRS450 de board FRC, qui est inclus dans le kit de pièces depuis plusieurs années :

48
  private final Gyro m_gyro = new ADXRS450_Gyro();

Méthode d’obtention du gyroscope

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

173
174
175
176
177
178
179
180
  /**
   * Returns the heading of the robot.
   *
   * @return the robot's heading in degrees, from -180 to 180
   */
  public double getHeading() {
    return m_gyro.getRotation2d().getDegrees();
  }

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:

51
  private final DifferentialDriveOdometry m_odometry;

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 :

66
67
68
69
70
  public void periodic() {
    // Update the odometry in the periodic block
    m_odometry.update(m_gyro.getRotation2d(), m_leftEncoder.getDistance(),
                      m_rightEncoder.getDistance());
  }

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

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

72
73
74
75
76
77
78
79
  /**
   * Returns the currently-estimated pose of the robot.
   *
   * @return The pose.
   */
  public Pose2d getPose() {
    return m_odometry.getPoseMeters();
  }

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 soit la méthode setVoltage() de l’interface SpeedController. La classe par défaut de la WPILib dédiée au déplacement n’inclut pas cette fonctionnalité, nous devons donc l’écrire nous-mêmes :

110
111
112
113
114
115
116
117
118
119
120
  /**
   * Controls the left and right sides of the drive directly with voltages.
   *
   * @param leftVolts  the commanded left output
   * @param rightVolts the commanded right output
   */
  public void tankDriveVolts(double leftVolts, double rightVolts) {
    m_leftMotors.setVoltage(leftVolts);
    m_rightMotors.setVoltage(-rightVolts);
    m_drive.feed();
  }

Il est très important d’utiliser la méthode setVoltage() plutôt que la méthode ordinaire set(), car cela compensera automatiquement un éventuel « creux de tension » de la batterie pendant le suivi de la trajectoire. Étant donné que nos tensions de commande prédictive ont une signification physique (car elles sont basées sur des données de caractérisation mesurées), cela est essentiel pour garantir leur précision.