added back explicit 0 indices
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;