mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-21 01:01:43 +00:00
Clean up Java style (#5990)
Also make equivalent changes in C++ where applicable. Co-authored-by: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com>
This commit is contained in:
Tyler Veness
@@ -16,9 +16,8 @@ import java.util.function.Function;
|
||||
* Constructs a control-affine plant inversion model-based feedforward from given model dynamics.
|
||||
*
|
||||
* <p>If given the vector valued function as f(x, u) where x is the state vector and u is the input
|
||||
* vector, the B matrix(continuous input matrix) is calculated through a {@link
|
||||
* edu.wpi.first.math.system.NumericalJacobian}. In this case f has to be control-affine (of the
|
||||
* form f(x) + Bu).
|
||||
* vector, the B matrix(continuous input matrix) is calculated through a {@link NumericalJacobian}.
|
||||
* In this case f has to be control-affine (of the form f(x) + Bu).
|
||||
*
|
||||
* <p>The feedforward is calculated as <strong> u_ff = B<sup>+</sup> (rDot - f(x))</strong>, where
|
||||
* <strong> B<sup>+</sup> </strong> is the pseudoinverse of B.
|
||||
@@ -26,8 +25,8 @@ import java.util.function.Function;
|
||||
* <p>This feedforward does not account for a dynamic B matrix, B is either determined or supplied
|
||||
* when the feedforward is created and remains constant.
|
||||
*
|
||||
* <p>For more on the underlying math, read
|
||||
* https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
|
||||
* <p>For more on the underlying math, read <a
|
||||
* href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">https://file.tavsys.net/control/controls-engineering-in-frc.pdf</a>.
|
||||
*/
|
||||
public class ControlAffinePlantInversionFeedforward<States extends Num, Inputs extends Num> {
|
||||
/** The current reference state. */
|
||||
|
||||
@@ -17,8 +17,8 @@ import org.ejml.simple.SimpleMatrix;
|
||||
* system and makes it behave like some other system. This can be used to make a drivetrain more
|
||||
* controllable during teleop driving by making it behave like a slower or more benign drivetrain.
|
||||
*
|
||||
* <p>For more on the underlying math, read appendix B.3 in
|
||||
* https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
|
||||
* <p>For more on the underlying math, read appendix B.3 in <a
|
||||
* href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">https://file.tavsys.net/control/controls-engineering-in-frc.pdf</a>.
|
||||
*/
|
||||
public class ImplicitModelFollower<States extends Num, Inputs extends Num, Outputs extends Num> {
|
||||
// Computed controller output
|
||||
|
||||
@@ -65,8 +65,8 @@ public class LTVDifferentialDriveController {
|
||||
/**
|
||||
* Constructs a linear time-varying differential drive controller.
|
||||
*
|
||||
* <p>See
|
||||
* https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning
|
||||
* <p>See <a
|
||||
* href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning">https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning</a>
|
||||
* for how to select the tolerances.
|
||||
*
|
||||
* @param plant The differential drive velocity plant.
|
||||
|
||||
@@ -80,8 +80,8 @@ public class LTVUnicycleController {
|
||||
/**
|
||||
* Constructs a linear time-varying unicycle controller.
|
||||
*
|
||||
* <p>See
|
||||
* https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning
|
||||
* <p>See <a
|
||||
* href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning">https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning</a>
|
||||
* for how to select the tolerances.
|
||||
*
|
||||
* @param qelems The maximum desired error tolerance for each state.
|
||||
@@ -95,8 +95,8 @@ public class LTVUnicycleController {
|
||||
/**
|
||||
* Constructs a linear time-varying unicycle controller.
|
||||
*
|
||||
* <p>See
|
||||
* https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning
|
||||
* <p>See <a
|
||||
* href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning">https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning</a>
|
||||
* for how to select the tolerances.
|
||||
*
|
||||
* @param qelems The maximum desired error tolerance for each state.
|
||||
|
||||
@@ -17,8 +17,8 @@ import org.ejml.simple.SimpleMatrix;
|
||||
* <p>The feedforward is calculated as <strong> u_ff = B<sup>+</sup> (r_k+1 - A r_k) </strong>,
|
||||
* where <strong> B<sup>+</sup> </strong> is the pseudoinverse of B.
|
||||
*
|
||||
* <p>For more on the underlying math, read
|
||||
* https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
|
||||
* <p>For more on the underlying math, read <a
|
||||
* href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">https://file.tavsys.net/control/controls-engineering-in-frc.pdf</a>.
|
||||
*/
|
||||
public class LinearPlantInversionFeedforward<
|
||||
States extends Num, Inputs extends Num, Outputs extends Num> {
|
||||
|
||||
@@ -19,8 +19,8 @@ import org.ejml.simple.SimpleMatrix;
|
||||
* Contains the controller coefficients and logic for a linear-quadratic regulator (LQR). LQRs use
|
||||
* the control law u = K(r - x).
|
||||
*
|
||||
* <p>For more on the underlying math, read
|
||||
* https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
|
||||
* <p>For more on the underlying math, read <a
|
||||
* href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">https://file.tavsys.net/control/controls-engineering-in-frc.pdf</a>.
|
||||
*/
|
||||
public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Outputs extends Num> {
|
||||
/** The current reference state. */
|
||||
@@ -35,8 +35,8 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Ou
|
||||
/**
|
||||
* Constructs a controller with the given coefficients and plant. Rho is defaulted to 1.
|
||||
*
|
||||
* <p>See
|
||||
* https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning
|
||||
* <p>See <a
|
||||
* href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning">https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning</a>
|
||||
* for how to select the tolerances.
|
||||
*
|
||||
* @param plant The plant being controlled.
|
||||
@@ -61,8 +61,8 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Ou
|
||||
/**
|
||||
* Constructs a controller with the given coefficients and plant.
|
||||
*
|
||||
* <p>See
|
||||
* https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning
|
||||
* <p>See <a
|
||||
* href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning">https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning</a>
|
||||
* for how to select the tolerances.
|
||||
*
|
||||
* @param A Continuous system matrix of the plant being controlled.
|
||||
@@ -107,14 +107,12 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Ou
|
||||
var discB = discABPair.getSecond();
|
||||
|
||||
if (!StateSpaceUtil.isStabilizable(discA, discB)) {
|
||||
var builder = new StringBuilder("The system passed to the LQR is uncontrollable!\n\nA =\n");
|
||||
builder
|
||||
.append(discA.getStorage().toString())
|
||||
.append("\nB =\n")
|
||||
.append(discB.getStorage().toString())
|
||||
.append('\n');
|
||||
|
||||
var msg = builder.toString();
|
||||
var msg =
|
||||
"The system passed to the LQR is uncontrollable!\n\nA =\n"
|
||||
+ discA.getStorage().toString()
|
||||
+ "\nB =\n"
|
||||
+ discB.getStorage().toString()
|
||||
+ '\n';
|
||||
MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace());
|
||||
throw new IllegalArgumentException(msg);
|
||||
}
|
||||
@@ -258,8 +256,9 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Ou
|
||||
* are time-delayed too long, the LQR may be unstable. However, if we know the amount of delay, we
|
||||
* can compute the control based on where the system will be after the time delay.
|
||||
*
|
||||
* <p>See https://file.tavsys.net/control/controls-engineering-in-frc.pdf appendix C.4 for a
|
||||
* derivation.
|
||||
* <p>See <a
|
||||
* href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">https://file.tavsys.net/control/controls-engineering-in-frc.pdf</a>
|
||||
* appendix C.4 for a derivation.
|
||||
*
|
||||
* @param plant The plant being controlled.
|
||||
* @param dtSeconds Discretization timestep in seconds.
|
||||
|
||||
Reference in New Issue
Block a user