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();
diff --git a/y2014/control_loops/claw/claw_lib_test.cc b/y2014/control_loops/claw/claw_lib_test.cc
index 4079da4..d51049d 100644
--- a/y2014/control_loops/claw/claw_lib_test.cc
+++ b/y2014/control_loops/claw/claw_lib_test.cc
@@ -214,9 +214,10 @@
const constants::Values& v = constants::GetValues();
EXPECT_TRUE(claw_queue.output.FetchLatest());
- claw_plant_->mutable_U() << claw_queue.output->bottom_claw_voltage,
+ Eigen::Matrix<double, 2, 1> U;
+ U << claw_queue.output->bottom_claw_voltage,
claw_queue.output->top_claw_voltage;
- claw_plant_->Update();
+ claw_plant_->Update(U);
// Check that the claw is within the limits.
EXPECT_GE(v.claw.upper_claw.upper_limit, claw_plant_->Y(0, 0));
diff --git a/y2014/control_loops/shooter/shooter_lib_test.cc b/y2014/control_loops/shooter/shooter_lib_test.cc
index 9248725..5463265 100644
--- a/y2014/control_loops/shooter/shooter_lib_test.cc
+++ b/y2014/control_loops/shooter/shooter_lib_test.cc
@@ -221,14 +221,12 @@
}
if (brake_piston_state_) {
- shooter_plant_->mutable_U() << 0.0;
shooter_plant_->mutable_X(1, 0) = 0.0;
- shooter_plant_->mutable_Y() = shooter_plant_->C() * shooter_plant_->X() +
- shooter_plant_->D() * shooter_plant_->U();
+ shooter_plant_->mutable_Y() = shooter_plant_->C() * shooter_plant_->X();
} else {
- shooter_plant_->mutable_U() << last_voltage_;
- //shooter_plant_->U << shooter_queue_.output->voltage;
- shooter_plant_->Update();
+ Eigen::Matrix<double, 1, 1> U;
+ U << last_voltage_;
+ shooter_plant_->Update(U);
}
LOG(DEBUG, "Plant index is %d\n", shooter_plant_->plant_index());
diff --git a/y2015/control_loops/claw/claw_lib_test.cc b/y2015/control_loops/claw/claw_lib_test.cc
index ed82322..72f84b8 100644
--- a/y2015/control_loops/claw/claw_lib_test.cc
+++ b/y2015/control_loops/claw/claw_lib_test.cc
@@ -74,8 +74,9 @@
EXPECT_TRUE(claw_queue_.output.FetchLatest());
// Feed voltages into physics simulation.
- claw_plant_->mutable_U() << claw_queue_.output->voltage;
- claw_plant_->Update();
+ ::Eigen::Matrix<double, 1, 1> U;
+ U << claw_queue_.output->voltage;
+ claw_plant_->Update(U);
const double wrist_angle = claw_plant_->Y(0, 0);
diff --git a/y2015/control_loops/fridge/fridge_lib_test.cc b/y2015/control_loops/fridge/fridge_lib_test.cc
index 16a1f15..8f12730 100644
--- a/y2015/control_loops/fridge/fridge_lib_test.cc
+++ b/y2015/control_loops/fridge/fridge_lib_test.cc
@@ -133,23 +133,25 @@
void Simulate() {
EXPECT_TRUE(fridge_queue_.output.FetchLatest());
+ ::Eigen::Matrix<double, 2, 1> arm_U;
+ ::Eigen::Matrix<double, 2, 1> elevator_U;
// Feed voltages into physics simulation.
if (arm_power_error_ != 0.0) {
- arm_plant_->mutable_U() << ::aos::Clip(
+ arm_U << ::aos::Clip(
fridge_queue_.output->left_arm + arm_power_error_, -12, 12),
::aos::Clip(fridge_queue_.output->right_arm + arm_power_error_, -12,
12);
} else {
- arm_plant_->mutable_U() << fridge_queue_.output->left_arm,
+ arm_U << fridge_queue_.output->left_arm,
fridge_queue_.output->right_arm;
}
- elevator_plant_->mutable_U() << fridge_queue_.output->left_elevator,
+ elevator_U << fridge_queue_.output->left_elevator,
fridge_queue_.output->right_elevator;
// Use the plant to generate the next physical state given the voltages to
// the motors.
- arm_plant_->Update();
- elevator_plant_->Update();
+ arm_plant_->Update(arm_U);
+ elevator_plant_->Update(elevator_U);
const double left_arm_angle = arm_plant_->Y(0, 0);
const double right_arm_angle = arm_plant_->Y(1, 0);
diff --git a/y2015_bot3/control_loops/elevator/elevator_lib_test.cc b/y2015_bot3/control_loops/elevator/elevator_lib_test.cc
index 3455f1d..ba3d5a6 100644
--- a/y2015_bot3/control_loops/elevator/elevator_lib_test.cc
+++ b/y2015_bot3/control_loops/elevator/elevator_lib_test.cc
@@ -39,6 +39,7 @@
".y2015_bot3.control_loops.elevator_queue.status") {
// Initialize the elevator.
InitializePosition(kElevLowerLimit);
+ U_.setZero();
}
void InitializePosition(double start_pos) {
@@ -67,9 +68,9 @@
void Simulate() {
EXPECT_TRUE(queue_.output.FetchLatest());
- plant_->mutable_U() << queue_.output->elevator;
+ U_ << queue_.output->elevator;
- plant_->Update();
+ plant_->Update(U_);
plant_->mutable_X()(1, 0) += acceleration_offset_ * 0.005;
const double height = plant_->Y(0, 0);
@@ -82,10 +83,11 @@
void MoveTo(double position) { position_sim_.MoveTo(position); }
- double GetVoltage() const { return plant_->U()(0,0); }
+ double GetVoltage() const { return U_(0, 0); }
private:
::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> plant_;
+ ::Eigen::Matrix<double, 1, 1> U_;
PositionSensorSimulator position_sim_;
diff --git a/y2016/control_loops/shooter/shooter_lib_test.cc b/y2016/control_loops/shooter/shooter_lib_test.cc
index 93b3b17..5cd7ce5 100644
--- a/y2016/control_loops/shooter/shooter_lib_test.cc
+++ b/y2016/control_loops/shooter/shooter_lib_test.cc
@@ -28,9 +28,9 @@
explicit ShooterPlant(StateFeedbackPlant<2, 1, 1> &&other)
: StateFeedbackPlant<2, 1, 1>(::std::move(other)) {}
- void CheckU() override {
- assert(U(0, 0) <= U_max(0, 0) + 0.00001 + voltage_offset_);
- assert(U(0, 0) >= U_min(0, 0) - 0.00001 + voltage_offset_);
+ void CheckU(const ::Eigen::Matrix<double, 1, 1> &U) override {
+ EXPECT_LE(U(0, 0), U_max(0, 0) + 0.00001 + voltage_offset_);
+ EXPECT_GE(U(0, 0), U_min(0, 0) - 0.00001 + voltage_offset_);
}
double voltage_offset() const { return voltage_offset_; }
@@ -81,15 +81,15 @@
void Simulate() {
EXPECT_TRUE(shooter_queue_.output.FetchLatest());
- shooter_plant_left_->mutable_U(0, 0) =
- shooter_queue_.output->voltage_left +
- shooter_plant_left_->voltage_offset();
- shooter_plant_right_->mutable_U(0, 0) =
- shooter_queue_.output->voltage_right +
- shooter_plant_right_->voltage_offset();
+ ::Eigen::Matrix<double, 1, 1> U_left;
+ ::Eigen::Matrix<double, 1, 1> U_right;
+ U_left(0, 0) = shooter_queue_.output->voltage_left +
+ shooter_plant_left_->voltage_offset();
+ U_right(0, 0) = shooter_queue_.output->voltage_right +
+ shooter_plant_right_->voltage_offset();
- shooter_plant_left_->Update();
- shooter_plant_right_->Update();
+ shooter_plant_left_->Update(U_left);
+ shooter_plant_right_->Update(U_right);
}
::std::unique_ptr<ShooterPlant> shooter_plant_left_, shooter_plant_right_;
diff --git a/y2016/control_loops/superstructure/superstructure_lib_test.cc b/y2016/control_loops/superstructure/superstructure_lib_test.cc
index e95267c..b583078 100644
--- a/y2016/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2016/control_loops/superstructure/superstructure_lib_test.cc
@@ -33,11 +33,11 @@
explicit ArmPlant(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 + shoulder_voltage_offset_);
- assert(U(0, 0) >= U_min(0, 0) - 0.00001 + shoulder_voltage_offset_);
- assert(U(1, 0) <= U_max(1, 0) + 0.00001 + wrist_voltage_offset_);
- assert(U(1, 0) >= U_min(1, 0) - 0.00001 + wrist_voltage_offset_);
+ void CheckU(const ::Eigen::Matrix<double, 2, 1> &U) override {
+ EXPECT_LT(U(0, 0), U_max(0, 0) + 0.00001 + shoulder_voltage_offset_);
+ EXPECT_GT(U(0, 0), U_min(0, 0) - 0.00001 + shoulder_voltage_offset_);
+ EXPECT_LT(U(1, 0), U_max(1, 0) + 0.00001 + wrist_voltage_offset_);
+ EXPECT_GT(U(1, 0), U_min(1, 0) - 0.00001 + wrist_voltage_offset_);
}
double shoulder_voltage_offset() const { return shoulder_voltage_offset_; }
@@ -60,10 +60,10 @@
explicit IntakePlant(StateFeedbackPlant<2, 1, 1> &&other)
: StateFeedbackPlant<2, 1, 1>(::std::move(other)) {}
- void CheckU() override {
+ void CheckU(const ::Eigen::Matrix<double, 1, 1> &U) override {
for (int i = 0; i < kNumInputs; ++i) {
- assert(U(i, 0) <= U_max(i, 0) + 0.00001 + voltage_offset_);
- assert(U(i, 0) >= U_min(i, 0) - 0.00001 + voltage_offset_);
+ EXPECT_LE(U(i, 0), U_max(i, 0) + 0.00001 + voltage_offset_);
+ EXPECT_GE(U(i, 0), U_min(i, 0) - 0.00001 + voltage_offset_);
}
}
@@ -158,11 +158,13 @@
EXPECT_TRUE(superstructure_queue_.output.FetchLatest());
// Feed voltages into physics simulation.
- intake_plant_->mutable_U() << superstructure_queue_.output->voltage_intake +
- intake_plant_->voltage_offset();
+ ::Eigen::Matrix<double, 1, 1> intake_U;
+ ::Eigen::Matrix<double, 2, 1> arm_U;
+ intake_U << superstructure_queue_.output->voltage_intake +
+ intake_plant_->voltage_offset();
- arm_plant_->mutable_U() << superstructure_queue_.output->voltage_shoulder +
- arm_plant_->shoulder_voltage_offset(),
+ arm_U << superstructure_queue_.output->voltage_shoulder +
+ arm_plant_->shoulder_voltage_offset(),
superstructure_queue_.output->voltage_wrist +
arm_plant_->wrist_voltage_offset();
@@ -194,15 +196,15 @@
// Use the plant to generate the next physical state given the voltages to
// the motors.
- intake_plant_->Update();
+ intake_plant_->Update(intake_U);
{
const double bemf_voltage = arm_plant_->X(1, 0) / kV_shoulder;
bool is_accelerating = false;
if (bemf_voltage > 0) {
- is_accelerating = arm_plant_->U(0, 0) > bemf_voltage;
+ is_accelerating = arm_U(0, 0) > bemf_voltage;
} else {
- is_accelerating = arm_plant_->U(0, 0) < bemf_voltage;
+ is_accelerating = arm_U(0, 0) < bemf_voltage;
}
if (is_accelerating) {
arm_plant_->set_plant_index(0);
@@ -210,7 +212,7 @@
arm_plant_->set_plant_index(1);
}
}
- arm_plant_->Update();
+ arm_plant_->Update(arm_U);
const double angle_intake = intake_plant_->Y(0, 0);
const double angle_shoulder = arm_plant_->Y(0, 0);
diff --git a/y2017/control_loops/superstructure/superstructure_lib_test.cc b/y2017/control_loops/superstructure/superstructure_lib_test.cc
index 3075f03..579fb1e 100644
--- a/y2017/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2017/control_loops/superstructure/superstructure_lib_test.cc
@@ -35,7 +35,7 @@
explicit ShooterPlant(StateFeedbackPlant<2, 1, 1> &&other)
: StateFeedbackPlant<2, 1, 1>(::std::move(other)) {}
- void CheckU() override {
+ void CheckU(const Eigen::Matrix<double, 1, 1> &U) override {
EXPECT_LE(U(0, 0), U_max(0, 0) + 0.00001 + voltage_offset_);
EXPECT_GE(U(0, 0), U_min(0, 0) - 0.00001 + voltage_offset_);
}
@@ -54,7 +54,7 @@
explicit IndexerPlant(StateFeedbackPlant<2, 1, 1> &&other)
: StateFeedbackPlant<2, 1, 1>(::std::move(other)) {}
- void CheckU() override {
+ void CheckU(const ::Eigen::Matrix<double, 1, 1> &U) override {
EXPECT_LE(U(0, 0), U_max(0, 0) + 0.00001 + voltage_offset_);
EXPECT_GE(U(0, 0), U_min(0, 0) - 0.00001 + voltage_offset_);
}
@@ -73,7 +73,7 @@
explicit HoodPlant(StateFeedbackPlant<2, 1, 1> &&other)
: StateFeedbackPlant<2, 1, 1>(::std::move(other)) {}
- void CheckU() override {
+ void CheckU(const ::Eigen::Matrix<double, 1, 1> &U) override {
EXPECT_LE(U(0, 0), U_max(0, 0) + 0.00001 + voltage_offset_);
EXPECT_GE(U(0, 0), U_min(0, 0) - 0.00001 + voltage_offset_);
}
@@ -92,7 +92,7 @@
explicit TurretPlant(StateFeedbackPlant<2, 1, 1> &&other)
: StateFeedbackPlant<2, 1, 1>(::std::move(other)) {}
- void CheckU() override {
+ void CheckU(const ::Eigen::Matrix<double, 1, 1> &U) override {
EXPECT_LE(U(0, 0), U_max(0, 0) + 0.00001 + voltage_offset_);
EXPECT_GE(U(0, 0), U_min(0, 0) - 0.00001 + voltage_offset_);
}
@@ -111,7 +111,7 @@
explicit IntakePlant(StateFeedbackPlant<2, 1, 1> &&other)
: StateFeedbackPlant<2, 1, 1>(::std::move(other)) {}
- void CheckU() override {
+ void CheckU(const ::Eigen::Matrix<double, 1, 1> &U) override {
EXPECT_LE(U(0, 0), U_max(0, 0) + 0.00001 + voltage_offset_);
EXPECT_GE(U(0, 0), U_min(0, 0) - 0.00001 + voltage_offset_);
}
@@ -284,31 +284,34 @@
CHECK_LE(::std::abs(superstructure_queue_.output->voltage_intake),
voltage_check_intake);
- hood_plant_->mutable_U() << superstructure_queue_.output->voltage_hood +
- hood_plant_->voltage_offset();
+ ::Eigen::Matrix<double, 1, 1> hood_U;
+ hood_U << superstructure_queue_.output->voltage_hood +
+ hood_plant_->voltage_offset();
- turret_plant_->mutable_U() << superstructure_queue_.output->voltage_turret +
- turret_plant_->voltage_offset();
+ ::Eigen::Matrix<double, 1, 1> turret_U;
+ turret_U << superstructure_queue_.output->voltage_turret +
+ turret_plant_->voltage_offset();
- intake_plant_->mutable_U() << superstructure_queue_.output->voltage_intake +
- intake_plant_->voltage_offset();
+ ::Eigen::Matrix<double, 1, 1> intake_U;
+ intake_U << superstructure_queue_.output->voltage_intake +
+ intake_plant_->voltage_offset();
- shooter_plant_->mutable_U()
- << superstructure_queue_.output->voltage_shooter +
- shooter_plant_->voltage_offset();
+ ::Eigen::Matrix<double, 1, 1> shooter_U;
+ shooter_U << superstructure_queue_.output->voltage_shooter +
+ shooter_plant_->voltage_offset();
- indexer_plant_->mutable_U()
- << superstructure_queue_.output->voltage_indexer +
- indexer_plant_->voltage_offset();
+ ::Eigen::Matrix<double, 1, 1> indexer_U;
+ indexer_U << superstructure_queue_.output->voltage_indexer +
+ indexer_plant_->voltage_offset();
- hood_plant_->Update();
- turret_plant_->Update();
- intake_plant_->Update();
- shooter_plant_->Update();
+ hood_plant_->Update(hood_U);
+ turret_plant_->Update(turret_U);
+ intake_plant_->Update(intake_U);
+ shooter_plant_->Update(shooter_U);
if (freeze_indexer_) {
indexer_plant_->mutable_X(1, 0) = 0.0;
} else {
- indexer_plant_->Update();
+ indexer_plant_->Update(indexer_U);
}
double angle_hood = hood_plant_->Y(0, 0);