Class LinearSystemLoop<States extends Num,​Inputs extends Num,​Outputs extends Num>


  • public class LinearSystemLoop<States extends Num,​Inputs extends Num,​Outputs extends Num>
    extends Object
    Combines a controller, feedforward, and observer for controlling a mechanism with full state feedback.

    For everything in this file, "inputs" and "outputs" are defined from the perspective of the plant. This means U is an input and Y is an output (because you give the plant U (powers) and it gives you back a Y (sensor values). This is the opposite of what they mean from the perspective of the controller (U is an output because that's what goes to the motors and Y is an input because that's what comes back from the sensors).

    For more on the underlying math, read https://file.tavsys.net/control/controls-engineering-in-frc.pdf.

    • Method Detail

      • getXHat

        public Matrix<States,​N1getXHat()
        Returns the observer's state estimate x-hat.
        Returns:
        the observer's state estimate x-hat.
      • getXHat

        public double getXHat​(int row)
        Returns an element of the observer's state estimate x-hat.
        Parameters:
        row - Row of x-hat.
        Returns:
        the i-th element of the observer's state estimate x-hat.
      • setXHat

        public void setXHat​(Matrix<States,​N1> xhat)
        Set the initial state estimate x-hat.
        Parameters:
        xhat - The initial state estimate x-hat.
      • setXHat

        public void setXHat​(int row,
                            double value)
        Set an element of the initial state estimate x-hat.
        Parameters:
        row - Row of x-hat.
        value - Value for element of x-hat.
      • getNextR

        public double getNextR​(int row)
        Returns an element of the controller's next reference r.
        Parameters:
        row - Row of r.
        Returns:
        the element i of the controller's next reference r.
      • getNextR

        public Matrix<States,​N1getNextR()
        Returns the controller's next reference r.
        Returns:
        the controller's next reference r.
      • setNextR

        public void setNextR​(Matrix<States,​N1> nextR)
        Set the next reference r.
        Parameters:
        nextR - Next reference.
      • setNextR

        public void setNextR​(double... nextR)
        Set the next reference r.
        Parameters:
        nextR - Next reference.
      • getU

        public Matrix<Inputs,​N1getU()
        Returns the controller's calculated control input u plus the calculated feedforward u_ff.
        Returns:
        the calculated control input u.
      • getU

        public double getU​(int row)
        Returns an element of the controller's calculated control input u.
        Parameters:
        row - Row of u.
        Returns:
        the calculated control input u at the row i.
      • reset

        public void reset​(Matrix<States,​N1> initialState)
        Zeroes reference r and controller output u. The previous reference of the PlantInversionFeedforward and the initial state estimate of the KalmanFilter are set to the initial state provided.
        Parameters:
        initialState - The initial state.
      • getError

        public Matrix<States,​N1getError()
        Returns difference between reference r and current state x-hat.
        Returns:
        The state error matrix.
      • getError

        public double getError​(int index)
        Returns difference between reference r and current state x-hat.
        Parameters:
        index - The index of the error matrix to return.
        Returns:
        The error at that index.
      • correct

        public void correct​(Matrix<Outputs,​N1> y)
        Correct the state estimate x-hat using the measurements in y.
        Parameters:
        y - Measurement vector.
      • predict

        public void predict​(double dtSeconds)
        Sets new controller output, projects model forward, and runs observer prediction.

        After calling this, the user should send the elements of u to the actuators.

        Parameters:
        dtSeconds - Timestep for model update.
      • clampInput

        public Matrix<Inputs,​N1clampInput​(Matrix<Inputs,​N1> unclampedU)
        Clamp the input u to the min and max.
        Parameters:
        unclampedU - The input to clamp.
        Returns:
        The clamped input.