HolonomicDriveController

class wpilib.controller.HolonomicDriveController(xController: wpilib.controller.PIDController, yController: wpilib.controller.PIDController, thetaController: wpilib.controller.ProfiledPIDControllerRadians)

Bases: pybind11_builtins.pybind11_object

This holonomic drive controller can be used to follow trajectories using a holonomic drive train (i.e. swerve or mecanum). Holonomic trajectory following is a much simpler problem to solve compared to skid-steer style drivetrains because it is possible to individually control forward, sideways, and angular velocity.

The holonomic drive controller takes in one PID controller for each direction, forward and sideways, and one profiled PID controller for the angular direction. Because the heading dynamics are decoupled from translations, users can specify a custom heading that the drivetrain should point toward. This heading reference is profiled for smoothness.

Constructs a holonomic drive controller.

Parameters
  • xController – A PID Controller to respond to error in the field-relative x direction.

  • yController – A PID Controller to respond to error in the field-relative y direction.

  • thetaController – A profiled PID controller to respond to error in angle.

atReference()bool

Returns true if the pose error is within tolerance of the reference.

calculate(*args, **kwargs)

Overloaded function.

  1. calculate(self: wpilib.controller._controller.HolonomicDriveController, currentPose: wpimath.geometry._geometry.Pose2d, poseRef: wpimath.geometry._geometry.Pose2d, linearVelocityRef: meters_per_second, angleRef: wpimath.geometry._geometry.Rotation2d) -> wpimath.kinematics._kinematics.ChassisSpeeds

Returns the next output of the holonomic drive controller.

The reference pose, linear velocity, and angular velocity should come from a drivetrain trajectory.

Parameters
  • currentPose – The current pose.

  • poseRef – The desired pose.

  • linearVelocityRef – The desired linear velocity.

  • angleRef – The desired ending angle.

  1. calculate(self: wpilib.controller._controller.HolonomicDriveController, currentPose: wpimath.geometry._geometry.Pose2d, desiredState: wpimath.trajectory._trajectory.Trajectory.State, angleRef: wpimath.geometry._geometry.Rotation2d) -> wpimath.kinematics._kinematics.ChassisSpeeds

Returns the next output of the holonomic drive controller.

The reference pose, linear velocity, and angular velocity should come from a drivetrain trajectory.

Parameters
  • currentPose – The current pose.

  • desiredState – The desired pose, linear velocity, and angular velocity from a trajectory.

  • angleRef – The desired ending angle.

setEnabled(enabled: bool)None

Enables and disables the controller for troubleshooting purposes. When Calculate() is called on a disabled controller, only feedforward values are returned.

Parameters

enabled – If the controller is enabled or not.

setTolerance(tolerance: wpimath.geometry.Pose2d)None

Sets the pose error which is considered tolerable for use with AtReference().

Parameters

poseTolerance – Pose error which is tolerable.