Split StatespaceLoop into a Plant, Controller, and Observer.
This doesn't yet move any of the logic out of the Loop.
Change-Id: I2cb0ea6d1a75c7011576ba752c50e512eeff5890
diff --git a/y2014/control_loops/claw/claw.cc b/y2014/control_loops/claw/claw.cc
index 9320e6b..86ec084 100644
--- a/y2014/control_loops/claw/claw.cc
+++ b/y2014/control_loops/claw/claw.cc
@@ -106,11 +106,11 @@
// (H * position_K) * position_error <= k - H * UVel
Eigen::Matrix<double, 2, 2> position_K;
- position_K << K(0, 0), K(0, 1),
- K(1, 0), K(1, 1);
+ position_K << controller().K(0, 0), controller().K(0, 1),
+ controller().K(1, 0), controller().K(1, 1);
Eigen::Matrix<double, 2, 2> velocity_K;
- velocity_K << K(0, 2), K(0, 3),
- K(1, 2), K(1, 3);
+ velocity_K << controller().K(0, 2), controller().K(0, 3),
+ controller().K(1, 2), controller().K(1, 3);
Eigen::Matrix<double, 2, 1> position_error;
position_error << error(0, 0), error(1, 0);
@@ -924,19 +924,19 @@
case FINE_TUNE_TOP:
case UNKNOWN_LOCATION: {
if (claw_.uncapped_average_voltage() > values.claw.max_zeroing_voltage) {
- double dx_bot = (claw_.U_uncapped(0, 0) -
- values.claw.max_zeroing_voltage) /
- claw_.K(0, 0);
- double dx_top = (claw_.U_uncapped(1, 0) -
- values.claw.max_zeroing_voltage) /
- claw_.K(0, 0);
+ double dx_bot =
+ (claw_.U_uncapped(0, 0) - values.claw.max_zeroing_voltage) /
+ claw_.controller().K(0, 0);
+ double dx_top =
+ (claw_.U_uncapped(1, 0) - values.claw.max_zeroing_voltage) /
+ claw_.controller().K(0, 0);
double dx = ::std::max(dx_top, dx_bot);
bottom_claw_goal_ -= dx;
top_claw_goal_ -= dx;
Eigen::Matrix<double, 4, 1> R;
R << bottom_claw_goal_, top_claw_goal_ - bottom_claw_goal_, claw_.R(2, 0),
claw_.R(3, 0);
- claw_.mutable_U() = claw_.K() * (R - claw_.X_hat());
+ claw_.mutable_U() = claw_.controller().K() * (R - claw_.X_hat());
capped_goal_ = true;
LOG(DEBUG, "Moving the goal by %f to prevent windup."
" Uncapped is %f, max is %f, difference is %f\n",
@@ -946,19 +946,19 @@
values.claw.max_zeroing_voltage));
} else if (claw_.uncapped_average_voltage() <
-values.claw.max_zeroing_voltage) {
- double dx_bot = (claw_.U_uncapped(0, 0) +
- values.claw.max_zeroing_voltage) /
- claw_.K(0, 0);
- double dx_top = (claw_.U_uncapped(1, 0) +
- values.claw.max_zeroing_voltage) /
- claw_.K(0, 0);
+ double dx_bot =
+ (claw_.U_uncapped(0, 0) + values.claw.max_zeroing_voltage) /
+ claw_.controller().K(0, 0);
+ double dx_top =
+ (claw_.U_uncapped(1, 0) + values.claw.max_zeroing_voltage) /
+ claw_.controller().K(0, 0);
double dx = ::std::min(dx_top, dx_bot);
bottom_claw_goal_ -= dx;
top_claw_goal_ -= dx;
Eigen::Matrix<double, 4, 1> R;
R << bottom_claw_goal_, top_claw_goal_ - bottom_claw_goal_, claw_.R(2, 0),
claw_.R(3, 0);
- claw_.mutable_U() = claw_.K() * (R - claw_.X_hat());
+ claw_.mutable_U() = claw_.controller().K() * (R - claw_.X_hat());
capped_goal_ = true;
LOG(DEBUG, "Moving the goal by %f to prevent windup\n", dx);
}
diff --git a/y2014/control_loops/claw/claw_lib_test.cc b/y2014/control_loops/claw/claw_lib_test.cc
index d51049d..8444cea 100644
--- a/y2014/control_loops/claw/claw_lib_test.cc
+++ b/y2014/control_loops/claw/claw_lib_test.cc
@@ -542,7 +542,8 @@
claw_motor_.top_claw_goal_ - claw_motor_.bottom_claw_goal_, 0.0,
0.0;
Eigen::Matrix<double, 2, 1> uncapped_voltage =
- claw_motor_.claw_.K() * (R - claw_motor_.claw_.X_hat());
+ claw_motor_.claw_.controller().K() *
+ (R - claw_motor_.claw_.X_hat());
// Use a factor of 1.8 because so long as it isn't actually running
// away, the CapU function will deal with getting the actual output
// down.
diff --git a/y2014/control_loops/shooter/shooter.cc b/y2014/control_loops/shooter/shooter.cc
index 9c6b182..70a6cad 100644
--- a/y2014/control_loops/shooter/shooter.cc
+++ b/y2014/control_loops/shooter/shooter.cc
@@ -58,11 +58,12 @@
double dx;
if (index() == 0) {
dx = (uncapped_voltage() - max_voltage_) /
- (K(0, 0) - plant().A(1, 0) * K(0, 2) / plant().A(1, 2));
+ (controller().K(0, 0) -
+ plant().A(1, 0) * controller().K(0, 2) / plant().A(1, 2));
mutable_R(0, 0) -= dx;
mutable_R(2, 0) -= -plant().A(1, 0) / plant().A(1, 2) * dx;
} else {
- dx = (uncapped_voltage() - max_voltage_) / K(0, 0);
+ dx = (uncapped_voltage() - max_voltage_) / controller().K(0, 0);
mutable_R(0, 0) -= dx;
}
capped_goal_ = true;
@@ -72,11 +73,12 @@
double dx;
if (index() == 0) {
dx = (uncapped_voltage() + max_voltage_) /
- (K(0, 0) - plant().A(1, 0) * K(0, 2) / plant().A(1, 2));
+ (controller().K(0, 0) -
+ plant().A(1, 0) * controller().K(0, 2) / plant().A(1, 2));
mutable_R(0, 0) -= dx;
mutable_R(2, 0) -= -plant().A(1, 0) / plant().A(1, 2) * dx;
} else {
- dx = (uncapped_voltage() + max_voltage_) / K(0, 0);
+ dx = (uncapped_voltage() + max_voltage_) / controller().K(0, 0);
mutable_R(0, 0) -= dx;
}
capped_goal_ = true;