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

JavaC++ (Header)C++ (Source)

package edu.wpi.first.wpilibj.examples.ramsetecommand.subsystems;

import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
import edu.wpi.first.wpilibj.ADXRS450_Gyro;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants;
import edu.wpi.first.wpilibj.interfaces.Gyro;
import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
import edu.wpi.first.wpilibj2.command.SubsystemBase;

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

  // The motors on the right side of the drive.
  private final MotorControllerGroup m_rightMotors =
      new MotorControllerGroup(
          new PWMSparkMax(DriveConstants.kRightMotor1Port),
          new PWMSparkMax(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() {
    // We need to invert one side of the drivetrain so that positive voltages
    // result in both sides moving forward. Depending on how your robot's
    // gearbox is constructed, you might have to invert the left side instead.
    m_rightMotors.setInverted(true);

    // 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(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
  }

  @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(
        m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance(), pose);
  }

  /**
   * 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 :

JavaC++ (Source)

  // 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);

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 !

JavaC++ (Source)

    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.

JavaC++ (Source)

  /**
   * 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 :

JavaC++ (Header)

  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 :

JavaC++ (Source)

  /**
   * 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:

JavaC++ (Header)

  // Odometry class for tracking robot pose
  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 :

JavaC++ (Source)

  @Override
  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 :

JavaC++ (Source)

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

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:

JavaC++ (Source)

  /**
   * 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 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.