Package frc.team88.swerve.util

Class TrapezoidalProfileController

  • public class TrapezoidalProfileController
extends java.lang.Object
    Class that performs the math of a trapezoidal profile controller. Calculates the required speed to follow the profile live. Handles a combined position and velocity target.

    • Constructor Detail

      • TrapezoidalProfileController

        public TrapezoidalProfileController​(TrapezoidalControllerConfiguration config)
        Constructor.
        Parameters:
        config - The configuration for this controller.

    • Method Detail

      • setTargetVelocity

        public void setTargetVelocity​(double velocity)
        Sets the target velocity.
        Parameters:
        velocity - The target velocity which is a setpoint for this controller, in units per second.

      • setTargetPosition

        public void setTargetPosition​(double position)
        Sets the target position.
        Parameters:
        position - The target position which is a setpoint for this controller.

      • getMaxSpeed

        public double getMaxSpeed()
        Gets the max speed limit.
        Returns:
        The maximum speed value that can be both commanded and output, in units per second.

      • getMaxAcceleration

        public double getMaxAcceleration()
        Gets the max acceleration limit.
        Returns:
        The maximum acceleration for the trapezoidal profile, in units per second^2

      • getTargetVelocity

        public double getTargetVelocity()
        Gets the target velocity.
        Returns:
        The target velocity which is a setpoint for this controller, in units per second.

      • getTargetPosition

        public double getTargetPosition()
        Gets the target position.
        Returns:
        The target position which is a setpoint for this controller.

      • getLastCommandedPosition

        public double getLastCommandedPosition()
        Gets the last commanded position.
        Returns:
        The last commanded position.

      • getLastCommandedVelocity

        public double getLastCommandedVelocity()
        Gets the last commanded velocity.
        Returns:
        The last commanded velocity.

      • getPositionPID

        public SyncPIDController getPositionPID()
        Gets the position PID controller.
        Returns:
        The position controller used to adjust the velocity.

      • reset

        public void reset​(double currentPosition)
        Resets the trapezoidal controller, including the associated position PID. Should be called after this controller hasn't been used for a long time.
        Parameters:
        currentPosition - The current positon.

      • calculateCommandVelocity

        public double calculateCommandVelocity​(double currentPosition,
                                       double currentVelocity)
        Calculates the velocity output following the trapezoidal profile.
        Parameters:
        currentPosition - The current position as an input.
        currentVelocity - The current velocity as an input, in units per second.
        Returns:
        The velocity value to command as the output, in units per second

      • calculateAcceleratedVelocity

        protected double calculateAcceleratedVelocity​(double currentVelocity,
                                              boolean forwards)
        Calculates the command velocity based on the current velocity and max acceleration.
        Parameters:
        currentVelocity - The current velocity, in units per second.
        forwards - True if the value returned should be greater than the current velocity, false if it should be smaller.
        Returns:
        The calculated command velocity, in units per second.

      • applyMaxSpeedLimit

        protected double applyMaxSpeedLimit​(double commandVelocity)
        Limits the command velocity such that it does not exceed the max speed limit.
        Parameters:
        commandVelocity - The command velocity to limit, in units per second.
        Returns:
        The limited velocity, in units per second.

      • applyDeccelerationLimit

        protected double applyDeccelerationLimit​(double commandVelocity,
                                         boolean forwards)
        Limit the command velocity such that it has time to deccelerate to hit the target at the right velocity.
        Parameters:
        commandVelocity - The command velocity to limit, in units per second.
        forwards - Whether we are moving forwards or backwards.
        Returns:
        The limited velocity, in units per second.

      • calculateCommandPosition

        protected double calculateCommandPosition​(double commandVelocity)
        Determine the command position based on the last command position and the velocity.
        Parameters:
        commandVelocity - The currently commanded velocity, in units per second.
        Returns:
        The position to command.