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