Odometría Mecanum Drive

Un usuario puede utilizar las clases de cinemática de la unidad mecanum para realizar odometría. WPILib contiene una clase MecanumDriveOdometry que se puede utilizar para rastrear la posición de un robot de accionamiento mecanum en el campo.

Nota

Debido a que este método solo usa codificadores y un giróscopo, la estimación de la posición del robot en el campo variará con el tiempo, especialmente cuando tu robot entre en contacto con otros robots durante el juego. Sin embargo, la odometría suele ser muy precisa durante el período autónomo.

Creando el objeto de odometría

La clase MecanumDriveOdometry requiere dos argumentos obligatorios y un argumento opcional. Los argumentos obligatorios son el objeto cinemático que representa su chasis mecanum (en forma de una clase MecanumDriveKinematics) y el ángulo informado por su giroscopio (como un Rotation2d). El tercer argumento opcional es la pose inicial de su robot en el campo (como una Pose2d). De forma predeterminada, el robot comenzará en x = 0, y = 0, theta = 0.

Nota

0 grados / radianes representa el ángulo del robot cuando el robot está mirando directamente hacia la estación de alianza de tu oponente. A medida que su robot gira hacia la izquierda, el ángulo de su giroscopio debería aumentar. Por defecto, los giroscopios WPILib exhiben el comportamiento opuesto, por lo que debe negar el ángulo del giroscopio.

// Locations of the wheels relative to the robot center.
Translation2d m_frontLeftLocation = new Translation2d(0.381, 0.381);
Translation2d m_frontRightLocation = new Translation2d(0.381, -0.381);
Translation2d m_backLeftLocation = new Translation2d(-0.381, 0.381);
Translation2d m_backRightLocation = new Translation2d(-0.381, -0.381);

// Creating my kinematics object using the wheel locations.
MecanumDriveKinematics m_kinematics = new MecanumDriveKinematics(
  m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation
);

// Creating my odometry object from the kinematics object. Here,
// our starting pose is 5 meters along the long end of the field and in the
// center of the field along the short end, facing forward.
MecanumDriveOdometry m_odometry = new MecanumDriveOdometry(m_kinematics,
  getGyroHeading(), new Pose2d(5.0, 13.5, new Rotation2d()));

Actualizar la pose del robot

El método update de la clase de odometría actualiza la posición del robot en el campo. El método de actualización toma el ángulo de giro del robot, junto con un objeto MecanumDriveWheelSpeeds que representa la velocidad de cada una de las 4 ruedas del robot. Este método de update debe ser llamado periódicamente, preferiblemente en el método periodic() de a Subsystem. El método de update devuelve la nueva pose actualizada del robot.

Nota

La clase MecanumDriveWheelSpeeds en Java debe construirse con la velocidad de cada rueda en metros por segundo. En C++, la biblioteca de unidades debe usarse para representar las velocidades de las ruedas.

@Override
public void periodic() {
  // Get my wheel speeds
  var wheelSpeeds = new MecanumDriveWheelSpeeds(
      m_frontLeftEncoder.getRate(), m_frontRightEncoder.getRate(),
      m_backLeftEncoder.getRate(), m_backRightEncoder.getRate());

  // Get my gyro angle. We are negating the value because gyros return positive
  // values as the robot turns clockwise. This is not standard convention that is
  // used by the WPILib classes.
  var gyroAngle = Rotation2d.fromDegrees(-m_gyro.getAngle());

  // Update the pose
  m_pose = m_odometry.update(gyroAngle, wheelSpeeds);
}

Restablecer la postura del robot

La pose del robot se puede restablecer mediante el método resetPose. Este método acepta dos argumentos: la nueva pose relativa al campo y el ángulo de giro actual.

Importante

Si en algún momento, decide restablecer su giroscopio, se DEBE llamar al método resetPose con el nuevo ángulo de giro.

Nota

Un ejemplo completo de un mecanum drive con odometría está disponible aquí: C++ / Java.

Además, los métodos GetPose (C++) / getPoseMeters (Java) se pueden utilizar para recuperar la pose actual del robot sin una actualización.