Contrôleur pour entraînement de type holonomique

Le contrôleur pour entraînement de type holonomique permet de tracer la trajectoire pour un robot ayant une transmission holonomique (par exemple, à embardée (Swerve) ou Mécanum, etc.). Ceci peut être utilisé pour suivre avec précision une trajectoire avec la possibilité de corriger des perturbations mineures.

Construire un contrôleur holonomique

Le contrôleur d’entraînement holonomique doit être instancié avec 2 contrôleurs PID et 1 contrôleur PID profilé.

Note

Pour plus d’informations sur le contrôle PID, voir Contrôle PID dans WPILib.

Les 2 contrôleurs PID sont des contrôleurs qui doivent corriger les erreurs respectivement dans les directions x et y relatives au champ. Par exemple, si les 2 premiers arguments sont respectivement PIDController(1, 0, 0) et PIDController(1.2, 0, 0), le contrôleur de lecteur holonomique ajoutera un mètre supplémentaire par seconde dans le x direction pour chaque mètre d’erreur dans la direction x et ajoutera 1,2 mètre supplémentaire par seconde dans la direction y pour chaque mètre d’erreur dans la direction y.

Le paramètre final est un ProfiledPIDController pour la rotation du robot. Étant donné que la dynamique de rotation d’une transmission holonomique est découplée du mouvement dans les directions x et y, les utilisateurs peuvent définir des références de cap personnalisées lorsque le but est de suivre une trajectoire. Ces références d’en-tête sont profilées en fonction des paramètres définis dans le ProfiledPIDController.

var controller = new HolonomicDriveController(
  new PIDController(1, 0, 0), new PIDController(1, 0, 0),
  new ProfiledPIDController(1, 0, 0,
    new TrapezoidProfile.Constraints(6.28, 3.14)));
// Here, our rotation profile constraints were a max velocity
// of 1 rotation per second and a max acceleration of 180 degrees
// per second squared.

frc::HolonomicDriveController controller{
  frc::PIDController{1, 0, 0}, frc::PIDController{1, 0, 0},
  frc::ProfiledPIDController<units::radian>{
    1, 0, 0, frc::TrapezoidProfile<units::radian>::Constraints{
      6.28_rad_per_s, 3.14_rad_per_s / 1_s}}};
// Here, our rotation profile constraints were a max velocity
// of 1 rotation per second and a max acceleration of 180 degrees
// per second squared.

from wpimath.controller import (
   HolonomicDriveController,
   PIDController,
   ProfiledPIDControllerRadians,
)
from wpimath.trajectory import TrapezoidProfileRadians

controller = HolonomicDriveController(
   PIDController(1, 0, 0),
   PIDController(1, 0, 0),
   ProfiledPIDControllerRadians(
      1, 0, 0, TrapezoidProfileRadians.Constraints(6.28, 3.14)
   ),
)
# Here, our rotation profile constraints were a max velocity
# of 1 rotation per second and a max acceleration of 180 degrees
# per second squared.

Obtenir des vitesses ajustées

Le contrôleur d’entraînement holonomique renvoie des « vitesses ajustées » de telle sorte que lorsque le robot suit ces vitesses, il atteint avec précision le point cible. Le contrôleur doit être mis à jour périodiquement avec le nouvel objectif. L’objectif est composé d’une pose désirée, d’une vitesse linéaire et d’un cap souhaités.

Note

The « goal pose » represents the position that the robot should be at a particular timestamp when tracking the trajectory. It does NOT represent the trajectory’s endpoint.

The controller can be updated using the Calculate (C++) / calculate (Java/Python) method. There are two overloads for this method. Both of these overloads accept the current robot position as the first parameter and the desired heading as the last parameter. For the middle parameters, one overload accepts the desired pose and the linear velocity reference while the other accepts a Trajectory.State object, which contains information about the goal pose. The latter method is preferred for tracking trajectories.

// Sample the trajectory at 3.4 seconds from the beginning.
Trajectory.State goal = trajectory.sample(3.4);

// Get the adjusted speeds. Here, we want the robot to be facing
// 70 degrees (in the field-relative coordinate system).
ChassisSpeeds adjustedSpeeds = controller.calculate(
  currentRobotPose, goal, Rotation2d.fromDegrees(70.0));

// Sample the trajectoty at 3.4 seconds from the beginning.
const auto goal = trajectory.Sample(3.4_s);

// Get the adjusted speeds. Here, we want the robot to be facing
// 70 degrees (in the field-relative coordinate system).
const auto adjustedSpeeds = controller.Calculate(
  currentRobotPose, goal, 70_deg);

from wpimath.geometry import Rotation2d

# Sample the trajectory at 3.4 seconds from the beginning.
goal = trajectory.sample(3.4)

# Get the adjusted speeds. Here, we want the robot to be facing
# 70 degrees (in the field-relative coordinate system).
adjustedSpeeds = controller.calculate(
   currentRobotPose, goal, Rotation2d.fromDegrees(70.0)
)

Utilisation des vitesses ajustées

Les vitesses ajustées sont de type ChassisSpeeds, qui contient un vx (vitesse linéaire dans le sens avant), un vy (vitesse linéaire dans le sens latéral) et un omega (vitesse angulaire autour du centre du châssis du robot).

Les vitesses ajustées renvoyées peuvent être converties en vitesses utilisables à l’aide des classes de cinématique correspondant à votre type de transmission. Dans l’exemple de code ci-dessous, nous supposerons un robot à entraînement de type Swerve; cependant, le code cinématique est exactement le même pour un robot avec entraînement Mécanum sauf en utilisant MecanumDriveKinematics.

SwerveModuleState[] moduleStates = kinematics.toSwerveModuleStates(adjustedSpeeds);

SwerveModuleState frontLeft = moduleStates[0];
SwerveModuleState frontRight = moduleStates[1];
SwerveModuleState backLeft = moduleStates[2];
SwerveModuleState backRight = moduleStates[3];

auto [fl, fr, bl, br] = kinematics.ToSwerveModuleStates(adjustedSpeeds);

fl, fr, bl, br = kinematics.toSwerveModuleStates(adjustedSpeeds)

Étant donné que les états de module pour un entraînement de type Swerve sont définis par des vitesses et des angles, vous devrez utiliser des contrôleurs PID pour définir ces vitesses et ces angles.