added back explicit 0 indices
diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
index b4be917..7959f30 100644
--- a/frc971/control_loops/claw/claw.cc
+++ b/frc971/control_loops/claw/claw.cc
@@ -81,8 +81,8 @@
void ClawLimitedLoop::CapU() {
const Eigen::Matrix<double, 4, 1> error = R() - X_hat();
- double u_top = U(1);
- double u_bottom = U(0);
+ double u_top = U(1, 0);
+ double u_bottom = U(0, 0);
uncapped_average_voltage_ = (u_top + u_bottom) / 2;
@@ -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) + X_hat(1) >
+ if (X_hat(0, 0) + X_hat(1, 0) >
constants::GetValues().claw.upper_claw.upper_limit) {
angle_45 << 1, 1;
} else {
@@ -183,22 +183,22 @@
{
const auto values = constants::GetValues().claw;
if (top_known_) {
- if (X_hat(0) + X_hat(1) > values.upper_claw.upper_limit && U(1) > 0) {
+ if (X_hat(0, 0) + X_hat(1, 0) > values.upper_claw.upper_limit && U(1, 0) > 0) {
LOG(WARNING, "upper claw too high and moving up\n");
- mutable_U(1) = 0;
- } else if (X_hat(0) + X_hat(1) < values.upper_claw.lower_limit &&
- U(1) < 0) {
+ mutable_U(1, 0) = 0;
+ } else if (X_hat(0, 0) + X_hat(1, 0) < values.upper_claw.lower_limit &&
+ U(1, 0) < 0) {
LOG(WARNING, "upper claw too low and moving down\n");
- mutable_U(1) = 0;
+ mutable_U(1, 0) = 0;
}
}
if (bottom_known_) {
- if (X_hat(0) > values.lower_claw.upper_limit && U(0) > 0) {
+ if (X_hat(0, 0) > values.lower_claw.upper_limit && U(0, 0) > 0) {
LOG(WARNING, "lower claw too high and moving up\n");
- mutable_U(0) = 0;
- } else if (X_hat(0) < values.lower_claw.lower_limit && U(0) < 0) {
+ mutable_U(0, 0) = 0;
+ } else if (X_hat(0, 0) < values.lower_claw.lower_limit && U(0, 0) < 0) {
LOG(WARNING, "lower claw too low and moving down\n");
- mutable_U(0) = 0;
+ mutable_U(0, 0) = 0;
}
}
}
@@ -886,18 +886,18 @@
case FINE_TUNE_TOP:
case UNKNOWN_LOCATION: {
if (claw_.uncapped_average_voltage() > values.claw.max_zeroing_voltage) {
- double dx_bot = (claw_.U_uncapped(0) -
+ double dx_bot = (claw_.U_uncapped(0, 0) -
values.claw.max_zeroing_voltage) /
claw_.K(0, 0);
- double dx_top = (claw_.U_uncapped(1) -
+ double dx_top = (claw_.U_uncapped(1, 0) -
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),
- claw_.R(3);
+ 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());
capped_goal_ = true;
LOG(DEBUG, "Moving the goal by %f to prevent windup."
@@ -908,18 +908,18 @@
values.claw.max_zeroing_voltage));
} else if (claw_.uncapped_average_voltage() <
-values.claw.max_zeroing_voltage) {
- double dx_bot = (claw_.U_uncapped(0) +
+ double dx_bot = (claw_.U_uncapped(0, 0) +
values.claw.max_zeroing_voltage) /
claw_.K(0, 0);
- double dx_top = (claw_.U_uncapped(1) +
+ double dx_top = (claw_.U_uncapped(1, 0) +
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),
- claw_.R(3);
+ 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());
capped_goal_ = true;
LOG(DEBUG, "Moving the goal by %f to prevent windup\n", dx);
@@ -939,8 +939,8 @@
? -12.0
: goal->centering;
}
- output->top_claw_voltage = claw_.U(1);
- output->bottom_claw_voltage = claw_.U(0);
+ output->top_claw_voltage = claw_.U(1, 0);
+ output->bottom_claw_voltage = claw_.U(0, 0);
if (output->top_claw_voltage > kMaxVoltage) {
output->top_claw_voltage = kMaxVoltage;
@@ -957,8 +957,8 @@
status->bottom = bottom_absolute_position();
status->separation = top_absolute_position() - bottom_absolute_position();
- status->bottom_velocity = claw_.X_hat(2);
- status->separation_velocity = claw_.X_hat(3);
+ status->bottom_velocity = claw_.X_hat(2, 0);
+ status->separation_velocity = claw_.X_hat(3, 0);
if (goal) {
bool bottom_done =
diff --git a/frc971/control_loops/claw/claw.h b/frc971/control_loops/claw/claw.h
index f444bbe..12f7264 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) + claw_.X_hat(0);
+ return claw_.X_hat(1, 0) + claw_.X_hat(0, 0);
}
- double bottom_absolute_position() const { return claw_.X_hat(0); }
+ double bottom_absolute_position() const { return claw_.X_hat(0, 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 7881203..cfdcd11 100644
--- a/frc971/control_loops/claw/claw_lib_test.cc
+++ b/frc971/control_loops/claw/claw_lib_test.cc
@@ -54,10 +54,10 @@
double initial_bottom_position) {
LOG(INFO, "Reinitializing to {top: %f, bottom: %f}\n", initial_top_position,
initial_bottom_position);
- claw_plant_->mutable_X(0) = initial_bottom_position;
- claw_plant_->mutable_X(1) = initial_top_position - initial_bottom_position;
- claw_plant_->mutable_X(2) = 0.0;
- claw_plant_->mutable_X(3) = 0.0;
+ claw_plant_->mutable_X(0, 0) = initial_bottom_position;
+ claw_plant_->mutable_X(1, 0) = initial_top_position - initial_bottom_position;
+ claw_plant_->mutable_X(2, 0) = 0.0;
+ claw_plant_->mutable_X(3, 0) = 0.0;
claw_plant_->mutable_Y() = claw_plant_->C() * claw_plant_->X();
ReinitializePartial(TOP_CLAW, initial_top_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);
+ return claw_plant_->Y(1, 0);
} else {
- return claw_plant_->Y(0);
+ return claw_plant_->Y(0, 0);
}
}
@@ -219,15 +219,15 @@
claw_plant_->Update();
// Check that the claw is within the limits.
- 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.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.lower_claw.upper_hard_limit, claw_plant_->Y(1));
- EXPECT_LE(v.claw.lower_claw.lower_hard_limit, claw_plant_->Y(1));
+ 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_LE(claw_plant_->Y(1) - claw_plant_->Y(0),
+ EXPECT_LE(claw_plant_->Y(1, 0) - claw_plant_->Y(0, 0),
v.claw.claw_max_separation);
- EXPECT_GE(claw_plant_->Y(1) - claw_plant_->Y(0),
+ EXPECT_GE(claw_plant_->Y(1, 0) - claw_plant_->Y(0, 0),
v.claw.claw_min_separation);
}
// The whole claw.