Paso 3: creación de un subsistema de desplazamiento o drive

Ahora que nuestra unidad está caracterizada, es hora de comenzar a escribir nuestro código de robot adecuadamente. Como se mencionó anteriormente, utilizaremos el Framework command-based para nuestro código de robot. En consecuencia, nuestro primer paso es escribir una clase adecuada subsistema .

La clase de drive completa del RamseteCommand Example Project (Java, C++) se puede ver a continuación. El resto del artículo describirá los pasos necesarios para escribir esta clase.

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();
  }
}

Configuración de los codificadores de unidad

Los codificadores de transmisión miden la rotación de las ruedas en cada lado de la transmisión. Para configurar correctamente los codificadores, necesitamos especificar dos cosas: los puertos a los que están conectados los codificadores y la distancia por pulso del codificador. Luego, necesitamos escribir métodos que permitan el acceso a los valores del codificador desde el código que usa el subsistema.

Puertos del codificador

Los puertos del codificador se especifican en el constructor del codificador, así:

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

Distancia del codificador por pulso

The distance per pulse is specified by calling the encoder’s setDistancePerPulse method. Note that for the WPILib Encoder class, «pulse» refers to a full encoder cycle (i.e. four edges), and thus will be 1/4 the value that was specified in the SysId config. Remember, as well, that the distance should be measured in meters!

JavaC++ (Source)

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

Método de acceso del codificador

Para acceder a los valores medidos por los codificadores, incluimos el siguiente método:

Importante

¡Las velocidades devueltas deben estar en metros! Dado que hemos configurado la distancia por pulso en los codificadores anteriormente, al llamar a getRate() se aplicará automáticamente el factor de conversión de las unidades del codificador a metros. Si no estás usando la clase Encoder de WPILib, debes realizar esta conversión a través de la API del proveedor respectivo o multiplicando manualmente por un factor de conversión.

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());
  }

Envolvemos los valores medidos del codificador en un objeto DifferentialDriveWheelSpeeds para facilitar la integración con la clase RamseteCommand más adelante.

Configurar el giroscopio

El giroscopio mide la tasa de cambio del rumbo del robot (que luego se puede integrar para proporcionar una medida del rumbo del robot en relación con la primera vez que se encendió). En nuestro ejemplo, utilizamos el Analog Devices ADXRS450 FRC Gyro Board, que se ha incluido en el kit de piezas durante varios años:

JavaC++ (Header)

  private final Gyro m_gyro = new ADXRS450_Gyro();

Método de acceso al giroscopio

Para acceder al rumbo actual medido por el giroscopio, incluimos el siguiente método:

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();
  }

Configuración de la odometría

Ahora que tenemos nuestros codificadores y giroscopio configurados, es hora de configurar nuestro subsistema de transmisión para calcular automáticamente su posición a partir de las lecturas del codificador y del giroscopio.

Primero, creamos una instancia de miembro de la clase DifferentialDriveOdometry:

JavaC++ (Header)

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

Actualización de la odometría

La clase de odometría debe actualizarse periódicamente para incorporar nuevas lecturas del codificador y el giroscopio. Logramos esto dentro del método periódico del subsistema, que se llama automáticamente una vez por iteración del ciclo principal:

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étodo de acceso de odometría

Para acceder a la pose calculada actual del robot, incluimos el siguiente método:

JavaC++ (Source)

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

Método de accionamiento basado en voltaje

Finally, we must include one additional method - a method that allows us to set the voltage to each side of the drive using the setVoltage() method of the MotorController interface. The default WPILib drive class does not include this functionality, so we must write it ourselves:

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();
  }

It is very important to use the setVoltage() method rather than the ordinary set() method, as this will automatically compensate for battery «voltage sag» during operation. Since our feedforward voltages are physically-meaningful (as they are based on measured identification data), this is essential to ensuring their accuracy.