Odométrie pour l’entaînement de type « à embardée » (Swerve)

Un utilisateur peut utiliser les classes cinématiques « swerve drive » pour effectuer l”odométrie. WPILib contient une classe SwerveDriveOdometry qui peut être utilisée pour localiser la position d’un robot avec entraînement par embardée sur le terrain.

Note

Étant donné que cette méthode n’utilise que des encodeurs et un gyroscope, l’estimation de la position du robot sur le terrain dérivera avec le temps, d’autant plus que votre robot entre en contact avec d’autres robots pendant le jeu. Cependant, l’odométrie est généralement très précise pendant la période autonome.

Création de l’objet odométrie

La classe SwerveDriveOdometry1<int NumModules> requiert un argument de modèle (uniquement C ++), deux arguments obligatoires et un argument facultatif. L’argument de modèle (uniquement C ++) est un entier représentant le nombre de modules swerve. Les arguments obligatoires sont l’objet cinématique qui représente votre entraînement par embardée (sous la forme d’une classe SwerveDriveKinematics) et l’angle rapporté par votre gyroscope (comme un Rotation2d). Le troisième argument optionnel est la pose de départ de votre robot sur le terrain (comme un Pose2d). Par défaut, le robot démarre à x = 0, y = 0, theta = 0.

Note

0 degrés / radians représente l’angle du robot lorsque le robot fait face directement à la station d’alliance de votre adversaire. Lorsque votre robot se tourne vers la gauche, l’angle de votre gyroscope devrait augmenter. Par défaut, les gyroscopes WPILib présentent le comportement opposé, vous devez donc compenser l’angle du gyroscope en inversant son signe (nouvel angle = 0 - angle gyro).

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

Mise à jour de la pose du robot

La méthode update de la classe d’odométrie met à jour la position du robot sur le terrainde jeu. La méthode « update » prend en compte l’angle gyroscopique du robot, ainsi qu’une série de valeurs (vitesses et angles) prenant la forme d’un SwerveModuleState . Il est important que l’ordre dans lequel vous passez les objets SwerveModuleState soit le même que l’ordre dans lequel vous avez créé l’objet cinématique.

Cette méthode Update doit être appelée périodiquement, de préférence dans la méthode periodic() du Sous-Système. La méthode update envoie la nouvelle pose du 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());
}

Réinitialisation de la pose de robot

La pose du robot peut être réinitialisée via la méthode resetPose. Cette méthode accepte deux arguments - la nouvelle pose relative au terrain et l’angle gyroscopique actuel.

Important

Si à tout moment, vous décidez de réinitialiser votre gyroscope, la méthode resetPose DOIT être appelée avec le nouvel angle gyroscopique.

Note

L’implémentation de getState () / GetState () ci-dessus est laissée à l’utilisateur. L’idée est d’obtenir l’état de chaque module (vitesse et angle) . Pour un exemple complet, voir ici: C++ / Java.

De plus, les méthodes GetPose (C++) / getPoseMeters (Java) peuvent être utilisées pour récupérer la pose actuelle du robot sans mise à jour.