Added A_continuous and B_continuous to StateFeedbackPlantCoefficients
Change-Id: I0c2649e0ef4986c6b9122bf59adc8ad1d45572f4
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index d6bd85f..bb34b85 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -276,15 +276,17 @@
num_states, num_inputs, num_outputs, self._name)]
ans.append(self._DumpMatrix('A', self.A))
+ ans.append(self._DumpMatrix('A_continuous', self.A_continuous))
ans.append(self._DumpMatrix('B', self.B))
+ ans.append(self._DumpMatrix('B_continuous', self.B_continuous))
ans.append(self._DumpMatrix('C', self.C))
ans.append(self._DumpMatrix('D', self.D))
ans.append(self._DumpMatrix('U_max', self.U_max))
ans.append(self._DumpMatrix('U_min', self.U_min))
ans.append(' return StateFeedbackPlantCoefficients<%d, %d, %d>'
- '(A, B, C, D, U_max, U_min);\n' % (num_states, num_inputs,
- num_outputs))
+ '(A, A_continuous, B, B_continuous, C, D, U_max, U_min);\n' % (
+ num_states, num_inputs, num_outputs))
ans.append('}\n')
return ''.join(ans)
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index e535ae7..a81f2cc 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -26,7 +26,9 @@
StateFeedbackPlantCoefficients(const StateFeedbackPlantCoefficients &other)
: A_(other.A()),
+ A_continuous_(other.A_continuous()),
B_(other.B()),
+ B_continuous_(other.B_continuous()),
C_(other.C()),
D_(other.D()),
U_min_(other.U_min()),
@@ -34,21 +36,41 @@
StateFeedbackPlantCoefficients(
const Eigen::Matrix<double, number_of_states, number_of_states> &A,
+ const Eigen::Matrix<double, number_of_states, number_of_states>
+ &A_continuous,
const Eigen::Matrix<double, number_of_states, number_of_inputs> &B,
+ const Eigen::Matrix<double, number_of_states, number_of_inputs>
+ &B_continuous,
const Eigen::Matrix<double, number_of_outputs, number_of_states> &C,
const Eigen::Matrix<double, number_of_outputs, number_of_inputs> &D,
const Eigen::Matrix<double, number_of_inputs, 1> &U_max,
const Eigen::Matrix<double, number_of_inputs, 1> &U_min)
- : A_(A), B_(B), C_(C), D_(D), U_min_(U_min), U_max_(U_max) {}
+ : A_(A),
+ A_continuous_(A_continuous),
+ B_(B),
+ B_continuous_(B_continuous),
+ C_(C),
+ D_(D),
+ U_min_(U_min),
+ U_max_(U_max) {}
const Eigen::Matrix<double, number_of_states, number_of_states> &A() const {
return A_;
}
double A(int i, int j) const { return A()(i, j); }
+ const Eigen::Matrix<double, number_of_states, number_of_states> &A_continuous()
+ const {
+ return A_continuous_;
+ }
+ double A_continuous(int i, int j) const { return A_continuous()(i, j); }
const Eigen::Matrix<double, number_of_states, number_of_inputs> &B() const {
return B_;
}
double B(int i, int j) const { return B()(i, j); }
+ const Eigen::Matrix<double, number_of_states, number_of_inputs> &B_continuous() const {
+ return B_continuous_;
+ }
+ double B_continuous(int i, int j) const { return B_continuous()(i, j); }
const Eigen::Matrix<double, number_of_outputs, number_of_states> &C() const {
return C_;
}
@@ -68,7 +90,9 @@
private:
const Eigen::Matrix<double, number_of_states, number_of_states> A_;
+ const Eigen::Matrix<double, number_of_states, number_of_states> A_continuous_;
const Eigen::Matrix<double, number_of_states, number_of_inputs> B_;
+ const Eigen::Matrix<double, number_of_states, number_of_inputs> B_continuous_;
const Eigen::Matrix<double, number_of_outputs, number_of_states> C_;
const Eigen::Matrix<double, number_of_outputs, number_of_inputs> D_;
const Eigen::Matrix<double, number_of_inputs, 1> U_min_;
diff --git a/frc971/control_loops/state_feedback_loop_test.cc b/frc971/control_loops/state_feedback_loop_test.cc
index 6f97bc7..a08841e 100644
--- a/frc971/control_loops/state_feedback_loop_test.cc
+++ b/frc971/control_loops/state_feedback_loop_test.cc
@@ -14,6 +14,8 @@
// compile or have assertion failures at runtime.
const StateFeedbackPlantCoefficients<2, 4, 7> coefficients(
Eigen::Matrix<double, 2, 2>::Identity(),
+ Eigen::Matrix<double, 2, 2>::Identity(),
+ Eigen::Matrix<double, 2, 4>::Identity(),
Eigen::Matrix<double, 2, 4>::Identity(),
Eigen::Matrix<double, 7, 2>::Identity(),
Eigen::Matrix<double, 7, 4>::Identity(),