Odometría para un Swerve Drive

Un usuario puede usar las clases cinemáticas del swerve drive para ejecutar odometría. WPILib contiene una clase SwerveDriveOdometry que puede ser utilizada para rastrear la posicion de un robot con swerve drive en el campo.

Nota

Dado a que este método utiliza solamente encoders y un giroscopio, el estimado de la posición del robot en el campo variará con el tiempo, especialmente cuando el robot tenga contacto con otros robots durante el juego. Sin embargo, la odometría es usualmente muy acertada para el periodo autónomo.

Creando el objeto odométrico

La clase SwerveDriveOdometry<int NumModules> requiere un argumento de plantilla (solo en C++), dos argumentos obligatorios y un argumento opcional. El argumento de plantilla (solo en C++) es un entero que representa el número de módulos swerve. Los argumentos obligatorios son el objeto cinématico que representa tu swerve drive (en la forma de una clase SwerveDriveKinematics ) y el ángulo reportado por tu giroscopio (como Rotation2d). El tercer argumento opcional es la posición inicial de tu robot en el campo (como Pose2d). De forma predeterminada, el robot empezará en x = 0, y = 0, theta = 0.

Nota

0 grados / radianes representa el ángulo del robot cuando este se encuentra de frente hacia la estación del oponente. Si el robot empieza a girar a la izquierda, el ángulo del giroscopio debe aumentar. De forma predeterminada, los giroscopios de WPILib muestras un comportamiento opuesto, así que debes anular el ángulo del giroscopio.

// Locations for the swerve drive modules 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 module locations
SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(
  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.
SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(m_kinematics,
  getGyroHeading(), new Pose2d(5.0, 13.5, new Rotation2d()));

Actualizando la posición del robot

El método update de la clase de odometría actualiza la posición del robot en el campo. Este método recibe el ángulo del robot del giroscopio, así como una serie de estados modulares (velocidades y ángulos) en forma de SwerveModuleState. Es importante que el orden en el que pasas los objetos SwerveModuleState es el mismo orden en el que creaste el objeto cinemático.

Este método update debe ser llamado periódicamente, de preferencia en el método periodic() de un Subsistema. El método update regresa la nueva posición actualizada del robot.

@Override
public void periodic() {
  // 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, m_frontLeftModule.getState(), m_frontRightModule.getState(),
      m_backLeftModule.getState(), m_backRightModule.getState());
}

Restableciendo la posición del robot

La posición del robot puede restablecerse por medio del método resetPose. Este método acepta dos argumentos – la nueva posición relativa al campo y el ángulo del giroscopio actual.

Importante

Si en algún momento, decides reiniciar tu giroscopio, el método resetPose DEBE ser llamado con el nuevo ángulo del giroscopio.

Nota

La implementación de getState() / GetState()  anterior se deja al usuario. La idea es obtener el estado del módulo (velocidad y ángulo) de cada módulo. Para un ejemplo completo, vea aquí: <https://github.com/wpilibsuite/allwpilib/tree/main/wpilibcExamples/src/main/cpp/examples/SwerveBot>`_ / Java.

Además, los métodos GetPose (C++) / getPoseMeters (Java) pueden ser utilizado para recuperar la posición actual del robot sin la necesidad de actualizar.