HolonomicDriveController
- class wpimath.controller.HolonomicDriveController(xController: wpimath._controls._controls.controller.PIDController, yController: wpimath._controls._controls.controller.PIDController, thetaController: wpimath._controls._controls.controller.ProfiledPIDControllerRadians)
Bases:
pybind11_object
This holonomic drive controller can be used to follow trajectories using a holonomic drivetrain (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.
calculate(self: wpimath._controls._controls.controller.HolonomicDriveController, currentPose: wpimath.geometry._geometry.Pose2d, trajectoryPose: wpimath.geometry._geometry.Pose2d, desiredLinearVelocity: meters_per_second, desiredHeading: wpimath.geometry._geometry.Rotation2d) -> wpimath.kinematics._kinematics.ChassisSpeeds
Returns the next output of the holonomic drive controller.
- Parameters:
currentPose – The current pose, as measured by odometry or pose estimator.
trajectoryPose – The desired trajectory pose, as sampled for the current timestep.
desiredLinearVelocity – The desired linear velocity.
desiredHeading – The desired heading.
- Returns:
The next output of the holonomic drive controller.
calculate(self: wpimath._controls._controls.controller.HolonomicDriveController, currentPose: wpimath.geometry._geometry.Pose2d, desiredState: wpimath._controls._controls.trajectory.Trajectory.State, desiredHeading: wpimath.geometry._geometry.Rotation2d) -> wpimath.kinematics._kinematics.ChassisSpeeds
Returns the next output of the holonomic drive controller.
- Parameters:
currentPose – The current pose, as measured by odometry or pose estimator.
desiredState – The desired trajectory pose, as sampled for the current timestep.
desiredHeading – The desired heading.
- Returns:
The next output of the holonomic drive controller.
- getThetaController() wpimath._controls._controls.controller.ProfiledPIDControllerRadians
Returns the rotation ProfiledPIDController
- getXController() wpimath._controls._controls.controller.PIDController
Returns the X PIDController
- getYController() wpimath._controls._controls.controller.PIDController
Returns the Y PIDController
- 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._geometry.Pose2d) None
Sets the pose error which is considered tolerable for use with AtReference().
- Parameters:
tolerance – Pose error which is tolerable.