全向驱动控制器

全向驱动器控制器是一种轨迹跟踪器，适用于具有全向传动系统（例如，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``中的参数设置的概要。

JAVA

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.

C++

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.

PYTHON

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.

调整速度

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

备注

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.

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

C++

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

PYTHON

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

使用调整后的速度

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

可以使用适用于您的传动系统类型的运动学类将返回的调整速度转换为可用速度。在下面的示例代码中，我们将假定一个弧形驱动机器人；但是，除了使用``MecanumDriveKinematics’’外，运动学代码与麦克纳姆驱动机器人完全相同。

JAVA

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

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

C++

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

PYTHON

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

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