Pulled U out of the plant.
Change-Id: I4410b74a4b03e1a6e3a142eb8d9762bb9803763f
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 403d32c..41a4f3c 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -67,11 +67,11 @@
explicit DrivetrainPlant(StateFeedbackPlant<4, 2, 2> &&other)
: StateFeedbackPlant<4, 2, 2>(::std::move(other)) {}
- void CheckU() override {
- assert(U(0, 0) <= U_max(0, 0) + 0.00001 + left_voltage_offset_);
- assert(U(0, 0) >= U_min(0, 0) - 0.00001 + left_voltage_offset_);
- assert(U(1, 0) <= U_max(1, 0) + 0.00001 + right_voltage_offset_);
- assert(U(1, 0) >= U_min(1, 0) - 0.00001 + right_voltage_offset_);
+ void CheckU(const Eigen::Matrix<double, 2, 1> &U) override {
+ EXPECT_LE(U(0, 0), U_max(0, 0) + 0.00001 + left_voltage_offset_);
+ EXPECT_GE(U(0, 0), U_min(0, 0) - 0.00001 + left_voltage_offset_);
+ EXPECT_LE(U(1, 0), U_max(1, 0) + 0.00001 + right_voltage_offset_);
+ EXPECT_GE(U(1, 0), U_min(1, 0) - 0.00001 + right_voltage_offset_);
}
double left_voltage_offset() const { return left_voltage_offset_; }
@@ -154,7 +154,7 @@
last_left_position_ = drivetrain_plant_->Y(0, 0);
last_right_position_ = drivetrain_plant_->Y(1, 0);
EXPECT_TRUE(my_drivetrain_queue_.output.FetchLatest());
- drivetrain_plant_->mutable_U() = last_U_;
+ ::Eigen::Matrix<double, 2, 1> U = last_U_;
last_U_ << my_drivetrain_queue_.output->left_voltage,
my_drivetrain_queue_.output->right_voltage;
{
@@ -178,11 +178,9 @@
}
}
- drivetrain_plant_->mutable_U(0, 0) +=
- drivetrain_plant_->left_voltage_offset();
- drivetrain_plant_->mutable_U(1, 0) +=
- drivetrain_plant_->right_voltage_offset();
- drivetrain_plant_->Update();
+ U(0, 0) += drivetrain_plant_->left_voltage_offset();
+ U(1, 0) += drivetrain_plant_->right_voltage_offset();
+ drivetrain_plant_->Update(U);
}
void set_left_voltage_offset(double left_voltage_offset) {
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index bb34b85..fba883c 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -71,8 +71,8 @@
typename, num_states, num_inputs, num_outputs)
def _ControllerType(self):
- """Returns a template name for StateFeedbackController."""
- return self._GenericType('StateFeedbackController')
+ """Returns a template name for StateFeedbackControllerConstants."""
+ return self._GenericType('StateFeedbackControllerConstants')
def _LoopType(self):
"""Returns a template name for StateFeedbackLoop."""
@@ -307,7 +307,7 @@
num_states = self.A.shape[0]
num_inputs = self.B.shape[1]
num_outputs = self.C.shape[0]
- return 'StateFeedbackController<%d, %d, %d> %s;\n' % (
+ return 'StateFeedbackControllerConstants<%d, %d, %d> %s;\n' % (
num_states, num_inputs, num_outputs, self.ControllerFunction())
def DumpController(self):
@@ -319,7 +319,7 @@
num_states = self.A.shape[0]
num_inputs = self.B.shape[1]
num_outputs = self.C.shape[0]
- ans = ['StateFeedbackController<%d, %d, %d> %s {\n' % (
+ ans = ['StateFeedbackControllerConstants<%d, %d, %d> %s {\n' % (
num_states, num_inputs, num_outputs, self.ControllerFunction())]
ans.append(self._DumpMatrix('L', self.L))
@@ -330,7 +330,7 @@
ans.append(self._DumpMatrix('Kff', self.Kff))
ans.append(self._DumpMatrix('A_inv', numpy.linalg.inv(self.A)))
- ans.append(' return StateFeedbackController<%d, %d, %d>'
+ ans.append(' return StateFeedbackControllerConstants<%d, %d, %d>'
'(L, K, Kff, A_inv, Make%sPlantCoefficients());\n' % (
num_states, num_inputs, num_outputs, self._name))
ans.append('}\n')
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index a81f2cc..3f4a6ad 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -109,8 +109,8 @@
StateFeedbackPlant(
::std::vector<::std::unique_ptr<StateFeedbackPlantCoefficients<
- number_of_states, number_of_inputs, number_of_outputs>>> *
- coefficients)
+ number_of_states, number_of_inputs, number_of_outputs>>>
+ *coefficients)
: coefficients_(::std::move(*coefficients)), plant_index_(0) {
Reset();
}
@@ -120,7 +120,6 @@
::std::swap(coefficients_, other.coefficients_);
X_.swap(other.X_);
Y_.swap(other.Y_);
- U_.swap(other.U_);
}
virtual ~StateFeedbackPlant() {}
@@ -154,15 +153,11 @@
double X(int i, int j) const { return X()(i, j); }
const Eigen::Matrix<double, number_of_outputs, 1> &Y() const { return Y_; }
double Y(int i, int j) const { return Y()(i, j); }
- const Eigen::Matrix<double, number_of_inputs, 1> &U() const { return U_; }
- double U(int i, int j) const { return U()(i, j); }
Eigen::Matrix<double, number_of_states, 1> &mutable_X() { return X_; }
double &mutable_X(int i, int j) { return mutable_X()(i, j); }
Eigen::Matrix<double, number_of_outputs, 1> &mutable_Y() { return Y_; }
double &mutable_Y(int i, int j) { return mutable_Y()(i, j); }
- Eigen::Matrix<double, number_of_inputs, 1> &mutable_U() { return U_; }
- double &mutable_U(int i, int j) { return mutable_U()(i, j); }
const StateFeedbackPlantCoefficients<number_of_states, number_of_inputs,
number_of_outputs> &
@@ -180,24 +175,24 @@
void Reset() {
X_.setZero();
Y_.setZero();
- U_.setZero();
}
// Assert that U is within the hardware range.
- virtual void CheckU() {
+ virtual void CheckU(const Eigen::Matrix<double, number_of_inputs, 1> &U) {
for (int i = 0; i < kNumInputs; ++i) {
- assert(U(i, 0) <= U_max(i, 0) + 0.00001);
- assert(U(i, 0) >= U_min(i, 0) - 0.00001);
+ if (U(i, 0) > U_max(i, 0) + 0.00001 || U(i, 0) < U_min(i, 0) - 0.00001) {
+ LOG(FATAL, "U out of range\n");
+ }
}
}
// Computes the new X and Y given the control input.
- void Update() {
+ void Update(const Eigen::Matrix<double, number_of_inputs, 1> &U) {
// Powers outside of the range are more likely controller bugs than things
// that the plant should deal with.
- CheckU();
- X_ = A() * X() + B() * U();
- Y_ = C() * X() + D() * U();
+ CheckU(U);
+ X_ = A() * X() + B() * U;
+ Y_ = C() * X() + D() * U;
}
protected:
@@ -209,7 +204,6 @@
private:
Eigen::Matrix<double, number_of_states, 1> X_;
Eigen::Matrix<double, number_of_outputs, 1> Y_;
- Eigen::Matrix<double, number_of_inputs, 1> U_;
::std::vector<::std::unique_ptr<StateFeedbackPlantCoefficients<
number_of_states, number_of_inputs, number_of_outputs>>> coefficients_;
@@ -223,7 +217,7 @@
// This is designed such that multiple controllers can share one set of state to
// support gain scheduling easily.
template <int number_of_states, int number_of_inputs, int number_of_outputs>
-struct StateFeedbackController final {
+struct StateFeedbackControllerConstants final {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
const Eigen::Matrix<double, number_of_states, number_of_outputs> L;
@@ -231,9 +225,10 @@
const Eigen::Matrix<double, number_of_inputs, number_of_states> Kff;
const Eigen::Matrix<double, number_of_states, number_of_states> A_inv;
StateFeedbackPlantCoefficients<number_of_states, number_of_inputs,
- number_of_outputs> plant;
+ number_of_outputs>
+ plant;
- StateFeedbackController(
+ StateFeedbackControllerConstants(
const Eigen::Matrix<double, number_of_states, number_of_outputs> &L,
const Eigen::Matrix<double, number_of_inputs, number_of_states> &K,
const Eigen::Matrix<double, number_of_inputs, number_of_states> &Kff,
@@ -243,7 +238,7 @@
: L(L), K(K), Kff(Kff), A_inv(A_inv), plant(plant) {}
// TODO(Brian): Remove this overload once they're all converted.
- StateFeedbackController(
+ StateFeedbackControllerConstants(
const Eigen::Matrix<double, number_of_states, number_of_outputs> &L,
const Eigen::Matrix<double, number_of_inputs, number_of_states> &K,
const Eigen::Matrix<double, number_of_states, number_of_states> &A_inv,
@@ -262,17 +257,19 @@
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
- StateFeedbackLoop(const StateFeedbackController<
- number_of_states, number_of_inputs, number_of_outputs> &controller)
+ StateFeedbackLoop(
+ const StateFeedbackControllerConstants<number_of_states, number_of_inputs,
+ number_of_outputs> &controller)
: controller_index_(0) {
controllers_.emplace_back(
- new StateFeedbackController<number_of_states, number_of_inputs,
- number_of_outputs>(controller));
+ new StateFeedbackControllerConstants<number_of_states, number_of_inputs,
+ number_of_outputs>(controller));
Reset();
}
- StateFeedbackLoop(::std::vector<::std::unique_ptr<StateFeedbackController<
- number_of_states, number_of_inputs, number_of_outputs>>> *controllers)
+ StateFeedbackLoop(
+ ::std::vector<::std::unique_ptr<StateFeedbackControllerConstants<
+ number_of_states, number_of_inputs, number_of_outputs>>> *controllers)
: controllers_(::std::move(*controllers)), controller_index_(0) {
Reset();
}
@@ -371,15 +368,15 @@
return mutable_U_uncapped()(i, j);
}
- const StateFeedbackController<number_of_states, number_of_inputs,
- number_of_outputs> &
- controller() const {
+ const StateFeedbackControllerConstants<number_of_states, number_of_inputs,
+ number_of_outputs>
+ &controller() const {
return *controllers_[controller_index_];
}
- const StateFeedbackController<number_of_states, number_of_inputs,
- number_of_outputs> &
- controller(int index) const {
+ const StateFeedbackControllerConstants<number_of_states, number_of_inputs,
+ number_of_outputs>
+ &controller(int index) const {
return *controllers_[index];
}
@@ -466,8 +463,9 @@
int controller_index() const { return controller_index_; }
protected:
- ::std::vector<::std::unique_ptr<StateFeedbackController<
- number_of_states, number_of_inputs, number_of_outputs>>> controllers_;
+ ::std::vector<::std::unique_ptr<StateFeedbackControllerConstants<
+ number_of_states, number_of_inputs, number_of_outputs>>>
+ controllers_;
// These are accessible from non-templated subclasses.
static constexpr int kNumStates = number_of_states;
diff --git a/frc971/control_loops/state_feedback_loop_test.cc b/frc971/control_loops/state_feedback_loop_test.cc
index a08841e..5b8803a 100644
--- a/frc971/control_loops/state_feedback_loop_test.cc
+++ b/frc971/control_loops/state_feedback_loop_test.cc
@@ -26,25 +26,27 @@
::std::vector< ::std::unique_ptr<StateFeedbackPlantCoefficients<2, 4, 7>>> v;
v.emplace_back(new StateFeedbackPlantCoefficients<2, 4, 7>(coefficients));
StateFeedbackPlant<2, 4, 7> plant(&v);
- plant.Update();
+ plant.Update(Eigen::Matrix<double, 4, 1>::Zero());
plant.Reset();
- plant.CheckU();
+ plant.CheckU(Eigen::Matrix<double, 4, 1>::Zero());
}
{
- StateFeedbackLoop<2, 4, 7> test_loop(StateFeedbackController<2, 4, 7>(
- Eigen::Matrix<double, 2, 7>::Identity(),
- Eigen::Matrix<double, 4, 2>::Identity(),
- Eigen::Matrix<double, 4, 2>::Identity(),
- Eigen::Matrix<double, 2, 2>::Identity(), coefficients));
+ StateFeedbackLoop<2, 4, 7> test_loop(
+ StateFeedbackControllerConstants<2, 4, 7>(
+ Eigen::Matrix<double, 2, 7>::Identity(),
+ Eigen::Matrix<double, 4, 2>::Identity(),
+ Eigen::Matrix<double, 4, 2>::Identity(),
+ Eigen::Matrix<double, 2, 2>::Identity(), coefficients));
test_loop.Correct(Eigen::Matrix<double, 7, 1>::Identity());
test_loop.Update(false);
test_loop.CapU();
}
{
- StateFeedbackLoop<2, 4, 7> test_loop(StateFeedbackController<2, 4, 7>(
- Eigen::Matrix<double, 2, 7>::Identity(),
- Eigen::Matrix<double, 4, 2>::Identity(),
- Eigen::Matrix<double, 2, 2>::Identity(), coefficients));
+ StateFeedbackLoop<2, 4, 7> test_loop(
+ StateFeedbackControllerConstants<2, 4, 7>(
+ Eigen::Matrix<double, 2, 7>::Identity(),
+ Eigen::Matrix<double, 4, 2>::Identity(),
+ Eigen::Matrix<double, 2, 2>::Identity(), coefficients));
test_loop.Correct(Eigen::Matrix<double, 7, 1>::Identity());
test_loop.Update(false);
test_loop.CapU();