Factor C2D out into a library for reuse
Hybrid State Feedback Loop already has an implementation. Grab it.
Change-Id: I8b294e3ee7d82249cbd2b0c732086a735a57c36b
diff --git a/frc971/control_loops/BUILD b/frc971/control_loops/BUILD
index 7988e51..b6b0362 100644
--- a/frc971/control_loops/BUILD
+++ b/frc971/control_loops/BUILD
@@ -155,6 +155,7 @@
"hybrid_state_feedback_loop.h",
],
deps = [
+ ":c2d",
":state_feedback_loop",
"//aos:macros",
"//aos/controls:control_loop",
@@ -265,6 +266,17 @@
)
cc_library(
+ name = "c2d",
+ hdrs = [
+ "c2d.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//third_party/eigen",
+ ],
+)
+
+cc_library(
name = "dlqr",
hdrs = [
"dlqr.h",
diff --git a/frc971/control_loops/c2d.h b/frc971/control_loops/c2d.h
new file mode 100644
index 0000000..453a6a5
--- /dev/null
+++ b/frc971/control_loops/c2d.h
@@ -0,0 +1,39 @@
+#ifndef FRC971_CONTROL_LOOPS_C2D_H_
+#define FRC971_CONTROL_LOOPS_C2D_H_
+
+#include <chrono>
+
+#include <Eigen/Dense>
+
+namespace frc971 {
+namespace controls {
+
+template <typename Scalar, int num_states, int num_inputs>
+void C2D(const ::Eigen::Matrix<Scalar, num_states, num_states> &A_continuous,
+ const ::Eigen::Matrix<Scalar, num_states, num_inputs> &B_continuous,
+ ::std::chrono::nanoseconds dt,
+ ::Eigen::Matrix<Scalar, num_states, num_states> *A,
+ ::Eigen::Matrix<Scalar, num_states, num_inputs> *B) {
+ // Trick from
+ // https://en.wikipedia.org/wiki/Discretization#Discretization_of_linear_state_space_models
+ // to solve for A and B more efficiently.
+ Eigen::Matrix<Scalar, num_states + num_inputs, num_states + num_inputs>
+ M_state_continuous;
+ M_state_continuous.setZero();
+ M_state_continuous.template block<num_states, num_states>(0, 0) =
+ A_continuous *
+ ::std::chrono::duration_cast<::std::chrono::duration<Scalar>>(dt).count();
+ M_state_continuous.template block<num_states, num_inputs>(0, num_states) =
+ B_continuous *
+ ::std::chrono::duration_cast<::std::chrono::duration<Scalar>>(dt).count();
+
+ Eigen::Matrix<Scalar, num_states + num_inputs, num_states + num_inputs>
+ M_state = M_state_continuous.exp();
+ *A = M_state.template block<num_states, num_states>(0, 0);
+ *B = M_state.template block<num_states, num_inputs>(0, num_states);
+}
+
+} // namespace controls
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_C2D_H_
diff --git a/frc971/control_loops/hybrid_state_feedback_loop.h b/frc971/control_loops/hybrid_state_feedback_loop.h
index 0fb803f..05d5aab 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop.h
+++ b/frc971/control_loops/hybrid_state_feedback_loop.h
@@ -15,6 +15,7 @@
#include "aos/controls/control_loop.h"
#include "aos/logging/logging.h"
#include "aos/macros.h"
+#include "frc971/control_loops/c2d.h"
#include "frc971/control_loops/state_feedback_loop.h"
template <int number_of_states, int number_of_inputs, int number_of_outputs,
@@ -188,27 +189,8 @@
private:
void UpdateAB(::std::chrono::nanoseconds dt) {
- Eigen::Matrix<Scalar, number_of_states + number_of_inputs,
- number_of_states + number_of_inputs>
- M_state_continuous;
- M_state_continuous.setZero();
- M_state_continuous.template block<number_of_states, number_of_states>(0,
- 0) =
- coefficients().A_continuous *
- ::std::chrono::duration_cast<::std::chrono::duration<Scalar>>(dt)
- .count();
- M_state_continuous.template block<number_of_states, number_of_inputs>(
- 0, number_of_states) =
- coefficients().B_continuous *
- ::std::chrono::duration_cast<::std::chrono::duration<Scalar>>(dt)
- .count();
-
- Eigen::Matrix<Scalar, number_of_states + number_of_inputs,
- number_of_states + number_of_inputs>
- M_state = M_state_continuous.exp();
- A_ = M_state.template block<number_of_states, number_of_states>(0, 0);
- B_ = M_state.template block<number_of_states, number_of_inputs>(
- 0, number_of_states);
+ ::frc971::controls::C2D(coefficients().A_continuous,
+ coefficients().B_continuous, dt, &A_, &B_);
}
Eigen::Matrix<Scalar, number_of_states, 1> X_;