HeadingLock

A modifier that controls the heading of the robot by using a turn controller to ensure the heading remains constant. This class implements Function<Translation, Translation>, meaning it can be used to modify a Translation object, specifically by adjusting its Z-component (turn) to maintain a desired heading.

Methods

HeadingLock(Angle target, Controller turnController, Supplier<Angle> getCurrentAngle)

Constructs a new HeadingLock modifier.

• target: The desired Angle that the robot should maintain.
• turnController: A Controller instance used for calculating the robot's turn (vz) values. This controller will receive the difference between the current angle and the target angle as input.
• getCurrentAngle: A Supplier<Angle> that provides the robot's current heading Angle.

apply(Translation translation)

Applies the heading lock modification to the input Translation. It calculates the necessary turn value using the turnController to keep the robot at the target heading and returns a new Translation with the adjusted Z-component. The X and Y components of the input translation remain unchanged.