La cinématique de l’entraînement Mécanum

La classe MecanumDriveKinematics est un outil utile qui convertit entre un objet ChassisSpeeds et un objet MecanumDriveWheelSpeeds , qui contient les vitesses pour chacune des quatre roues sur un entraînement mécanique.

Construction de l’objet cinématique

La classe MecanumDriveKinematics accepte quatre arguments de constructeur, chaque argument étant l’emplacement d’une roue par rapport au centre du robot (en tant que Translation2d). L’ordre des arguments est avant gauche, avant droit, arrière gauche et arrière droit. L’emplacement des roues doit être relatif au centre du robot. Les valeurs x positives représentent le déplacement vers l’avant du robot tandis que les valeurs y positives représentent le déplacement vers la gauche du robot.

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

Conversion des vitesses du châssis en vitesses de roue

La méthode toWheelSpeeds(ChassisSpeeds speeds) (Java) / ToWheelSpeeds(ChassisSpeeds speeds) (C++) doit être utilisée pour convertir un objet ChassisSpeeds en un objet MecanumDriveWheelSpeeds. Ceci est utile dans les situations où vous devez convertir une vitesse avant, une vitesse latérale et une vitesse angulaire en vitesses de roue individuelles.

// Example chassis speeds: 1 meter per second forward, 3 meters
// per second to the left, and rotation at 1.5 radians per second
// counterclockwise.
ChassisSpeeds speeds = new ChassisSpeeds(1.0, 3.0, 1.5);

// Convert to wheel speeds
MecanumDriveWheelSpeeds wheelSpeeds = kinematics.toWheelSpeeds(speeds);

// Get the individual wheel speeds
double frontLeft = wheelSpeeds.frontLeftMetersPerSecond
double frontRight = wheelSpeeds.frontRightMetersPerSecond
double backLeft = wheelSpeeds.rearLeftMetersPerSecond
double backRight = wheelSpeeds.rearRightMetersPerSecond

Entraînement orienté terrain

Recall qu’un objet ChassisSpeeds peut être créé à partir d’un ensemble de vitesses orientées en fonction du terrain de jeu. Cette fonction peut être utilisée également pour obtenir les vitesses de roue à partir du même ensemble (de vitesses orientées en fonction du terrain de jeu).

// The desired field relative speed here is 2 meters per second
// toward the opponent's alliance station wall, and 2 meters per
// second toward the left field boundary. The desired rotation
// is a quarter of a rotation per second counterclockwise. The current
// robot angle is 45 degrees.
ChassisSpeeds speeds = ChassisSpeeds.fromFieldRelativeSpeeds(
  2.0, 2.0, Math.PI / 2.0, Rotation2d.fromDegrees(45.0));

// Now use this in our kinematics
MecanumDriveWheelSpeeds wheelSpeeds = kinematics.toWheelSpeeds(speeds);

Utilisation de centres de rotation personnalisés

Parfois, la rotation autour d’un point spécifique peut être souhaitable pour que le robot puisse éviter un obstacle. Cette manoeuvre est également prise en charge par les classes WPILib. La même méthode ToWheelSpeeds() accepte un deuxième paramètre pour le centre de rotation (comme un Translation2d). Tout comme les emplacements des roues, le Translation2d qui correspond au centre de rotation doit être défini par rapport au centre du robot.

Note

Étant donné que tous les robots ont un châssis rigide, les vitesses vx et vy fournies par l’objet ChassisSpeeds s’appliqueront toujours à l’intégralité du robot. Cependant, omega``de l'objet ``ChassisSpeeds sera mesuré à partir du centre de rotation.

Par exemple, on peut définir le centre de rotation sur une certaine roue et si l’objet ChassisSpeeds fourni a un vx et un vy de zéro et un omega différent de zéro, le robot tournera autour de cette roue spécifique.

Conversion des vitesses de roue en vitesses de châssis

On peut également utiliser l’objet cinématique pour convertir un objet MecanumDriveWheelSpeeds en un objet ChassisSpeeds unique. La méthode toChassisSpeeds(MecanumDriveWheelSpeeds speeds) (Java) / ToChassisSpeeds(MecanumDriveWheelSpeeds speeds) (C++) peut être utilisée pour y parvenir.

// Example wheel speeds
var wheelSpeeds = new MecanumDriveWheelSpeeds(-17.67, 20.51, -13.44, 16.26);

// Convert to chassis speeds
ChassisSpeeds chassisSpeeds = kinematics.toChassisSpeeds(wheelSpeeds);

// Getting individual speeds
double forward = chassisSpeeds.vxMetersPerSecond;
double sideways = chassisSpeeds.vyMetersPerSecond;
double angular = chassisSpeeds.omegaRadiansPerSecond;