全向驱动控制器

全向驱动器控制器是一种轨迹跟踪器，适用于具有全向传动系统（例如，swerve底盘，麦克纳姆轮等）的机器人。可以使用它来校正微小的扰动，从而准确地跟踪轨迹。

构造全向驱动控制器

全向驱动控制器应使用2个PID控制器和1个简略PID控制器实例化。

备注

有关PID控制的更多信息，请参见 WPILib中的PID控制。

2个PID控制器是分别应校正相对于磁场的x和y方向误差的控制器。例如，如果前两个参数分别是``PIDController(1, 0, 0)`` 和``PIDController(1.2, 0, 0)``，则完整驱动控制器将在x方向上为x方向上每米的误差增加每秒1米的附加误差。在y方向上为y方向上每米的误差增加每秒1.2米的附加误差。

最后一个参数是用于机器人旋转的“ProfiledPIDController”。由于完整传动系统的旋转动力学在x和y方向上与运动解耦，用户可以设置自定义的航向参考，同时遵循轨迹。这些标题引用是根据``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{
  frc2::PIDController{1, 0, 0}, frc2::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.

调整速度

完整的驱动控制器返回“已调整的速度”，以便当机器人跟踪这些速度时，它可以精确地达到目标点。应使用新目标定期更新控制器。目标包括所需的姿势，线速度和航向。

备注

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.

控制器可以使用``Calculate`` (c++) / Calculate (Java)方法更新。此方法有两个重载。这两种重载都接受当前机器人位置作为第一个参数，并将期望的航向作为最后一个参数。对于中间的参数，一个重载接受所需的姿态和线速度参考，而另一个接受’ ‘轨迹。对象，它包含关于目标姿态的信息。后一种方法是跟踪轨迹的首选方法。

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

使用调整后的速度

调整后的速度为``ChassisSpeeds’’类型，其中包含``vx’’（向前方向的线速度），``vy’’（横向方向的线速度）和``omega’’ （围绕机器人框架中心的角速度）。

可以使用适用于您的传动系统类型的运动学类将返回的调整速度转换为可用速度。在下面的示例代码中，我们将假定一个弧形驱动机器人；但是，除了使用``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);

由于这些转弯模块状态仍然是速度和角度，因此您将需要使用PID控制器来设置这些速度和角度。