Add generic linearized LQR class
This provides a convenient class for taking a system with non-linear
dynamics and generating an LQR controller for it. It still needs some
optimizations to be able to run effectively on a roborio.
Change-Id: I57eaec75738b05713bef272b677aeaa439980354
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/frc971/control_loops/swerve/linearization_utils.h b/frc971/control_loops/swerve/linearization_utils.h
new file mode 100644
index 0000000..ee1275e
--- /dev/null
+++ b/frc971/control_loops/swerve/linearization_utils.h
@@ -0,0 +1,13 @@
+#ifndef FRC971_CONTROL_LOOPS_SWERVE_LINEARIZATION_UTILS_H_
+#define FRC971_CONTROL_LOOPS_SWERVE_LINEARIZATION_UTILS_H_
+
+namespace frc971::control_loops::swerve {
+template <typename State, typename Input>
+struct DynamicsInterface {
+ virtual ~DynamicsInterface() {}
+ // To be overridden by the implementation; returns the derivative of the state
+ // at the given state with the provided control input.
+ virtual State operator()(const State &X, const Input &U) const = 0;
+};
+} // namespace frc971::control_loops::swerve
+#endif // FRC971_CONTROL_LOOPS_SWERVE_LINEARIZATION_UTILS_H_