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.
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 701ea1b..3573885 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -49,7 +49,7 @@
virtual void CapU() {
const Eigen::Matrix<double, 4, 1> error = R() - X_hat();
- if (::std::abs(U(0)) > 12.0 || ::std::abs(U(1)) > 12.0) {
+ if (::std::abs(U(0, 0)) > 12.0 || ::std::abs(U(1, 0)) > 12.0) {
output_was_capped_ = true;
LOG_MATRIX(DEBUG, "U at start", U());
@@ -186,15 +186,15 @@
double GetEstimatedRobotSpeed() const {
// lets just call the average of left and right velocities close enough
- return (loop_->X_hat(1) + loop_->X_hat(3)) / 2;
+ return (loop_->X_hat(1, 0) + loop_->X_hat(3, 0)) / 2;
}
double GetEstimatedLeftEncoder() const {
- return loop_->X_hat(0);
+ return loop_->X_hat(0, 0);
}
double GetEstimatedRightEncoder() const {
- return loop_->X_hat(2);
+ return loop_->X_hat(2, 0);
}
bool OutputWasCapped() const {
@@ -203,8 +203,8 @@
void SendMotors(Drivetrain::Output *output) const {
if (output) {
- output->left_voltage = loop_->U(0);
- output->right_voltage = loop_->U(1);
+ output->left_voltage = loop_->U(0, 0);
+ output->right_voltage = loop_->U(1, 0);
output->left_high = false;
output->right_high = false;
}
@@ -471,7 +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) + loop_->X_hat(1)) / 2.0) /
+ ttrust_ * min_K_sum * (loop_->X_hat(0, 0) + loop_->X_hat(1, 0)) / 2.0) /
(ttrust_ * min_K_sum + min_FF_sum));
}
@@ -604,10 +604,10 @@
const double wiggle =
(static_cast<double>((counter_ % 20) / 10) - 0.5) * 5.0;
- loop_->mutable_U(0) = ::aos::Clip(
+ loop_->mutable_U(0, 0) = ::aos::Clip(
(R_left / Kv)(0, 0) + (IsInGear(left_gear_) ? 0 : wiggle),
-12.0, 12.0);
- loop_->mutable_U(1) = ::aos::Clip(
+ loop_->mutable_U(1, 0) = ::aos::Clip(
(R_right / Kv)(0, 0) + (IsInGear(right_gear_) ? 0 : wiggle),
-12.0, 12.0);
loop_->mutable_U() *= 12.0 / position_.battery_voltage;
@@ -616,8 +616,8 @@
void SendMotors(Drivetrain::Output *output) {
if (output != NULL) {
- output->left_voltage = loop_->U(0);
- output->right_voltage = loop_->U(1);
+ output->left_voltage = loop_->U(0, 0);
+ output->right_voltage = loop_->U(1, 0);
output->left_high = left_gear_ == HIGH || left_gear_ == SHIFTING_UP;
output->right_high = right_gear_ == HIGH || right_gear_ == SHIFTING_UP;
}
@@ -722,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);
- status->uncapped_right_voltage = dt_closedloop.loop().U_uncapped(1);
+ status->uncapped_left_voltage = dt_closedloop.loop().U_uncapped(0, 0);
+ status->uncapped_right_voltage = dt_closedloop.loop().U_uncapped(1, 0);
}
}
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 548bd06..3b607fe 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -59,17 +59,17 @@
// Resets the plant.
void Reinitialize() {
- drivetrain_plant_->mutable_X(0) = 0.0;
- drivetrain_plant_->mutable_X(1) = 0.0;
+ drivetrain_plant_->mutable_X(0, 0) = 0.0;
+ drivetrain_plant_->mutable_X(1, 0) = 0.0;
drivetrain_plant_->mutable_Y() =
drivetrain_plant_->C() * drivetrain_plant_->X();
- last_left_position_ = drivetrain_plant_->Y(0);
- last_right_position_ = drivetrain_plant_->Y(1);
+ last_left_position_ = drivetrain_plant_->Y(0, 0);
+ last_right_position_ = drivetrain_plant_->Y(1, 0);
}
// Returns the position of the drivetrain.
- double GetLeftPosition() const { return drivetrain_plant_->Y(0); }
- double GetRightPosition() const { return drivetrain_plant_->Y(1); }
+ double GetLeftPosition() const { return drivetrain_plant_->Y(0, 0); }
+ double GetRightPosition() const { return drivetrain_plant_->Y(1, 0); }
// Sends out the position queue messages.
void SendPositionMessage() {
@@ -85,8 +85,8 @@
// Simulates the drivetrain moving for one timestep.
void Simulate() {
- last_left_position_ = drivetrain_plant_->Y(0);
- last_right_position_ = drivetrain_plant_->Y(1);
+ last_left_position_ = drivetrain_plant_->Y(0, 0);
+ last_right_position_ = drivetrain_plant_->Y(1, 0);
EXPECT_TRUE(my_drivetrain_loop_.output.FetchLatest());
drivetrain_plant_->mutable_U() << my_drivetrain_loop_.output->left_voltage,
my_drivetrain_loop_.output->right_voltage;
diff --git a/frc971/control_loops/shooter/shooter.cc b/frc971/control_loops/shooter/shooter.cc
index 2d8df35..b6d71c6 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);
+ voltage_ += U(0, 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), so compare
+ // delay by one cycle between the applied voltage and X_hat(2, 0), so compare
// against last cycle's voltage.
- if (X_hat(2) > last_voltage_ + 4.0) {
- voltage_ -= X_hat(2) - (last_voltage_ + 4.0);
+ if (X_hat(2, 0) > last_voltage_ + 4.0) {
+ voltage_ -= X_hat(2, 0) - (last_voltage_ + 4.0);
LOG(DEBUG, "Capping due to runaway\n");
- } else if (X_hat(2) < last_voltage_ - 4.0) {
- voltage_ += X_hat(2) - (last_voltage_ - 4.0);
+ } else if (X_hat(2, 0) < last_voltage_ - 4.0) {
+ voltage_ += X_hat(2, 0) - (last_voltage_ - 4.0);
LOG(DEBUG, "Capping due to runaway\n");
}
voltage_ = std::min(max_voltage_, voltage_);
voltage_ = std::max(-max_voltage_, voltage_);
- mutable_U(0) = voltage_ - old_voltage;
+ mutable_U(0, 0) = voltage_ - old_voltage;
- LOG_STRUCT(DEBUG, "output", ShooterVoltageToLog(X_hat(2), voltage_));
+ LOG_STRUCT(DEBUG, "output", ShooterVoltageToLog(X_hat(2, 0), 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));
- mutable_R(0) -= dx;
- mutable_R(2) -= -A(1, 0) / A(1, 2) * dx;
+ mutable_R(0, 0) -= dx;
+ mutable_R(2, 0) -= -A(1, 0) / A(1, 2) * dx;
} else {
dx = (uncapped_voltage() - max_voltage_) / K(0, 0);
- mutable_R(0) -= dx;
+ mutable_R(0, 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));
- mutable_R(0) -= dx;
- mutable_R(2) -= -A(1, 0) / A(1, 2) * dx;
+ mutable_R(0, 0) -= dx;
+ mutable_R(2, 0) -= -A(1, 0) / A(1, 2) * dx;
} else {
dx = (uncapped_voltage() + max_voltage_) / K(0, 0);
- mutable_R(0) -= dx;
+ mutable_R(0, 0) -= dx;
}
capped_goal_ = true;
LOG_STRUCT(DEBUG, "to prevent windup", ShooterMovingGoal(dx));
@@ -77,9 +77,9 @@
void ZeroedStateFeedbackLoop::RecalculatePowerGoal() {
if (controller_index() == 0) {
- mutable_R(2) = (-A(1, 0) / A(1, 2) * R(0) - A(1, 1) / A(1, 2) * R(1));
+ mutable_R(2, 0) = (-A(1, 0) / A(1, 2) * R(0, 0) - A(1, 1) / A(1, 2) * R(1, 0));
} else {
- mutable_R(2) = -A(1, 1) / A(1, 2) * R(1);
+ mutable_R(2, 0) = -A(1, 1) / A(1, 2) * R(1, 0);
}
}
@@ -89,15 +89,15 @@
double previous_offset = offset_;
offset_ = known_position - encoder_val;
double doffset = offset_ - previous_offset;
- mutable_X_hat(0) += doffset;
+ mutable_X_hat(0, 0) += doffset;
// Offset our measurements because the offset is baked into them.
// 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.
- mutable_Y(0) += doffset;
+ mutable_Y(0, 0) += doffset;
// Offset the goal so we don't move.
- mutable_R(0) += doffset;
+ mutable_R(0, 0) += doffset;
if (controller_index() == 0) {
- mutable_R(2) += -A(1, 0) / A(1, 2) * (doffset);
+ mutable_R(2, 0) += -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 71c1fa4..820b15f 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) + kPositionOffset; }
- double absolute_velocity() const { return X_hat(1); }
+ double absolute_position() const { return X_hat(0, 0) + kPositionOffset; }
+ double absolute_velocity() const { return X_hat(1, 0); }
void CorrectPosition(double position) {
Eigen::Matrix<double, 1, 1> Y;
@@ -71,10 +71,10 @@
// Recomputes the power goal for the current controller and position/velocity.
void RecalculatePowerGoal();
- double goal_position() const { return R(0) + kPositionOffset; }
- double goal_velocity() const { return R(1); }
+ double goal_position() const { return R(0, 0) + kPositionOffset; }
+ double goal_velocity() const { return R(1, 0); }
void InitializeState(double position) {
- mutable_X_hat(0) = position - kPositionOffset;
+ mutable_X_hat(0, 0) = position - kPositionOffset;
}
void SetGoalPosition(double desired_position, double desired_velocity) {
@@ -86,7 +86,7 @@
A(1, 1) / A(1, 2) * desired_velocity);
}
- double position() const { return X_hat(0) - offset_ + kPositionOffset; }
+ double position() const { return X_hat(0, 0) - offset_ + kPositionOffset; }
void set_max_voltage(double max_voltage) { max_voltage_ = max_voltage; }
bool capped_goal() const { return capped_goal_; }
diff --git a/frc971/control_loops/shooter/shooter_lib_test.cc b/frc971/control_loops/shooter/shooter_lib_test.cc
index 49263c7..37b186b 100755
--- a/frc971/control_loops/shooter/shooter_lib_test.cc
+++ b/frc971/control_loops/shooter/shooter_lib_test.cc
@@ -55,8 +55,8 @@
LOG(INFO, "Reinitializing to {pos: %f}\n", initial_position);
StateFeedbackPlant<2, 1, 1> *plant = shooter_plant_.get();
initial_position_ = initial_position;
- plant->mutable_X(0) = initial_position_ - kPositionOffset;
- plant->mutable_X(1) = 0.0;
+ plant->mutable_X(0, 0) = initial_position_ - kPositionOffset;
+ plant->mutable_X(1, 0) = 0.0;
plant->mutable_Y() = plant->C() * plant->X();
last_voltage_ = 0.0;
last_plant_position_ = 0.0;
@@ -65,7 +65,7 @@
// Returns the absolute angle of the wrist.
double GetAbsolutePosition() const {
- return shooter_plant_->Y(0) + kPositionOffset;
+ return shooter_plant_->Y(0, 0) + kPositionOffset;
}
// Returns the adjusted angle of the wrist.
@@ -214,7 +214,7 @@
if (brake_piston_state_) {
shooter_plant_->mutable_U() << 0.0;
- shooter_plant_->mutable_X(1) = 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();
} else {
@@ -246,7 +246,7 @@
plunger_latched_ = false;
// TODO(austin): The brake should be set for a number of cycles after
// this as well.
- shooter_plant_->mutable_X(0) += 0.005;
+ shooter_plant_->mutable_X(0, 0) += 0.005;
}
latch_delay_count_++;
}
@@ -534,7 +534,7 @@
LOG(DEBUG, "State is UnloadMove\n");
--kicked_delay;
if (kicked_delay == 0) {
- shooter_motor_.shooter_.mutable_R(0) -= 100;
+ shooter_motor_.shooter_.mutable_R(0, 0) -= 100;
}
}
if (shooter_motor_.capped_goal() && kicked_delay < 0) {
@@ -574,7 +574,7 @@
LOG(DEBUG, "State is UnloadMove\n");
--kicked_delay;
if (kicked_delay == 0) {
- shooter_motor_.shooter_.mutable_R(0) += 0.1;
+ shooter_motor_.shooter_.mutable_R(0, 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 9bd0165..eefaa1a 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -59,11 +59,11 @@
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); }
+ 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 U_max_;
}
- double U_max(int i) const { return U_max()(i, 0); }
+ double U_max(int i, int j) const { return U_max()(i, j); }
private:
const Eigen::Matrix<double, number_of_states, number_of_states> A_;
@@ -124,25 +124,25 @@
const Eigen::Matrix<double, number_of_inputs, 1> &U_min() const {
return coefficients().U_min();
}
- double U_min(int i) const { return U_min()(i, 0); }
+ 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 coefficients().U_max();
}
- double U_max(int i) const { return U_max()(i, 0); }
+ double U_max(int i, int j) const { return U_max()(i, j); }
const Eigen::Matrix<double, number_of_states, 1> &X() const { return X_; }
- double X(int i) const { return X()(i, 0); }
+ 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) const { return Y()(i, 0); }
+ 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) const { return X()(i, 0); }
+ double U(int i, int j) const { return X()(i, j); }
Eigen::Matrix<double, number_of_states, 1> &mutable_X() { return X_; }
- double &mutable_X(int i) { return mutable_X()(i, 0); }
+ 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) { return mutable_Y()(i, 0); }
+ 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) { return mutable_U()(i, 0); }
+ double &mutable_U(int i, int j) { return mutable_U()(i, j); }
const StateFeedbackPlantCoefficients<
number_of_states, number_of_inputs, number_of_outputs>
@@ -170,8 +170,8 @@
// Assert that U is within the hardware range.
virtual void CheckU() {
for (int i = 0; i < kNumOutputs; ++i) {
- assert(U(i) <= U_max(i) + 0.00001);
- assert(U(i) >= U_min(i) - 0.00001);
+ assert(U(i, 0) <= U_max(i, 0) + 0.00001);
+ assert(U(i, 0) >= U_min(i, 0) - 0.00001);
}
}
@@ -294,11 +294,11 @@
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); }
+ 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) const { return U_max()(i, 0); }
+ double U_max(int i, int j) const { return U_max()(i, j); }
const Eigen::Matrix<double, number_of_outputs, number_of_states> &K() const {
return controller().K;
@@ -312,30 +312,32 @@
const Eigen::Matrix<double, number_of_states, 1> &X_hat() const {
return X_hat_;
}
- double X_hat(int i) const { return X_hat()(i, 0); }
+ double X_hat(int i, int j) const { return X_hat()(i, j); }
const Eigen::Matrix<double, number_of_states, 1> &R() const { return R_; }
- double R(int i) const { return R()(i, 0); }
+ double R(int i, int j) const { return R()(i, j); }
const Eigen::Matrix<double, number_of_inputs, 1> &U() const { return U_; }
- double U(int i) const { return U()(i, 0); }
+ double U(int i, int j) const { return U()(i, j); }
const Eigen::Matrix<double, number_of_inputs, 1> &U_uncapped() const {
return U_uncapped_;
}
- double U_uncapped(int i) const { return U_uncapped()(i, 0); }
+ double U_uncapped(int i, int j) const { return U_uncapped()(i, j); }
const Eigen::Matrix<double, number_of_outputs, 1> &Y() const { return Y_; }
- double Y(int i) const { return Y()(i, 0); }
+ double Y(int i, int j) const { return Y()(i, j); }
Eigen::Matrix<double, number_of_states, 1> &mutable_X_hat() { return X_hat_; }
- double &mutable_X_hat(int i) { return mutable_X_hat()(i, 0); }
+ double &mutable_X_hat(int i, int j) { return mutable_X_hat()(i, j); }
Eigen::Matrix<double, number_of_states, 1> &mutable_R() { return R_; }
- double &mutable_R(int i) { return mutable_R()(i, 0); }
+ double &mutable_R(int i, int j) { return mutable_R()(i, j); }
Eigen::Matrix<double, number_of_inputs, 1> &mutable_U() { return U_; }
- double &mutable_U(int i) { return mutable_U()(i, 0); }
+ double &mutable_U(int i, int j) { return mutable_U()(i, j); }
Eigen::Matrix<double, number_of_inputs, 1> &mutable_U_uncapped() {
return U_uncapped_;
}
- double &mutable_U_uncapped(int i) { return mutable_U_uncapped()(i, 0); }
+ double &mutable_U_uncapped(int i, int j) {
+ return mutable_U_uncapped()(i, j);
+ }
Eigen::Matrix<double, number_of_outputs, 1> &mutable_Y() { return Y_; }
- double &mutable_Y(int i) { return mutable_Y()(i, 0); }
+ double &mutable_Y(int i, int j) { return mutable_Y()(i, j); }
const StateFeedbackController<number_of_states, number_of_inputs,
number_of_outputs> &controller() const {
@@ -359,10 +361,10 @@
// it.
virtual void CapU() {
for (int i = 0; i < kNumOutputs; ++i) {
- 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);
+ 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);
}
}
}