Put a plant inside the loop and moved A,B,etc there.
Change-Id: I9cb3a1a16bd0ccda0c9287514577b3c3861bc42f
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 6a2822b..2e680a3 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -112,14 +112,14 @@
break;
}
- kf_.set_controller_index(ControllerIndexFromGears());
+ kf_.set_index(ControllerIndexFromGears());
{
GearLogging gear_logging;
gear_logging.left_state = static_cast<uint32_t>(left_gear_);
gear_logging.right_state = static_cast<uint32_t>(right_gear_);
gear_logging.left_loop_high = MaybeHigh(left_gear_);
gear_logging.right_loop_high = MaybeHigh(right_gear_);
- gear_logging.controller_index = kf_.controller_index();
+ gear_logging.controller_index = kf_.index();
LOG_STRUCT(DEBUG, "state", gear_logging);
}
const bool is_latest_imu_values = ::frc971::imu_values.FetchLatest();
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 41a4f3c..c66a4e3 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -166,15 +166,15 @@
if (left_gear_high_) {
if (right_gear_high_) {
- drivetrain_plant_->set_plant_index(3);
+ drivetrain_plant_->set_index(3);
} else {
- drivetrain_plant_->set_plant_index(2);
+ drivetrain_plant_->set_index(2);
}
} else {
if (right_gear_high_) {
- drivetrain_plant_->set_plant_index(1);
+ drivetrain_plant_->set_index(1);
} else {
- drivetrain_plant_->set_plant_index(0);
+ drivetrain_plant_->set_index(0);
}
}
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.cc b/frc971/control_loops/drivetrain/polydrivetrain.cc
index d3a220b..5fc4b17 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.cc
+++ b/frc971/control_loops/drivetrain/polydrivetrain.cc
@@ -1,16 +1,16 @@
#include "frc971/control_loops/drivetrain/polydrivetrain.h"
-#include "aos/common/logging/logging.h"
-#include "aos/common/controls/polytope.h"
#include "aos/common/commonmath.h"
-#include "aos/common/logging/queue_logging.h"
+#include "aos/common/controls/polytope.h"
+#include "aos/common/logging/logging.h"
#include "aos/common/logging/matrix_logging.h"
+#include "aos/common/logging/queue_logging.h"
#include "aos/common/messages/robot_state.q.h"
-#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/control_loops/coerce_goal.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/state_feedback_loop.h"
namespace frc971 {
namespace control_loops {
@@ -22,13 +22,16 @@
U_Poly_((Eigen::Matrix<double, 4, 2>() << /*[[*/ 1, 0 /*]*/,
/*[*/ -1, 0 /*]*/,
/*[*/ 0, 1 /*]*/,
- /*[*/ 0, -1 /*]]*/).finished(),
+ /*[*/ 0, -1 /*]]*/)
+ .finished(),
(Eigen::Matrix<double, 4, 1>() << /*[[*/ 12 /*]*/,
/*[*/ 12 /*]*/,
/*[*/ 12 /*]*/,
- /*[*/ 12 /*]]*/).finished(),
+ /*[*/ 12 /*]]*/)
+ .finished(),
(Eigen::Matrix<double, 2, 4>() << /*[[*/ 12, 12, -12, -12 /*]*/,
- /*[*/ -12, 12, 12, -12 /*]*/).finished()),
+ /*[*/ -12, 12, 12, -12 /*]*/)
+ .finished()),
loop_(new StateFeedbackLoop<2, 2, 2>(dt_config.make_v_drivetrain_loop())),
ttrust_(1.1),
wheel_(0.0),
@@ -148,14 +151,14 @@
double PolyDrivetrain::FilterVelocity(double throttle) const {
const Eigen::Matrix<double, 2, 2> FF =
- loop_->B().inverse() *
- (Eigen::Matrix<double, 2, 2>::Identity() - loop_->A());
+ loop_->plant().B().inverse() *
+ (Eigen::Matrix<double, 2, 2>::Identity() - loop_->plant().A());
constexpr int kHighGearController = 3;
const Eigen::Matrix<double, 2, 2> FF_high =
- loop_->controller(kHighGearController).plant.B.inverse() *
+ loop_->plant().coefficients(kHighGearController).B.inverse() *
(Eigen::Matrix<double, 2, 2>::Identity() -
- loop_->controller(kHighGearController).plant.A);
+ loop_->plant().coefficients(kHighGearController).A);
::Eigen::Matrix<double, 1, 2> FF_sum = FF.colwise().sum();
int min_FF_sum_index;
@@ -173,14 +176,14 @@
double PolyDrivetrain::MaxVelocity() {
const Eigen::Matrix<double, 2, 2> FF =
- loop_->B().inverse() *
- (Eigen::Matrix<double, 2, 2>::Identity() - loop_->A());
+ loop_->plant().B().inverse() *
+ (Eigen::Matrix<double, 2, 2>::Identity() - loop_->plant().A());
constexpr int kHighGearController = 3;
const Eigen::Matrix<double, 2, 2> FF_high =
- loop_->controller(kHighGearController).plant.B.inverse() *
+ loop_->plant().coefficients(kHighGearController).B.inverse() *
(Eigen::Matrix<double, 2, 2>::Identity() -
- loop_->controller(kHighGearController).plant.A);
+ loop_->plant().coefficients(kHighGearController).A);
::Eigen::Matrix<double, 1, 2> FF_sum = FF.colwise().sum();
int min_FF_sum_index;
@@ -206,8 +209,8 @@
if (IsInGear(left_gear_) && IsInGear(right_gear_)) {
// FF * X = U (steady state)
const Eigen::Matrix<double, 2, 2> FF =
- loop_->B().inverse() *
- (Eigen::Matrix<double, 2, 2>::Identity() - loop_->A());
+ loop_->plant().B().inverse() *
+ (Eigen::Matrix<double, 2, 2>::Identity() - loop_->plant().A());
// Invert the plant to figure out how the velocity filter would have to
// work
@@ -262,7 +265,7 @@
if (dt_config_.loop_type == LoopType::OPEN_LOOP) {
loop_->mutable_X_hat() =
- loop_->A() * loop_->X_hat() + loop_->B() * loop_->U();
+ loop_->plant().A() * loop_->X_hat() + loop_->plant().B() * loop_->U();
}
} else {
const double current_left_velocity =
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.h b/frc971/control_loops/drivetrain/polydrivetrain.h
index 20f1a49..570b87f 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.h
+++ b/frc971/control_loops/drivetrain/polydrivetrain.h
@@ -17,7 +17,7 @@
PolyDrivetrain(const DrivetrainConfig &dt_config,
StateFeedbackLoop<7, 2, 3> *kf);
- int controller_index() const { return loop_->controller_index(); }
+ int controller_index() const { return loop_->index(); }
// Computes the speed of the motor given the hall effect position and the
// speed of the robot.
diff --git a/frc971/control_loops/profiled_subsystem.h b/frc971/control_loops/profiled_subsystem.h
index 031795a..91e7ae7 100644
--- a/frc971/control_loops/profiled_subsystem.h
+++ b/frc971/control_loops/profiled_subsystem.h
@@ -60,7 +60,7 @@
return *loop_;
}
- int controller_index() const { return loop_->controller_index(); }
+ int controller_index() const { return loop_->index(); }
// Returns whether the estimators have been initialized and zeroed.
bool initialized() const { return initialized_; }
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index fba883c..e8900af 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -158,7 +158,8 @@
fd.write(' controllers[%d] = ::std::unique_ptr<%s>(new %s(%s));\n' %
(index, self._ControllerType(), self._ControllerType(),
loop.ControllerFunction()))
- fd.write(' return %s(&controllers);\n' % self._LoopType())
+ fd.write(' return %s(Make%sPlant(), &controllers);\n' %
+ (self._LoopType(), self._gain_schedule_name))
fd.write('}\n\n')
fd.write(self._namespace_end)
@@ -276,6 +277,7 @@
num_states, num_inputs, num_outputs, self._name)]
ans.append(self._DumpMatrix('A', self.A))
+ ans.append(self._DumpMatrix('A_inv', numpy.linalg.inv(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))
@@ -285,7 +287,7 @@
ans.append(self._DumpMatrix('U_min', self.U_min))
ans.append(' return StateFeedbackPlantCoefficients<%d, %d, %d>'
- '(A, A_continuous, B, B_continuous, C, D, U_max, U_min);\n' % (
+ '(A, A_inv, A_continuous, B, B_continuous, C, D, U_max, U_min);\n' % (
num_states, num_inputs, num_outputs))
ans.append('}\n')
return ''.join(ans)
@@ -328,10 +330,9 @@
self.Kff = numpy.matrix(numpy.zeros(self.K.shape))
ans.append(self._DumpMatrix('Kff', self.Kff))
- ans.append(self._DumpMatrix('A_inv', numpy.linalg.inv(self.A)))
ans.append(' return StateFeedbackControllerConstants<%d, %d, %d>'
- '(L, K, Kff, A_inv, Make%sPlantCoefficients());\n' % (
- num_states, num_inputs, num_outputs, self._name))
+ '(L, K, Kff);\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 3b935e2..3ed1a9e 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -3,9 +3,10 @@
#include <assert.h>
-#include <vector>
-#include <memory>
#include <iostream>
+#include <memory>
+#include <utility>
+#include <vector>
#include "Eigen/Dense"
@@ -26,6 +27,7 @@
StateFeedbackPlantCoefficients(const StateFeedbackPlantCoefficients &other)
: A(other.A),
+ A_inv(other.A_inv),
A_continuous(other.A_continuous),
B(other.B),
B_continuous(other.B_continuous),
@@ -36,6 +38,7 @@
StateFeedbackPlantCoefficients(
const Eigen::Matrix<double, number_of_states, number_of_states> &A,
+ const Eigen::Matrix<double, number_of_states, number_of_states> &A_inv,
const Eigen::Matrix<double, number_of_states, number_of_states>
&A_continuous,
const Eigen::Matrix<double, number_of_states, number_of_inputs> &B,
@@ -46,6 +49,7 @@
const Eigen::Matrix<double, number_of_inputs, 1> &U_max,
const Eigen::Matrix<double, number_of_inputs, 1> &U_min)
: A(A),
+ A_inv(A_inv),
A_continuous(A_continuous),
B(B),
B_continuous(B_continuous),
@@ -55,6 +59,7 @@
U_max(U_max) {}
const Eigen::Matrix<double, number_of_states, number_of_states> A;
+ const Eigen::Matrix<double, number_of_states, number_of_states> A_inv;
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;
@@ -73,12 +78,12 @@
::std::vector<::std::unique_ptr<StateFeedbackPlantCoefficients<
number_of_states, number_of_inputs, number_of_outputs>>>
*coefficients)
- : coefficients_(::std::move(*coefficients)), plant_index_(0) {
+ : coefficients_(::std::move(*coefficients)), index_(0) {
Reset();
}
StateFeedbackPlant(StateFeedbackPlant &&other)
- : plant_index_(other.plant_index_) {
+ : index_(other.index_) {
::std::swap(coefficients_, other.coefficients_);
X_.swap(other.X_);
Y_.swap(other.Y_);
@@ -90,6 +95,10 @@
return coefficients().A;
}
double A(int i, int j) const { return A()(i, j); }
+ const Eigen::Matrix<double, number_of_states, number_of_states> &A_inv() const {
+ return coefficients().A_inv;
+ }
+ double A_inv(int i, int j) const { return A_inv()(i, j); }
const Eigen::Matrix<double, number_of_states, number_of_inputs> &B() const {
return coefficients().B;
}
@@ -122,16 +131,22 @@
double &mutable_Y(int i, int j) { return mutable_Y()(i, j); }
const StateFeedbackPlantCoefficients<number_of_states, number_of_inputs,
- number_of_outputs> &
- coefficients() const {
- return *coefficients_[plant_index_];
+ number_of_outputs>
+ &coefficients(int index) const {
+ return *coefficients_[index];
}
- int plant_index() const { return plant_index_; }
- void set_plant_index(int plant_index) {
- assert(plant_index >= 0);
- assert(plant_index < static_cast<int>(coefficients_.size()));
- plant_index_ = plant_index;
+ const StateFeedbackPlantCoefficients<number_of_states, number_of_inputs,
+ number_of_outputs>
+ &coefficients() const {
+ return *coefficients_[index_];
+ }
+
+ int index() const { return index_; }
+ void set_index(int index) {
+ assert(index >= 0);
+ assert(index < static_cast<int>(coefficients_.size()));
+ index_ = index;
}
void Reset() {
@@ -171,7 +186,7 @@
number_of_states, number_of_inputs, number_of_outputs>>>
coefficients_;
- int plant_index_;
+ int index_;
DISALLOW_COPY_AND_ASSIGN(StateFeedbackPlant);
};
@@ -186,19 +201,12 @@
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;
- const Eigen::Matrix<double, number_of_states, number_of_states> A_inv;
- StateFeedbackPlantCoefficients<number_of_states, number_of_inputs,
- number_of_outputs>
- plant;
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,
- const Eigen::Matrix<double, number_of_states, number_of_states> &A_inv,
- const StateFeedbackPlantCoefficients<number_of_states, number_of_inputs,
- number_of_outputs> &plant)
- : L(L), K(K), Kff(Kff), A_inv(A_inv), plant(plant) {}
+ const Eigen::Matrix<double, number_of_inputs, number_of_states> &Kff)
+ : L(L), K(K), Kff(Kff) {}
};
template <int number_of_states, int number_of_inputs, int number_of_outputs>
@@ -207,13 +215,18 @@
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
StateFeedbackLoop(
+ StateFeedbackPlant<number_of_states, number_of_inputs, number_of_outputs>
+ &&plant,
::std::vector<::std::unique_ptr<StateFeedbackControllerConstants<
number_of_states, number_of_inputs, number_of_outputs>>> *controllers)
- : controllers_(::std::move(*controllers)), controller_index_(0) {
+ : plant_(::std::move(plant)),
+ controllers_(::std::move(*controllers)),
+ index_(0) {
Reset();
}
- StateFeedbackLoop(StateFeedbackLoop &&other) {
+ StateFeedbackLoop(StateFeedbackLoop &&other)
+ : plant_(::std::move(other.plant_)) {
X_hat_.swap(other.X_hat_);
R_.swap(other.R_);
next_R_.swap(other.next_R_);
@@ -221,41 +234,11 @@
U_uncapped_.swap(other.U_uncapped_);
ff_U_.swap(other.ff_U_);
::std::swap(controllers_, other.controllers_);
- controller_index_ = other.controller_index_;
+ index_ = other.index_;
}
virtual ~StateFeedbackLoop() {}
- const Eigen::Matrix<double, number_of_states, number_of_states> &A() const {
- return controller().plant.A;
- }
- double A(int i, int j) const { return A()(i, j); }
- const Eigen::Matrix<double, number_of_states, number_of_inputs> &B() const {
- return controller().plant.B;
- }
- const Eigen::Matrix<double, number_of_states, number_of_states> &A_inv()
- const {
- return controller().A_inv;
- }
- double A_inv(int i, int j) const { return A_inv()(i, j); }
- double B(int i, int j) const { return B()(i, j); }
- const Eigen::Matrix<double, number_of_outputs, number_of_states> &C() const {
- return controller().plant.C;
- }
- double C(int i, int j) const { return C()(i, j); }
- const Eigen::Matrix<double, number_of_outputs, number_of_inputs> &D() const {
- return controller().plant.D;
- }
- double D(int i, int j) const { return D()(i, j); }
- const Eigen::Matrix<double, number_of_inputs, 1> &U_min() const {
- return controller().plant.U_min;
- }
- double U_min(int i, int j) const { return U_min()(i, j); }
- const Eigen::Matrix<double, number_of_inputs, 1> &U_max() const {
- return controller().plant.U_max;
- }
- double U_max(int i, int j) const { return U_max()(i, j); }
-
const Eigen::Matrix<double, number_of_inputs, number_of_states> &K() const {
return controller().K;
}
@@ -307,10 +290,16 @@
return mutable_U_uncapped()(i, j);
}
+ const StateFeedbackPlant<number_of_states, number_of_inputs,
+ number_of_outputs>
+ &plant() const {
+ return plant_;
+ }
+
const StateFeedbackControllerConstants<number_of_states, number_of_inputs,
number_of_outputs>
&controller() const {
- return *controllers_[controller_index_];
+ return *controllers_[index_];
}
const StateFeedbackControllerConstants<number_of_states, number_of_inputs,
@@ -332,17 +321,18 @@
// it.
virtual void CapU() {
for (int i = 0; i < kNumInputs; ++i) {
- if (U(i, 0) > U_max(i, 0)) {
- U_(i, 0) = U_max(i, 0);
- } else if (U(i, 0) < U_min(i, 0)) {
- U_(i, 0) = U_min(i, 0);
+ if (U(i, 0) > plant().U_max(i, 0)) {
+ U_(i, 0) = plant().U_max(i, 0);
+ } else if (U(i, 0) < plant().U_min(i, 0)) {
+ U_(i, 0) = plant().U_min(i, 0);
}
}
}
// Corrects X_hat given the observation in Y.
void Correct(const Eigen::Matrix<double, number_of_outputs, 1> &Y) {
- X_hat_ += A_inv() * L() * (Y - C() * X_hat_ - D() * U());
+ X_hat_ +=
+ plant().A_inv() * L() * (Y - plant().C() * X_hat_ - plant().D() * U());
}
const Eigen::Matrix<double, number_of_states, 1> error() const {
@@ -357,7 +347,7 @@
// Calculates the feed forwards power.
virtual const Eigen::Matrix<double, number_of_inputs, 1> FeedForward() {
- return Kff() * (next_R() - A() * R());
+ return Kff() * (next_R() - plant().A() * R());
}
// stop_motors is whether or not to output all 0s.
@@ -380,28 +370,32 @@
void UpdateFFReference() {
ff_U_ -= U_uncapped() - U();
if (!Kff().isZero(0)) {
- R_ = A() * R() + B() * ff_U_;
+ R_ = plant().A() * R() + plant().B() * ff_U_;
}
}
void UpdateObserver(const Eigen::Matrix<double, number_of_inputs, 1> &new_u) {
- X_hat_ = A() * X_hat() + B() * new_u;
+ X_hat_ = plant().A() * X_hat() + plant().B() * new_u;
}
// Sets the current controller to be index, clamped to be within range.
- void set_controller_index(int index) {
+ void set_index(int index) {
if (index < 0) {
- controller_index_ = 0;
+ index_ = 0;
} else if (index >= static_cast<int>(controllers_.size())) {
- controller_index_ = static_cast<int>(controllers_.size()) - 1;
+ index_ = static_cast<int>(controllers_.size()) - 1;
} else {
- controller_index_ = index;
+ index_ = index;
}
+ plant_.set_index(index);
}
- int controller_index() const { return controller_index_; }
+ int index() const { return index_; }
protected:
+ StateFeedbackPlant<number_of_states, number_of_inputs, number_of_outputs>
+ plant_;
+
::std::vector<::std::unique_ptr<StateFeedbackControllerConstants<
number_of_states, number_of_inputs, number_of_outputs>>>
controllers_;
@@ -426,7 +420,7 @@
// Computed output before being capped.
Eigen::Matrix<double, number_of_inputs, 1> U_uncapped_;
- int controller_index_;
+ int index_;
DISALLOW_COPY_AND_ASSIGN(StateFeedbackLoop);
};
diff --git a/frc971/control_loops/state_feedback_loop_test.cc b/frc971/control_loops/state_feedback_loop_test.cc
index 47434e4..e81f6fd 100644
--- a/frc971/control_loops/state_feedback_loop_test.cc
+++ b/frc971/control_loops/state_feedback_loop_test.cc
@@ -15,6 +15,7 @@
const StateFeedbackPlantCoefficients<2, 4, 7> coefficients(
Eigen::Matrix<double, 2, 2>::Identity(),
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(),
@@ -22,27 +23,27 @@
Eigen::Matrix<double, 4, 1>::Constant(1),
Eigen::Matrix<double, 4, 1>::Constant(-1));
- {
- ::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(Eigen::Matrix<double, 4, 1>::Zero());
- plant.Reset();
- plant.CheckU(Eigen::Matrix<double, 4, 1>::Zero());
- }
- {
- ::std::vector<::std::unique_ptr<StateFeedbackControllerConstants<2, 4, 7>>>
- v;
- v.emplace_back(new 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));
- StateFeedbackLoop<2, 4, 7> test_loop(&v);
- test_loop.Correct(Eigen::Matrix<double, 7, 1>::Identity());
- test_loop.Update(false);
- test_loop.CapU();
- }
+ // Build a plant.
+ ::std::vector<::std::unique_ptr<StateFeedbackPlantCoefficients<2, 4, 7>>>
+ v_plant;
+ v_plant.emplace_back(
+ new StateFeedbackPlantCoefficients<2, 4, 7>(coefficients));
+ StateFeedbackPlant<2, 4, 7> plant(&v_plant);
+ plant.Update(Eigen::Matrix<double, 4, 1>::Zero());
+ plant.Reset();
+ plant.CheckU(Eigen::Matrix<double, 4, 1>::Zero());
+
+ // Now build a controller.
+ ::std::vector<::std::unique_ptr<StateFeedbackControllerConstants<2, 4, 7>>>
+ v_loop;
+ v_loop.emplace_back(new StateFeedbackControllerConstants<2, 4, 7>(
+ Eigen::Matrix<double, 2, 7>::Identity(),
+ Eigen::Matrix<double, 4, 2>::Identity(),
+ Eigen::Matrix<double, 4, 2>::Identity()));
+ StateFeedbackLoop<2, 4, 7> test_loop(::std::move(plant), &v_loop);
+ test_loop.Correct(Eigen::Matrix<double, 7, 1>::Identity());
+ test_loop.Update(false);
+ test_loop.CapU();
}
} // namespace testing
diff --git a/y2014/actors/drivetrain_actor.cc b/y2014/actors/drivetrain_actor.cc
index 7ab4f3d..172ca13 100644
--- a/y2014/actors/drivetrain_actor.cc
+++ b/y2014/actors/drivetrain_actor.cc
@@ -24,7 +24,7 @@
DrivetrainActor::DrivetrainActor(actors::DrivetrainActionQueueGroup* s)
: aos::common::actions::ActorBase<actors::DrivetrainActionQueueGroup>(s),
loop_(constants::GetValues().make_drivetrain_loop()) {
- loop_.set_controller_index(3);
+ loop_.set_index(3);
}
bool DrivetrainActor::RunAction(const actors::DrivetrainActionParams ¶ms) {
diff --git a/y2014/control_loops/shooter/shooter.cc b/y2014/control_loops/shooter/shooter.cc
index af310f0..9c6b182 100644
--- a/y2014/control_loops/shooter/shooter.cc
+++ b/y2014/control_loops/shooter/shooter.cc
@@ -56,11 +56,11 @@
void ZeroedStateFeedbackLoop::CapGoal() {
if (uncapped_voltage() > max_voltage_) {
double dx;
- if (controller_index() == 0) {
+ if (index() == 0) {
dx = (uncapped_voltage() - max_voltage_) /
- (K(0, 0) - A(1, 0) * K(0, 2) / A(1, 2));
+ (K(0, 0) - plant().A(1, 0) * K(0, 2) / plant().A(1, 2));
mutable_R(0, 0) -= dx;
- mutable_R(2, 0) -= -A(1, 0) / A(1, 2) * dx;
+ mutable_R(2, 0) -= -plant().A(1, 0) / plant().A(1, 2) * dx;
} else {
dx = (uncapped_voltage() - max_voltage_) / K(0, 0);
mutable_R(0, 0) -= dx;
@@ -70,11 +70,11 @@
::y2014::control_loops::ShooterMovingGoal(dx));
} else if (uncapped_voltage() < -max_voltage_) {
double dx;
- if (controller_index() == 0) {
+ if (index() == 0) {
dx = (uncapped_voltage() + max_voltage_) /
- (K(0, 0) - A(1, 0) * K(0, 2) / A(1, 2));
+ (K(0, 0) - plant().A(1, 0) * K(0, 2) / plant().A(1, 2));
mutable_R(0, 0) -= dx;
- mutable_R(2, 0) -= -A(1, 0) / A(1, 2) * dx;
+ mutable_R(2, 0) -= -plant().A(1, 0) / plant().A(1, 2) * dx;
} else {
dx = (uncapped_voltage() + max_voltage_) / K(0, 0);
mutable_R(0, 0) -= dx;
@@ -88,10 +88,11 @@
}
void ZeroedStateFeedbackLoop::RecalculatePowerGoal() {
- if (controller_index() == 0) {
- mutable_R(2, 0) = (-A(1, 0) / A(1, 2) * R(0, 0) - A(1, 1) / A(1, 2) * R(1, 0));
+ if (index() == 0) {
+ mutable_R(2, 0) = (-plant().A(1, 0) / plant().A(1, 2) * R(0, 0) -
+ plant().A(1, 1) / plant().A(1, 2) * R(1, 0));
} else {
- mutable_R(2, 0) = -A(1, 1) / A(1, 2) * R(1, 0);
+ mutable_R(2, 0) = -plant().A(1, 1) / plant().A(1, 2) * R(1, 0);
}
}
@@ -104,8 +105,8 @@
mutable_X_hat(0, 0) += doffset;
// Offset the goal so we don't move.
mutable_R(0, 0) += doffset;
- if (controller_index() == 0) {
- mutable_R(2, 0) += -A(1, 0) / A(1, 2) * (doffset);
+ if (index() == 0) {
+ mutable_R(2, 0) += -plant().A(1, 0) / plant().A(1, 2) * (doffset);
}
LOG_STRUCT(DEBUG, "sensor edge (fake?)",
::y2014::control_loops::ShooterChangeCalibration(
@@ -257,16 +258,16 @@
// Probably not needed yet.
if (position) {
- int last_controller_index = shooter_.controller_index();
+ int last_index = shooter_.index();
if (position->plunger && position->latch) {
// Use the controller without the spring if the latch is set and the
// plunger is back
- shooter_.set_controller_index(1);
+ shooter_.set_index(1);
} else {
// Otherwise use the controller with the spring.
- shooter_.set_controller_index(0);
+ shooter_.set_index(0);
}
- if (shooter_.controller_index() != last_controller_index) {
+ if (shooter_.index() != last_index) {
shooter_.RecalculatePowerGoal();
}
}
diff --git a/y2014/control_loops/shooter/shooter.h b/y2014/control_loops/shooter/shooter.h
index fb991e3..f2ddbc8 100644
--- a/y2014/control_loops/shooter/shooter.h
+++ b/y2014/control_loops/shooter/shooter.h
@@ -83,8 +83,9 @@
desired_velocity);
mutable_R() << desired_position - kPositionOffset, desired_velocity,
- (-A(1, 0) / A(1, 2) * (desired_position - kPositionOffset) -
- A(1, 1) / A(1, 2) * desired_velocity);
+ (-plant().A(1, 0) / plant().A(1, 2) *
+ (desired_position - kPositionOffset) -
+ plant().A(1, 1) / plant().A(1, 2) * desired_velocity);
}
double position() const { return X_hat(0, 0) - offset_ + kPositionOffset; }
diff --git a/y2014/control_loops/shooter/shooter_lib_test.cc b/y2014/control_loops/shooter/shooter_lib_test.cc
index 5463265..df3f5fd 100644
--- a/y2014/control_loops/shooter/shooter_lib_test.cc
+++ b/y2014/control_loops/shooter/shooter_lib_test.cc
@@ -102,12 +102,12 @@
// Only disengage the spring if we are greater than 0, which is where the
// latch will take the load off the pusher.
if (GetAbsolutePosition() > 0.0) {
- shooter_plant_->set_plant_index(1);
+ shooter_plant_->set_index(1);
} else {
- shooter_plant_->set_plant_index(0);
+ shooter_plant_->set_index(0);
}
} else {
- shooter_plant_->set_plant_index(0);
+ shooter_plant_->set_index(0);
position->plunger =
CheckRange(GetAbsolutePosition(), values.shooter.plunger_back);
}
@@ -228,7 +228,7 @@
U << last_voltage_;
shooter_plant_->Update(U);
}
- LOG(DEBUG, "Plant index is %d\n", shooter_plant_->plant_index());
+ LOG(DEBUG, "Plant index is %d\n", shooter_plant_->index());
// Handle latch hall effect
if (!latch_piston_state_ && latch_delay_count_ > 0) {
diff --git a/y2016/control_loops/superstructure/superstructure_controls.h b/y2016/control_loops/superstructure/superstructure_controls.h
index 6f853d6..95f04f8 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.h
+++ b/y2016/control_loops/superstructure/superstructure_controls.h
@@ -28,12 +28,12 @@
const Eigen::Matrix<double, 2, 1> ControllerOutput() override {
const Eigen::Matrix<double, 2, 1> accelerating_ff =
- controller(0).Kff * (next_R() - controller(0).plant.A * R());
+ controller(0).Kff * (next_R() - plant().coefficients(0).A * R());
const Eigen::Matrix<double, 2, 1> accelerating_controller =
controller(0).K * error() + accelerating_ff;
const Eigen::Matrix<double, 2, 1> decelerating_ff =
- controller(1).Kff * (next_R() - controller(1).plant.A * R());
+ controller(1).Kff * (next_R() - plant().coefficients(1).A * R());
const Eigen::Matrix<double, 2, 1> decelerating_controller =
controller(1).K * error() + decelerating_ff;
@@ -48,11 +48,11 @@
use_accelerating_controller = false;
}
if (use_accelerating_controller) {
+ set_index(0);
ff_U_ = accelerating_ff;
- set_controller_index(0);
return accelerating_controller;
} else {
- set_controller_index(1);
+ set_index(1);
ff_U_ = decelerating_ff;
return decelerating_controller;
}
@@ -67,7 +67,7 @@
const double overage_amount = U(0, 0) - max_voltage(0);
mutable_U(0, 0) = max_voltage(0);
const double coupled_amount =
- (Kff().block<1, 2>(1, 2) * B().block<2, 1>(2, 0))(0, 0) *
+ (Kff().block<1, 2>(1, 2) * plant().B().block<2, 1>(2, 0))(0, 0) *
overage_amount;
LOG(DEBUG, "Removing coupled amount %f\n", coupled_amount);
mutable_U(1, 0) += coupled_amount;
@@ -76,7 +76,7 @@
const double under_amount = U(0, 0) - min_voltage(0);
mutable_U(0, 0) = min_voltage(0);
const double coupled_amount =
- (Kff().block<1, 2>(1, 2) * B().block<2, 1>(2, 0))(0, 0) *
+ (Kff().block<1, 2>(1, 2) * plant().B().block<2, 1>(2, 0))(0, 0) *
under_amount;
LOG(DEBUG, "Removing coupled amount %f\n", coupled_amount);
mutable_U(1, 0) += coupled_amount;
diff --git a/y2016/control_loops/superstructure/superstructure_lib_test.cc b/y2016/control_loops/superstructure/superstructure_lib_test.cc
index b583078..b3e980d 100644
--- a/y2016/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2016/control_loops/superstructure/superstructure_lib_test.cc
@@ -207,9 +207,9 @@
is_accelerating = arm_U(0, 0) < bemf_voltage;
}
if (is_accelerating) {
- arm_plant_->set_plant_index(0);
+ arm_plant_->set_index(0);
} else {
- arm_plant_->set_plant_index(1);
+ arm_plant_->set_index(1);
}
}
arm_plant_->Update(arm_U);