cleaned up the control loop code
There were a lot of mutable public members, which are against the style
guide (and they were in the wrong place declaration order-wise).
diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
index 44ec4e8..80f471c 100644
--- a/frc971/control_loops/claw/claw.cc
+++ b/frc971/control_loops/claw/claw.cc
@@ -79,17 +79,17 @@
// constrains the region such that, if at all possible, it will maintain its
// current efforts to reduce velocity error.
void ClawLimitedLoop::CapU() {
- const Eigen::Matrix<double, 4, 1> error = R - X_hat;
+ const Eigen::Matrix<double, 4, 1> error = R() - X_hat();
- double u_top = U(1, 0);
- double u_bottom = U(0, 0);
+ double u_top = U(1);
+ double u_bottom = U(0);
uncapped_average_voltage_ = (u_top + u_bottom) / 2;
double max_voltage = is_zeroing_ ? kZeroingVoltage : kMaxVoltage;
if (::std::abs(u_bottom) > max_voltage || ::std::abs(u_top) > max_voltage) {
- LOG_MATRIX(DEBUG, "U at start", U);
+ LOG_MATRIX(DEBUG, "U at start", U());
// H * U <= k
// U = UPos + UVel
// H * (UPos + UVel) <= k
@@ -129,7 +129,7 @@
// If the top claw is above its soft upper limit, make the line actually
// 45 degrees to avoid smashing it into the limit in an attempt to fix the
// separation error faster than the bottom position one.
- if (X_hat(0, 0) + X_hat(1, 0) >
+ if (X_hat(0) + X_hat(1) >
constants::GetValues().claw.upper_claw.upper_limit) {
angle_45 << 1, 1;
} else {
@@ -177,29 +177,28 @@
}
LOG_MATRIX(DEBUG, "adjusted_pos_error", adjusted_pos_error);
- U = velocity_K * velocity_error + position_K * adjusted_pos_error;
- LOG_MATRIX(DEBUG, "U is now", U);
+ change_U() = velocity_K * velocity_error + position_K * adjusted_pos_error;
+ LOG_MATRIX(DEBUG, "U is now", U());
{
const auto values = constants::GetValues().claw;
if (top_known_) {
- if (X_hat(0, 0) + X_hat(1, 0) > values.upper_claw.upper_limit &&
- U(1, 0) > 0) {
+ if (X_hat(0) + X_hat(1) > values.upper_claw.upper_limit && U(1) > 0) {
LOG(WARNING, "upper claw too high and moving up\n");
- U(1, 0) = 0;
- } else if (X_hat(0, 0) + X_hat(1, 0) < values.upper_claw.lower_limit &&
- U(1, 0) < 0) {
+ change_U(1) = 0;
+ } else if (X_hat(0) + X_hat(1) < values.upper_claw.lower_limit &&
+ U(1) < 0) {
LOG(WARNING, "upper claw too low and moving down\n");
- U(1, 0) = 0;
+ change_U(1) = 0;
}
}
if (bottom_known_) {
- if (X_hat(0, 0) > values.lower_claw.upper_limit && U(0, 0) > 0) {
+ if (X_hat(0) > values.lower_claw.upper_limit && U(0) > 0) {
LOG(WARNING, "lower claw too high and moving up\n");
- U(0, 0) = 0;
- } else if (X_hat(0, 0) < values.lower_claw.lower_limit && U(0, 0) < 0) {
+ change_U(0) = 0;
+ } else if (X_hat(0) < values.lower_claw.lower_limit && U(0) < 0) {
LOG(WARNING, "lower claw too low and moving down\n");
- U(0, 0) = 0;
+ change_U(0) = 0;
}
}
}
@@ -505,14 +504,14 @@
}
void ClawLimitedLoop::ChangeTopOffset(double doffset) {
- Y_(1, 0) += doffset;
- X_hat(1, 0) += doffset;
+ change_Y()(1) += doffset;
+ change_X_hat()(1) += doffset;
LOG(INFO, "Changing top offset by %f\n", doffset);
}
void ClawLimitedLoop::ChangeBottomOffset(double doffset) {
- Y_(0, 0) += doffset;
- X_hat(0, 0) += doffset;
- X_hat(1, 0) -= doffset;
+ change_Y()(0) += doffset;
+ change_X_hat()(0) += doffset;
+ change_X_hat()(1) -= doffset;
LOG(INFO, "Changing bottom offset by %f\n", doffset);
}
@@ -859,11 +858,14 @@
LimitClawGoal(&bottom_claw_goal_, &top_claw_goal_, values);
}
- claw_.set_positions_known(top_claw_.zeroing_state() != ZeroedStateFeedbackLoop::UNKNOWN_POSITION, bottom_claw_.zeroing_state() != ZeroedStateFeedbackLoop::UNKNOWN_POSITION);
+ claw_.set_positions_known(
+ top_claw_.zeroing_state() != ZeroedStateFeedbackLoop::UNKNOWN_POSITION,
+ bottom_claw_.zeroing_state() !=
+ ZeroedStateFeedbackLoop::UNKNOWN_POSITION);
if (has_top_claw_goal_ && has_bottom_claw_goal_) {
- claw_.R << bottom_claw_goal_, top_claw_goal_ - bottom_claw_goal_,
+ claw_.change_R() << bottom_claw_goal_, top_claw_goal_ - bottom_claw_goal_,
bottom_claw_velocity_, top_claw_velocity_ - bottom_claw_velocity_;
- LOG_MATRIX(DEBUG, "actual goal", claw_.R);
+ LOG_MATRIX(DEBUG, "actual goal", claw_.R());
// Only cap power when one of the halves of the claw is moving slowly and
// could wind up.
@@ -884,18 +886,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) -
+ double dx_bot = (claw_.U_uncapped(0) -
values.claw.max_zeroing_voltage) /
claw_.K(0, 0);
- double dx_top = (claw_.U_uncapped(1, 0) -
+ double dx_top = (claw_.U_uncapped(1) -
values.claw.max_zeroing_voltage) /
claw_.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_.U = claw_.K() * (R - claw_.X_hat);
+ R << bottom_claw_goal_, top_claw_goal_ - bottom_claw_goal_, claw_.R(2),
+ claw_.R(3);
+ claw_.change_U() = claw_.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",
@@ -905,18 +908,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) +
+ double dx_bot = (claw_.U_uncapped(0) +
values.claw.max_zeroing_voltage) /
claw_.K(0, 0);
- double dx_top = (claw_.U_uncapped(1, 0) +
+ double dx_top = (claw_.U_uncapped(1) +
values.claw.max_zeroing_voltage) /
claw_.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_.U = claw_.K() * (R - claw_.X_hat);
+ R << bottom_claw_goal_, top_claw_goal_ - bottom_claw_goal_, claw_.R(2),
+ claw_.R(3);
+ claw_.change_U() = claw_.K() * (R - claw_.X_hat());
capped_goal_ = true;
LOG(DEBUG, "Moving the goal by %f to prevent windup\n", dx);
}
@@ -935,8 +939,8 @@
? -12.0
: goal->centering;
}
- output->top_claw_voltage = claw_.U(1, 0);
- output->bottom_claw_voltage = claw_.U(0, 0);
+ output->top_claw_voltage = claw_.U(1);
+ output->bottom_claw_voltage = claw_.U(0);
if (output->top_claw_voltage > kMaxVoltage) {
output->top_claw_voltage = kMaxVoltage;
@@ -953,8 +957,8 @@
status->bottom = bottom_absolute_position();
status->separation = top_absolute_position() - bottom_absolute_position();
- status->bottom_velocity = claw_.X_hat(2, 0);
- status->separation_velocity = claw_.X_hat(3, 0);
+ status->bottom_velocity = claw_.X_hat(2);
+ status->separation_velocity = claw_.X_hat(3);
if (goal) {
bool bottom_done =
diff --git a/frc971/control_loops/claw/claw.h b/frc971/control_loops/claw/claw.h
index 12f7264..f444bbe 100644
--- a/frc971/control_loops/claw/claw.h
+++ b/frc971/control_loops/claw/claw.h
@@ -224,9 +224,9 @@
control_loops::ClawGroup::Status *status);
double top_absolute_position() const {
- return claw_.X_hat(1, 0) + claw_.X_hat(0, 0);
+ return claw_.X_hat(1) + claw_.X_hat(0);
}
- double bottom_absolute_position() const { return claw_.X_hat(0, 0); }
+ double bottom_absolute_position() const { return claw_.X_hat(0); }
private:
// Friend the test classes for acces to the internal state.
diff --git a/frc971/control_loops/claw/claw_lib_test.cc b/frc971/control_loops/claw/claw_lib_test.cc
index 0882085..7d1fbc6 100644
--- a/frc971/control_loops/claw/claw_lib_test.cc
+++ b/frc971/control_loops/claw/claw_lib_test.cc
@@ -54,11 +54,11 @@
double initial_bottom_position) {
LOG(INFO, "Reinitializing to {top: %f, bottom: %f}\n", initial_top_position,
initial_bottom_position);
- claw_plant_->X(0, 0) = initial_bottom_position;
- claw_plant_->X(1, 0) = initial_top_position - initial_bottom_position;
- claw_plant_->X(2, 0) = 0.0;
- claw_plant_->X(3, 0) = 0.0;
- claw_plant_->Y = claw_plant_->C() * claw_plant_->X;
+ claw_plant_->change_X(0) = initial_bottom_position;
+ claw_plant_->change_X(1) = initial_top_position - initial_bottom_position;
+ claw_plant_->change_X(2) = 0.0;
+ claw_plant_->change_X(3) = 0.0;
+ claw_plant_->change_Y() = claw_plant_->C() * claw_plant_->X();
ReinitializePartial(TOP_CLAW, initial_top_position);
ReinitializePartial(BOTTOM_CLAW, initial_bottom_position);
@@ -69,9 +69,9 @@
// Returns the absolute angle of the wrist.
double GetAbsolutePosition(ClawType type) const {
if (type == TOP_CLAW) {
- return claw_plant_->Y(1, 0);
+ return claw_plant_->Y(1);
} else {
- return claw_plant_->Y(0, 0);
+ return claw_plant_->Y(0);
}
}
@@ -214,20 +214,20 @@
const frc971::constants::Values& v = constants::GetValues();
EXPECT_TRUE(claw_queue_group.output.FetchLatest());
- claw_plant_->U << claw_queue_group.output->bottom_claw_voltage,
+ claw_plant_->change_U() << claw_queue_group.output->bottom_claw_voltage,
claw_queue_group.output->top_claw_voltage;
claw_plant_->Update();
// Check that the claw is within the limits.
- EXPECT_GE(v.claw.upper_claw.upper_limit, claw_plant_->Y(0, 0));
- EXPECT_LE(v.claw.upper_claw.lower_hard_limit, claw_plant_->Y(0, 0));
+ EXPECT_GE(v.claw.upper_claw.upper_limit, claw_plant_->Y(0));
+ EXPECT_LE(v.claw.upper_claw.lower_hard_limit, claw_plant_->Y(0));
- EXPECT_GE(v.claw.lower_claw.upper_hard_limit, claw_plant_->Y(1, 0));
- EXPECT_LE(v.claw.lower_claw.lower_hard_limit, claw_plant_->Y(1, 0));
+ EXPECT_GE(v.claw.lower_claw.upper_hard_limit, claw_plant_->Y(1));
+ EXPECT_LE(v.claw.lower_claw.lower_hard_limit, claw_plant_->Y(1));
- EXPECT_LE(claw_plant_->Y(1, 0) - claw_plant_->Y(0, 0),
+ EXPECT_LE(claw_plant_->Y(1) - claw_plant_->Y(0),
v.claw.claw_max_separation);
- EXPECT_GE(claw_plant_->Y(1, 0) - claw_plant_->Y(0, 0),
+ EXPECT_GE(claw_plant_->Y(1) - claw_plant_->Y(0),
v.claw.claw_min_separation);
}
// The whole claw.
@@ -556,7 +556,7 @@
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_.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/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index c815b8d..2bf5621 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -47,11 +47,11 @@
private:
virtual void CapU() {
- const Eigen::Matrix<double, 4, 1> error = R - X_hat;
+ const Eigen::Matrix<double, 4, 1> error = R() - X_hat();
- if (::std::abs(U(0, 0)) > 12.0 || ::std::abs(U(1, 0)) > 12.0) {
+ if (::std::abs(U(0)) > 12.0 || ::std::abs(U(1)) > 12.0) {
output_was_capped_ = true;
- LOG_MATRIX(DEBUG, "U at start", U);
+ LOG_MATRIX(DEBUG, "U at start", U());
Eigen::Matrix<double, 2, 2> position_K;
position_K << K(0, 0), K(0, 2),
@@ -68,7 +68,8 @@
LOG_MATRIX(DEBUG, "error", error);
const auto &poly = U_Poly_;
- const Eigen::Matrix<double, 4, 2> pos_poly_H = poly.H() * position_K * T;
+ const Eigen::Matrix<double, 4, 2> pos_poly_H =
+ poly.H() * position_K * T;
const Eigen::Matrix<double, 4, 1> pos_poly_k =
poly.k() - poly.H() * velocity_K * velocity_error;
const ::aos::controls::HPolytope<2> pos_poly(pos_poly_H, pos_poly_k);
@@ -93,7 +94,8 @@
standard << L45, LH;
Eigen::Matrix<double, 2, 1> W;
W << w45, wh;
- const Eigen::Matrix<double, 2, 1> intersection = standard.inverse() * W;
+ const Eigen::Matrix<double, 2, 1> intersection =
+ standard.inverse() * W;
bool is_inside_h;
const auto adjusted_pos_error_h =
@@ -116,8 +118,9 @@
}
LOG_MATRIX(DEBUG, "adjusted_pos_error", adjusted_pos_error);
- U = velocity_K * velocity_error + position_K * T * adjusted_pos_error;
- LOG_MATRIX(DEBUG, "U is now", U);
+ change_U() =
+ velocity_K * velocity_error + position_K * T * adjusted_pos_error;
+ LOG_MATRIX(DEBUG, "U is now", U());
} else {
output_was_capped_ = false;
}
@@ -145,7 +148,7 @@
double right_velocity) {
left_goal_ = left;
right_goal_ = right;
- loop_->R << left, left_velocity, right, right_velocity;
+ loop_->change_R() << left, left_velocity, right, right_velocity;
}
void SetRawPosition(double left, double right) {
raw_right_ = right;
@@ -164,7 +167,7 @@
}
void SetExternalMotors(double left_voltage, double right_voltage) {
- loop_->U << left_voltage, right_voltage;
+ loop_->change_U() << left_voltage, right_voltage;
}
void Update(bool stop_motors, bool enable_control_loop) {
@@ -172,26 +175,26 @@
loop_->Update(stop_motors);
} else {
if (stop_motors) {
- loop_->U.setZero();
- loop_->U_uncapped.setZero();
+ loop_->change_U().setZero();
+ loop_->change_U_uncapped().setZero();
}
loop_->UpdateObserver();
}
- ::Eigen::Matrix<double, 4, 1> E = loop_->R - loop_->X_hat;
+ ::Eigen::Matrix<double, 4, 1> E = loop_->R() - loop_->X_hat();
LOG_MATRIX(DEBUG, "E", E);
}
double GetEstimatedRobotSpeed() const {
// lets just call the average of left and right velocities close enough
- return (loop_->X_hat(1, 0) + loop_->X_hat(3, 0)) / 2;
+ return (loop_->X_hat(1) + loop_->X_hat(3)) / 2;
}
double GetEstimatedLeftEncoder() const {
- return loop_->X_hat(0, 0);
+ return loop_->X_hat(0);
}
double GetEstimatedRightEncoder() const {
- return loop_->X_hat(2, 0);
+ return loop_->X_hat(2);
}
bool OutputWasCapped() const {
@@ -200,8 +203,8 @@
void SendMotors(Drivetrain::Output *output) const {
if (output) {
- output->left_voltage = loop_->U(0, 0);
- output->right_voltage = loop_->U(1, 0);
+ output->left_voltage = loop_->U(0);
+ output->right_voltage = loop_->U(1);
output->left_high = false;
output->right_high = false;
}
@@ -455,9 +458,9 @@
constexpr int kHighGearController = 3;
const Eigen::Matrix<double, 2, 2> FF_high =
- loop_->controller(kHighGearController).plant.B.inverse() *
+ loop_->controller(kHighGearController).plant.B().inverse() *
(Eigen::Matrix<double, 2, 2>::Identity() -
- loop_->controller(kHighGearController).plant.A);
+ loop_->controller(kHighGearController).plant.A());
::Eigen::Matrix<double, 1, 2> FF_sum = FF.colwise().sum();
int min_FF_sum_index;
@@ -468,8 +471,7 @@
const double adjusted_ff_voltage = ::aos::Clip(
throttle * 12.0 * min_FF_sum / high_min_FF_sum, -12.0, 12.0);
return ((adjusted_ff_voltage +
- ttrust_ * min_K_sum * (loop_->X_hat(0, 0) + loop_->X_hat(1, 0)) /
- 2.0) /
+ ttrust_ * min_K_sum * (loop_->X_hat(0) + loop_->X_hat(1)) / 2.0) /
(ttrust_ * min_K_sum + min_FF_sum));
}
@@ -480,9 +482,9 @@
constexpr int kHighGearController = 3;
const Eigen::Matrix<double, 2, 2> FF_high =
- loop_->controller(kHighGearController).plant.B.inverse() *
+ loop_->controller(kHighGearController).plant.B().inverse() *
(Eigen::Matrix<double, 2, 2>::Identity() -
- loop_->controller(kHighGearController).plant.A);
+ loop_->controller(kHighGearController).plant.A());
::Eigen::Matrix<double, 1, 2> FF_sum = FF.colwise().sum();
int min_FF_sum_index;
@@ -562,7 +564,7 @@
// Integrate velocity to get the position.
// This position is used to get integral control.
- loop_->R << left_velocity, right_velocity;
+ loop_->change_R() << left_velocity, right_velocity;
if (!quickturn_) {
// K * R = w
@@ -573,23 +575,25 @@
// Construct a constraint on R by manipulating the constraint on U
::aos::controls::HPolytope<2> R_poly = ::aos::controls::HPolytope<2>(
U_Poly_.H() * (loop_->K() + FF),
- U_Poly_.k() + U_Poly_.H() * loop_->K() * loop_->X_hat);
+ U_Poly_.k() + U_Poly_.H() * loop_->K() * loop_->X_hat());
// Limit R back inside the box.
- loop_->R = CoerceGoal(R_poly, equality_k, equality_w, loop_->R);
+ loop_->change_R() =
+ CoerceGoal(R_poly, equality_k, equality_w, loop_->R());
}
- const Eigen::Matrix<double, 2, 1> FF_volts = FF * loop_->R;
+ const Eigen::Matrix<double, 2, 1> FF_volts = FF * loop_->R();
const Eigen::Matrix<double, 2, 1> U_ideal =
- loop_->K() * (loop_->R - loop_->X_hat) + FF_volts;
+ loop_->K() * (loop_->R() - loop_->X_hat()) + FF_volts;
for (int i = 0; i < 2; i++) {
- loop_->U[i] = ::aos::Clip(U_ideal[i], -12, 12);
+ loop_->change_U()[i] = ::aos::Clip(U_ideal[i], -12, 12);
}
// TODO(austin): Model this better.
// TODO(austin): Feed back?
- loop_->X_hat = loop_->A() * loop_->X_hat + loop_->B() * loop_->U;
+ loop_->change_X_hat() =
+ loop_->A() * loop_->X_hat() + loop_->B() * loop_->U();
} else {
// Any motor is not in gear. Speed match.
::Eigen::Matrix<double, 1, 1> R_left;
@@ -600,20 +604,20 @@
const double wiggle =
(static_cast<double>((counter_ % 20) / 10) - 0.5) * 5.0;
- loop_->U(0, 0) = ::aos::Clip(
+ loop_->change_U(0) = ::aos::Clip(
(R_left / Kv)(0, 0) + (IsInGear(left_gear_) ? 0 : wiggle),
-12.0, 12.0);
- loop_->U(1, 0) = ::aos::Clip(
+ loop_->change_U(1) = ::aos::Clip(
(R_right / Kv)(0, 0) + (IsInGear(right_gear_) ? 0 : wiggle),
-12.0, 12.0);
- loop_->U *= 12.0 / position_.battery_voltage;
+ loop_->change_U() *= 12.0 / position_.battery_voltage;
}
}
void SendMotors(Drivetrain::Output *output) {
if (output != NULL) {
- output->left_voltage = loop_->U(0, 0);
- output->right_voltage = loop_->U(1, 0);
+ output->left_voltage = loop_->U(0);
+ output->right_voltage = loop_->U(1);
output->left_high = left_gear_ == HIGH || left_gear_ == SHIFTING_UP;
output->right_high = right_gear_ == HIGH || right_gear_ == SHIFTING_UP;
}
@@ -718,8 +722,8 @@
status->filtered_left_position = dt_closedloop.GetEstimatedLeftEncoder();
status->filtered_right_position = dt_closedloop.GetEstimatedRightEncoder();
status->output_was_capped = dt_closedloop.OutputWasCapped();
- status->uncapped_left_voltage = dt_closedloop.loop().U_uncapped(0, 0);
- status->uncapped_right_voltage = dt_closedloop.loop().U_uncapped(1, 0);
+ status->uncapped_left_voltage = dt_closedloop.loop().U_uncapped(0);
+ status->uncapped_right_voltage = dt_closedloop.loop().U_uncapped(1);
}
}
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 1983caa..a307388 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -40,8 +40,8 @@
::testing::Environment* const team_number_env =
::testing::AddGlobalTestEnvironment(new TeamNumberEnvironment);
-// Class which simulates the drivetrain and sends out queue messages containing the
-// position.
+// Class which simulates the drivetrain and sends out queue messages containing
+// the position.
class DrivetrainSimulation {
public:
// Constructs a motor simulation.
@@ -59,20 +59,17 @@
// Resets the plant.
void Reinitialize() {
- drivetrain_plant_->X(0, 0) = 0.0;
- drivetrain_plant_->X(1, 0) = 0.0;
- drivetrain_plant_->Y = drivetrain_plant_->C() * drivetrain_plant_->X;
- last_left_position_ = drivetrain_plant_->Y(0, 0);
- last_right_position_ = drivetrain_plant_->Y(1, 0);
+ drivetrain_plant_->change_X(0) = 0.0;
+ drivetrain_plant_->change_X(1) = 0.0;
+ drivetrain_plant_->change_Y() =
+ drivetrain_plant_->C() * drivetrain_plant_->X();
+ last_left_position_ = drivetrain_plant_->Y(0);
+ last_right_position_ = drivetrain_plant_->Y(1);
}
// Returns the position of the drivetrain.
- double GetLeftPosition() const {
- return drivetrain_plant_->Y(0, 0);
- }
- double GetRightPosition() const {
- return drivetrain_plant_->Y(1, 0);
- }
+ double GetLeftPosition() const { return drivetrain_plant_->Y(0); }
+ double GetRightPosition() const { return drivetrain_plant_->Y(1); }
// Sends out the position queue messages.
void SendPositionMessage() {
@@ -88,11 +85,11 @@
// Simulates the drivetrain moving for one timestep.
void Simulate() {
- last_left_position_ = drivetrain_plant_->Y(0, 0);
- last_right_position_ = drivetrain_plant_->Y(1, 0);
+ last_left_position_ = drivetrain_plant_->Y(0);
+ last_right_position_ = drivetrain_plant_->Y(1);
EXPECT_TRUE(my_drivetrain_loop_.output.FetchLatest());
- drivetrain_plant_->U << my_drivetrain_loop_.output->left_voltage,
- my_drivetrain_loop_.output->right_voltage;
+ drivetrain_plant_->change_U() << my_drivetrain_loop_.output->left_voltage,
+ my_drivetrain_loop_.output->right_voltage;
drivetrain_plant_->Update();
}
diff --git a/frc971/control_loops/shooter/shooter.cc b/frc971/control_loops/shooter/shooter.cc
index d8240db..c4d183a 100755
--- a/frc971/control_loops/shooter/shooter.cc
+++ b/frc971/control_loops/shooter/shooter.cc
@@ -18,26 +18,26 @@
void ZeroedStateFeedbackLoop::CapU() {
const double old_voltage = voltage_;
- voltage_ += U(0, 0);
+ voltage_ += U(0);
uncapped_voltage_ = voltage_;
// Make sure that reality and the observer can't get too far off. There is a
- // delay by one cycle between the applied voltage and X_hat(2, 0), so compare
+ // delay by one cycle between the applied voltage and X_hat(2), so compare
// against last cycle's voltage.
- if (X_hat(2, 0) > last_voltage_ + 4.0) {
- voltage_ -= X_hat(2, 0) - (last_voltage_ + 4.0);
+ if (X_hat(2) > last_voltage_ + 4.0) {
+ voltage_ -= X_hat(2) - (last_voltage_ + 4.0);
LOG(DEBUG, "Capping due to runaway\n");
- } else if (X_hat(2, 0) < last_voltage_ - 4.0) {
- voltage_ += X_hat(2, 0) - (last_voltage_ - 4.0);
+ } else if (X_hat(2) < last_voltage_ - 4.0) {
+ voltage_ += X_hat(2) - (last_voltage_ - 4.0);
LOG(DEBUG, "Capping due to runaway\n");
}
voltage_ = std::min(max_voltage_, voltage_);
voltage_ = std::max(-max_voltage_, voltage_);
- U(0, 0) = voltage_ - old_voltage;
+ change_U(0) = voltage_ - old_voltage;
- LOG_STRUCT(DEBUG, "output", ShooterVoltageToLog(X_hat(2, 0), voltage_));
+ LOG_STRUCT(DEBUG, "output", ShooterVoltageToLog(X_hat(2), voltage_));
last_voltage_ = voltage_;
capped_goal_ = false;
@@ -49,11 +49,11 @@
if (controller_index() == 0) {
dx = (uncapped_voltage() - max_voltage_) /
(K(0, 0) - A(1, 0) * K(0, 2) / A(1, 2));
- R(0, 0) -= dx;
- R(2, 0) -= -A(1, 0) / A(1, 2) * dx;
+ change_R(0) -= dx;
+ change_R(2) -= -A(1, 0) / A(1, 2) * dx;
} else {
dx = (uncapped_voltage() - max_voltage_) / K(0, 0);
- R(0, 0) -= dx;
+ change_R(0) -= dx;
}
capped_goal_ = true;
LOG_STRUCT(DEBUG, "to prevent windup", ShooterMovingGoal(dx));
@@ -62,11 +62,11 @@
if (controller_index() == 0) {
dx = (uncapped_voltage() + max_voltage_) /
(K(0, 0) - A(1, 0) * K(0, 2) / A(1, 2));
- R(0, 0) -= dx;
- R(2, 0) -= -A(1, 0) / A(1, 2) * dx;
+ change_R(0) -= dx;
+ change_R(2) -= -A(1, 0) / A(1, 2) * dx;
} else {
dx = (uncapped_voltage() + max_voltage_) / K(0, 0);
- R(0, 0) -= dx;
+ change_R(0) -= dx;
}
capped_goal_ = true;
LOG_STRUCT(DEBUG, "to prevent windup", ShooterMovingGoal(dx));
@@ -77,9 +77,9 @@
void ZeroedStateFeedbackLoop::RecalculatePowerGoal() {
if (controller_index() == 0) {
- R(2, 0) = (-A(1, 0) / A(1, 2) * R(0, 0) - A(1, 1) / A(1, 2) * R(1, 0));
+ change_R(2) = (-A(1, 0) / A(1, 2) * R(0) - A(1, 1) / A(1, 2) * R(1));
} else {
- R(2, 0) = -A(1, 1) / A(1, 2) * R(1, 0);
+ change_R(2) = -A(1, 1) / A(1, 2) * R(1);
}
}
@@ -89,13 +89,15 @@
double previous_offset = offset_;
offset_ = known_position - encoder_val;
double doffset = offset_ - previous_offset;
- X_hat(0, 0) += doffset;
+ change_X_hat(0) += doffset;
// Offset our measurements because the offset is baked into them.
- Y_(0, 0) += doffset;
+ // This is safe because if we got here, it means position != nullptr, which
+ // means we already set Y to something and it won't just get overwritten.
+ change_Y(0) += doffset;
// Offset the goal so we don't move.
- R(0, 0) += doffset;
+ change_R(0) += doffset;
if (controller_index() == 0) {
- R(2, 0) += -A(1, 0) / A(1, 2) * (doffset);
+ change_R(2) += -A(1, 0) / A(1, 2) * (doffset);
}
LOG_STRUCT(
DEBUG, "sensor edge (fake?)",
diff --git a/frc971/control_loops/shooter/shooter.h b/frc971/control_loops/shooter/shooter.h
index 82a06b6..7816c8e 100755
--- a/frc971/control_loops/shooter/shooter.h
+++ b/frc971/control_loops/shooter/shooter.h
@@ -59,8 +59,8 @@
double offset() const { return offset_; }
- double absolute_position() const { return X_hat(0, 0) + kPositionOffset; }
- double absolute_velocity() const { return X_hat(1, 0); }
+ double absolute_position() const { return X_hat(0) + kPositionOffset; }
+ double absolute_velocity() const { return X_hat(1); }
void CorrectPosition(double position) {
Eigen::Matrix<double, 1, 1> Y;
@@ -71,23 +71,24 @@
// Recomputes the power goal for the current controller and position/velocity.
void RecalculatePowerGoal();
- double goal_position() const { return R(0, 0) + kPositionOffset; }
- double goal_velocity() const { return R(1, 0); }
+ double goal_position() const { return R(0) + kPositionOffset; }
+ double goal_velocity() const { return R(1); }
void InitializeState(double position) {
- X_hat(0, 0) = position - kPositionOffset;
+ change_X_hat(0) = position - kPositionOffset;
}
void SetGoalPosition(double desired_position, double desired_velocity) {
- LOG(DEBUG, "Goal position: %f Goal velocity: %f\n", desired_position, desired_velocity);
+ LOG(DEBUG, "Goal position: %f Goal velocity: %f\n", desired_position,
+ desired_velocity);
- R << desired_position - kPositionOffset, desired_velocity,
+ change_R() << desired_position - kPositionOffset, desired_velocity,
(-A(1, 0) / A(1, 2) * (desired_position - kPositionOffset) -
A(1, 1) / A(1, 2) * desired_velocity);
}
- double position() const { return X_hat(0, 0) - offset_ + kPositionOffset; }
+ double position() const { return X_hat(0) - offset_ + kPositionOffset; }
- void set_max_voltage(const double max_voltage) { max_voltage_ = max_voltage; }
+ void set_max_voltage(double max_voltage) { max_voltage_ = max_voltage; }
bool capped_goal() const { return capped_goal_; }
void CapGoal();
diff --git a/frc971/control_loops/shooter/shooter_lib_test.cc b/frc971/control_loops/shooter/shooter_lib_test.cc
index a37cf6b..f1beafe 100755
--- a/frc971/control_loops/shooter/shooter_lib_test.cc
+++ b/frc971/control_loops/shooter/shooter_lib_test.cc
@@ -55,9 +55,9 @@
LOG(INFO, "Reinitializing to {pos: %f}\n", initial_position);
StateFeedbackPlant<2, 1, 1> *plant = shooter_plant_.get();
initial_position_ = initial_position;
- plant->X(0, 0) = initial_position_ - kPositionOffset;
- plant->X(1, 0) = 0.0;
- plant->Y = plant->C() * plant->X;
+ plant->change_X(0) = initial_position_ - kPositionOffset;
+ plant->change_X(1) = 0.0;
+ plant->change_Y() = plant->C() * plant->X();
last_voltage_ = 0.0;
last_plant_position_ = 0.0;
SetPhysicalSensors(&last_position_message_);
@@ -65,7 +65,7 @@
// Returns the absolute angle of the wrist.
double GetAbsolutePosition() const {
- return shooter_plant_->Y(0, 0) + kPositionOffset;
+ return shooter_plant_->Y(0) + kPositionOffset;
}
// Returns the adjusted angle of the wrist.
@@ -122,10 +122,12 @@
if (sensor->current && !last_sensor.current) {
++sensor->posedge_count;
if (last_position.position + initial_position_ < limits.lower_angle) {
- LOG(DEBUG, "Posedge value on lower edge of sensor, count is now %d\n", sensor->posedge_count);
+ LOG(DEBUG, "Posedge value on lower edge of sensor, count is now %d\n",
+ sensor->posedge_count);
sensor->posedge_value = limits.lower_angle - initial_position_;
} else {
- LOG(DEBUG, "Posedge value on upper edge of sensor, count is now %d\n", sensor->posedge_count);
+ LOG(DEBUG, "Posedge value on upper edge of sensor, count is now %d\n",
+ sensor->posedge_count);
sensor->posedge_value = limits.upper_angle - initial_position_;
}
}
@@ -211,12 +213,12 @@
}
if (brake_piston_state_) {
- shooter_plant_->U << 0.0;
- shooter_plant_->X(1, 0) = 0.0;
- shooter_plant_->Y = shooter_plant_->C() * shooter_plant_->X +
- shooter_plant_->D() * shooter_plant_->U;
+ shooter_plant_->change_U() << 0.0;
+ shooter_plant_->change_X(1) = 0.0;
+ shooter_plant_->change_Y() = shooter_plant_->C() * shooter_plant_->X() +
+ shooter_plant_->D() * shooter_plant_->U();
} else {
- shooter_plant_->U << last_voltage_;
+ shooter_plant_->change_U() << last_voltage_;
//shooter_plant_->U << shooter_queue_group_.output->voltage;
shooter_plant_->Update();
}
@@ -244,7 +246,7 @@
plunger_latched_ = false;
// TODO(austin): The brake should be set for a number of cycles after
// this as well.
- shooter_plant_->X(0, 0) += 0.005;
+ shooter_plant_->change_X(0) += 0.005;
}
latch_delay_count_++;
}
@@ -532,7 +534,7 @@
LOG(DEBUG, "State is UnloadMove\n");
--kicked_delay;
if (kicked_delay == 0) {
- shooter_motor_.shooter_.R(0, 0) -= 100;
+ shooter_motor_.shooter_.change_R(0) -= 100;
}
}
if (shooter_motor_.capped_goal() && kicked_delay < 0) {
@@ -572,7 +574,7 @@
LOG(DEBUG, "State is UnloadMove\n");
--kicked_delay;
if (kicked_delay == 0) {
- shooter_motor_.shooter_.R(0, 0) += 0.1;
+ shooter_motor_.shooter_.change_R(0) += 0.1;
}
}
if (shooter_motor_.capped_goal() && kicked_delay < 0) {
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index d62bc63..aa3f9a1 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -1,5 +1,5 @@
-#ifndef FRC971_CONTROL_LOOPS_STATEFEEDBACKLOOP_H_
-#define FRC971_CONTROL_LOOPS_STATEFEEDBACKLOOP_H_
+#ifndef FRC971_CONTROL_LOOPS_STATE_FEEDBACK_LOOP_H_
+#define FRC971_CONTROL_LOOPS_STATE_FEEDBACK_LOOP_H_
#include <assert.h>
@@ -11,29 +11,18 @@
#include "aos/common/logging/logging.h"
#include "aos/common/macros.h"
-// TODO(brians): There are a lot of public member variables in here that should
-// be made private and have (const) accessors instead (and have _s at the end of
-// their names).
-
template <int number_of_states, int number_of_inputs, int number_of_outputs>
-class StateFeedbackPlantCoefficients {
+class StateFeedbackPlantCoefficients final {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
- const Eigen::Matrix<double, number_of_states, number_of_states> A;
- const Eigen::Matrix<double, number_of_states, number_of_inputs> B;
- 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;
- const Eigen::Matrix<double, number_of_inputs, 1> U_max;
-
StateFeedbackPlantCoefficients(const StateFeedbackPlantCoefficients &other)
- : A(other.A),
- B(other.B),
- C(other.C),
- D(other.D),
- U_min(other.U_min),
- U_max(other.U_max) {
+ : A_(other.A()),
+ B_(other.B()),
+ C_(other.C()),
+ D_(other.D()),
+ U_min_(other.U_min()),
+ U_max_(other.U_max()) {
}
StateFeedbackPlantCoefficients(
@@ -43,19 +32,49 @@
const Eigen::Matrix<double, number_of_outputs, number_of_inputs> &D,
const Eigen::Matrix<double, number_of_outputs, 1> &U_max,
const Eigen::Matrix<double, number_of_outputs, 1> &U_min)
- : A(A),
- B(B),
- C(C),
- D(D),
- U_min(U_min),
- U_max(U_max) {
+ : A_(A),
+ B_(B),
+ C_(C),
+ D_(D),
+ U_min_(U_min),
+ U_max_(U_max) {
}
- protected:
- // these are accessible from non-templated subclasses
- static const int kNumStates = number_of_states;
- static const int kNumOutputs = number_of_outputs;
- static const int kNumInputs = number_of_inputs;
+ 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_inputs> &B() const {
+ return B_;
+ }
+ double B(int i, int j) const { return B()(i, j); }
+ const Eigen::Matrix<double, number_of_outputs, number_of_states> &C() const {
+ return 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 D_;
+ }
+ double D(int i, int j) const { return D()(i, j); }
+ const Eigen::Matrix<double, number_of_inputs, 1> &U_min() const {
+ return U_min_;
+ }
+ double U_min(int i) const { return U_min()(i, 0); }
+ const Eigen::Matrix<double, number_of_inputs, 1> &U_max() const {
+ return U_max_;
+ }
+ double U_max(int i) const { return U_max()(i, 0); }
+
+ private:
+ const Eigen::Matrix<double, number_of_states, number_of_states> A_;
+ const Eigen::Matrix<double, number_of_states, number_of_inputs> B_;
+ 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_;
+ const Eigen::Matrix<double, number_of_inputs, 1> U_max_;
+
+ StateFeedbackPlantCoefficients &operator=(
+ StateFeedbackPlantCoefficients other) = delete;
};
template <int number_of_states, int number_of_inputs, int number_of_outputs>
@@ -75,9 +94,9 @@
StateFeedbackPlant(StateFeedbackPlant &&other)
: plant_index_(other.plant_index_) {
::std::swap(coefficients_, other.coefficients_);
- X.swap(other.X);
- Y.swap(other.Y);
- U.swap(other.U);
+ X_.swap(other.X_);
+ Y_.swap(other.Y_);
+ U_.swap(other.U_);
}
virtual ~StateFeedbackPlant() {
@@ -86,33 +105,44 @@
}
}
- ::std::vector<StateFeedbackPlantCoefficients<
- number_of_states, number_of_inputs, number_of_outputs> *> coefficients_;
-
const Eigen::Matrix<double, number_of_states, number_of_states> &A() const {
- return coefficients().A;
+ return coefficients().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 coefficients().B;
+ return coefficients().B();
}
double B(int i, int j) const { return B()(i, j); }
const Eigen::Matrix<double, number_of_outputs, number_of_states> &C() const {
- return coefficients().C;
+ return coefficients().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 coefficients().D;
+ return coefficients().D();
}
double D(int i, int j) const { return D()(i, j); }
const Eigen::Matrix<double, number_of_inputs, 1> &U_min() const {
- return coefficients().U_min;
+ return coefficients().U_min();
}
- double U_min(int i, int j) const { return U_min()(i, j); }
+ double U_min(int i) const { return U_min()(i, 0); }
const Eigen::Matrix<double, number_of_inputs, 1> &U_max() const {
- return coefficients().U_max;
+ return coefficients().U_max();
}
- double U_max(int i, int j) const { return U_max()(i, j); }
+ double U_max(int i) const { return U_max()(i, 0); }
+
+ const Eigen::Matrix<double, number_of_states, 1> &X() const { return X_; }
+ double X(int i) const { return X()(i, 0); }
+ const Eigen::Matrix<double, number_of_outputs, 1> &Y() const { return Y_; }
+ double Y(int i) const { return Y()(i, 0); }
+ const Eigen::Matrix<double, number_of_inputs, 1> &U() const { return U_; }
+ double U(int i) const { return X()(i, 0); }
+
+ Eigen::Matrix<double, number_of_states, 1> &change_X() { return X_; }
+ double &change_X(int i) { return change_X()(i, 0); }
+ Eigen::Matrix<double, number_of_outputs, 1> &change_Y() { return Y_; }
+ double &change_Y(int i) { return change_Y()(i, 0); }
+ Eigen::Matrix<double, number_of_inputs, 1> &change_U() { return U_; }
+ double &change_U(int i) { return change_U()(i, 0); }
const StateFeedbackPlantCoefficients<
number_of_states, number_of_inputs, number_of_outputs>
@@ -132,20 +162,16 @@
}
void Reset() {
- X.setZero();
- Y.setZero();
- U.setZero();
+ X_.setZero();
+ Y_.setZero();
+ U_.setZero();
}
- Eigen::Matrix<double, number_of_states, 1> X;
- Eigen::Matrix<double, number_of_outputs, 1> Y;
- Eigen::Matrix<double, number_of_inputs, 1> U;
-
// Assert that U is within the hardware range.
virtual void CheckU() {
for (int i = 0; i < kNumOutputs; ++i) {
- assert(U(i, 0) <= U_max(i, 0) + 0.00001);
- assert(U(i, 0) >= U_min(i, 0) - 0.00001);
+ assert(U(i) <= U_max(i) + 0.00001);
+ assert(U(i) >= U_min(i) - 0.00001);
}
}
@@ -154,8 +180,8 @@
// 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;
+ X_ = A() * X() + B() * U();
+ Y_ = C() * X() + D() * U();
}
protected:
@@ -165,6 +191,13 @@
static const int kNumInputs = number_of_inputs;
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<StateFeedbackPlantCoefficients<
+ number_of_states, number_of_inputs, number_of_outputs> *> coefficients_;
+
int plant_index_;
DISALLOW_COPY_AND_ASSIGN(StateFeedbackPlant);
@@ -174,8 +207,9 @@
// 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 {
+struct StateFeedbackController final {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+
const Eigen::Matrix<double, number_of_states, number_of_outputs> L;
const Eigen::Matrix<double, number_of_outputs, number_of_states> K;
StateFeedbackPlantCoefficients<number_of_states, number_of_inputs,
@@ -205,24 +239,6 @@
Reset();
}
- StateFeedbackLoop(const ::std::vector<StateFeedbackController<
- number_of_states, number_of_inputs, number_of_outputs> *> &controllers)
- : controllers_(controllers), controller_index_(0) {
- Reset();
- }
-
- StateFeedbackLoop(StateFeedbackLoop &&other) {
- X_hat.swap(other.X_hat);
- R.swap(other.R);
- U.swap(other.U);
- U_uncapped.swap(other.U_uncapped);
- U_ff.swap(other.U_ff);
- ::std::swap(controllers_, other.controllers_);
- Y_.swap(other.Y_);
- new_y_ = other.new_y_;
- controller_index_ = other.controller_index_;
- }
-
StateFeedbackLoop(
const Eigen::Matrix<double, number_of_states, number_of_outputs> &L,
const Eigen::Matrix<double, number_of_outputs, number_of_states> &K,
@@ -236,6 +252,23 @@
Reset();
}
+ StateFeedbackLoop(const ::std::vector<StateFeedbackController<
+ number_of_states, number_of_inputs, number_of_outputs> *> &controllers)
+ : controllers_(controllers), controller_index_(0) {
+ Reset();
+ }
+
+ StateFeedbackLoop(StateFeedbackLoop &&other) {
+ X_hat_.swap(other.X_hat_);
+ R_.swap(other.R_);
+ U_.swap(other.U_);
+ U_uncapped_.swap(other.U_uncapped_);
+ ::std::swap(controllers_, other.controllers_);
+ Y_.swap(other.Y_);
+ new_y_ = other.new_y_;
+ controller_index_ = other.controller_index_;
+ }
+
virtual ~StateFeedbackLoop() {
for (auto c : controllers_) {
delete c;
@@ -243,21 +276,30 @@
}
const Eigen::Matrix<double, number_of_states, number_of_states> &A() const {
- return controller().plant.A;
+ 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;
+ return controller().plant.B();
}
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;
+ 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;
+ 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) const { return U_min()(i, 0); }
+ const Eigen::Matrix<double, number_of_inputs, 1> &U_max() const {
+ return controller().plant.U_max();
+ }
+ double U_max(int i) const { return U_max()(i, 0); }
+
const Eigen::Matrix<double, number_of_outputs, number_of_states> &K() const {
return controller().K;
}
@@ -266,14 +308,34 @@
return controller().L;
}
double L(int i, int j) const { return L()(i, j); }
- const Eigen::Matrix<double, number_of_inputs, 1> &U_min() const {
- return controller().plant.U_min;
+
+ const Eigen::Matrix<double, number_of_states, 1> &X_hat() const {
+ return X_hat_;
}
- 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 X_hat(int i) const { return X_hat()(i, 0); }
+ const Eigen::Matrix<double, number_of_states, 1> &R() const { return R_; }
+ double R(int i) const { return R()(i, 0); }
+ const Eigen::Matrix<double, number_of_inputs, 1> &U() const { return U_; }
+ double U(int i) const { return U()(i, 0); }
+ const Eigen::Matrix<double, number_of_inputs, 1> &U_uncapped() const {
+ return U_uncapped_;
}
- double U_max(int i, int j) const { return U_max()(i, j); }
+ double U_uncapped(int i) const { return U_uncapped()(i, 0); }
+ const Eigen::Matrix<double, number_of_outputs, 1> &Y() const { return Y_; }
+ double Y(int i) const { return Y()(i, 0); }
+
+ Eigen::Matrix<double, number_of_states, 1> &change_X_hat() { return X_hat_; }
+ double &change_X_hat(int i) { return change_X_hat()(i, 0); }
+ Eigen::Matrix<double, number_of_states, 1> &change_R() { return R_; }
+ double &change_R(int i) { return change_R()(i, 0); }
+ Eigen::Matrix<double, number_of_inputs, 1> &change_U() { return U_; }
+ double &change_U(int i) { return change_U()(i, 0); }
+ Eigen::Matrix<double, number_of_inputs, 1> &change_U_uncapped() {
+ return U_uncapped_;
+ }
+ double &change_U_uncapped(int i) { return change_U_uncapped()(i, 0); }
+ Eigen::Matrix<double, number_of_outputs, 1> &change_Y() { return Y_; }
+ double &change_Y(int i) { return change_Y()(i, 0); }
const StateFeedbackController<number_of_states, number_of_inputs,
number_of_outputs> &controller() const {
@@ -287,27 +349,20 @@
}
void Reset() {
- X_hat.setZero();
- R.setZero();
- U.setZero();
- U_uncapped.setZero();
- U_ff.setZero();
- }
-
- virtual void FeedForward() {
- for (int i = 0; i < number_of_outputs; ++i) {
- U_ff[i] = 0.0;
- }
+ X_hat_.setZero();
+ R_.setZero();
+ U_.setZero();
+ U_uncapped_.setZero();
}
// If U is outside the hardware range, limit it before the plant tries to use
// it.
virtual void CapU() {
for (int i = 0; i < kNumOutputs; ++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) > U_max(i)) {
+ U_(i, 0) = U_max(i);
+ } else if (U(i) < U_min(i)) {
+ U_(i, 0) = U_min(i);
}
}
}
@@ -338,31 +393,26 @@
// stop_motors is whether or not to output all 0s.
void Update(bool stop_motors) {
if (stop_motors) {
- U.setZero();
- U_uncapped.setZero();
+ U_.setZero();
+ U_uncapped_.setZero();
} else {
- U = U_uncapped = K() * (R - X_hat);
+ U_ = U_uncapped_ = K() * (R() - X_hat());
CapU();
}
- //::std::cout << "Predict xhat before " << X_hat;
- //::std::cout << "Measurement error is " << Y_ - C() * X_hat;
- //X_hat = A() * X_hat + B() * U;
- //::std::cout << "Predict xhat after " << X_hat;
UpdateObserver();
}
void UpdateObserver() {
if (new_y_) {
- X_hat = (A() - L() * C()) * X_hat + L() * Y_ + B() * U;
+ X_hat_ = (A() - L() * C()) * X_hat() + L() * Y() + B() * U();
new_y_ = false;
} else {
- X_hat = A() * X_hat + B() * U;
+ X_hat_ = A() * X_hat() + B() * U();
}
}
- // Sets the current controller to be index and verifies that it isn't out of
- // range.
+ // Sets the current controller to be index, clamped to be within range.
void set_controller_index(int index) {
if (index < 0) {
controller_index_ = 0;
@@ -375,21 +425,21 @@
int controller_index() const { return controller_index_; }
- Eigen::Matrix<double, number_of_states, 1> X_hat;
- Eigen::Matrix<double, number_of_states, 1> R;
- Eigen::Matrix<double, number_of_inputs, 1> U;
- Eigen::Matrix<double, number_of_inputs, 1> U_uncapped;
- Eigen::Matrix<double, number_of_outputs, 1> U_ff;
-
protected:
::std::vector<StateFeedbackController<number_of_states, number_of_inputs,
number_of_outputs> *> controllers_;
- // these are accessible from non-templated subclasses
+ // These are accessible from non-templated subclasses.
static const int kNumStates = number_of_states;
static const int kNumOutputs = number_of_outputs;
static const int kNumInputs = number_of_inputs;
+ private:
+ Eigen::Matrix<double, number_of_states, 1> X_hat_;
+ Eigen::Matrix<double, number_of_states, 1> R_;
+ Eigen::Matrix<double, number_of_inputs, 1> U_;
+ Eigen::Matrix<double, number_of_inputs, 1> U_uncapped_;
+
// Temporary storage for a measurement until I can figure out why I can't
// correct when the measurement is made.
Eigen::Matrix<double, number_of_outputs, 1> Y_;
@@ -397,8 +447,7 @@
int controller_index_;
- private:
DISALLOW_COPY_AND_ASSIGN(StateFeedbackLoop);
};
-#endif // FRC971_CONTROL_LOOPS_STATEFEEDBACKLOOP_H_
+#endif // FRC971_CONTROL_LOOPS_STATE_FEEDBACK_LOOP_H_