Put a plant inside the loop and moved A,B,etc there.
Change-Id: I9cb3a1a16bd0ccda0c9287514577b3c3861bc42f
diff --git a/y2014/control_loops/shooter/shooter.cc b/y2014/control_loops/shooter/shooter.cc
index af310f0..9c6b182 100644
--- a/y2014/control_loops/shooter/shooter.cc
+++ b/y2014/control_loops/shooter/shooter.cc
@@ -56,11 +56,11 @@
void ZeroedStateFeedbackLoop::CapGoal() {
if (uncapped_voltage() > max_voltage_) {
double dx;
- if (controller_index() == 0) {
+ if (index() == 0) {
dx = (uncapped_voltage() - max_voltage_) /
- (K(0, 0) - A(1, 0) * K(0, 2) / A(1, 2));
+ (K(0, 0) - plant().A(1, 0) * K(0, 2) / plant().A(1, 2));
mutable_R(0, 0) -= dx;
- mutable_R(2, 0) -= -A(1, 0) / A(1, 2) * dx;
+ mutable_R(2, 0) -= -plant().A(1, 0) / plant().A(1, 2) * dx;
} else {
dx = (uncapped_voltage() - max_voltage_) / K(0, 0);
mutable_R(0, 0) -= dx;
@@ -70,11 +70,11 @@
::y2014::control_loops::ShooterMovingGoal(dx));
} else if (uncapped_voltage() < -max_voltage_) {
double dx;
- if (controller_index() == 0) {
+ if (index() == 0) {
dx = (uncapped_voltage() + max_voltage_) /
- (K(0, 0) - A(1, 0) * K(0, 2) / A(1, 2));
+ (K(0, 0) - plant().A(1, 0) * K(0, 2) / plant().A(1, 2));
mutable_R(0, 0) -= dx;
- mutable_R(2, 0) -= -A(1, 0) / A(1, 2) * dx;
+ mutable_R(2, 0) -= -plant().A(1, 0) / plant().A(1, 2) * dx;
} else {
dx = (uncapped_voltage() + max_voltage_) / K(0, 0);
mutable_R(0, 0) -= dx;
@@ -88,10 +88,11 @@
}
void ZeroedStateFeedbackLoop::RecalculatePowerGoal() {
- if (controller_index() == 0) {
- mutable_R(2, 0) = (-A(1, 0) / A(1, 2) * R(0, 0) - A(1, 1) / A(1, 2) * R(1, 0));
+ if (index() == 0) {
+ mutable_R(2, 0) = (-plant().A(1, 0) / plant().A(1, 2) * R(0, 0) -
+ plant().A(1, 1) / plant().A(1, 2) * R(1, 0));
} else {
- mutable_R(2, 0) = -A(1, 1) / A(1, 2) * R(1, 0);
+ mutable_R(2, 0) = -plant().A(1, 1) / plant().A(1, 2) * R(1, 0);
}
}
@@ -104,8 +105,8 @@
mutable_X_hat(0, 0) += doffset;
// Offset the goal so we don't move.
mutable_R(0, 0) += doffset;
- if (controller_index() == 0) {
- mutable_R(2, 0) += -A(1, 0) / A(1, 2) * (doffset);
+ if (index() == 0) {
+ mutable_R(2, 0) += -plant().A(1, 0) / plant().A(1, 2) * (doffset);
}
LOG_STRUCT(DEBUG, "sensor edge (fake?)",
::y2014::control_loops::ShooterChangeCalibration(
@@ -257,16 +258,16 @@
// Probably not needed yet.
if (position) {
- int last_controller_index = shooter_.controller_index();
+ int last_index = shooter_.index();
if (position->plunger && position->latch) {
// Use the controller without the spring if the latch is set and the
// plunger is back
- shooter_.set_controller_index(1);
+ shooter_.set_index(1);
} else {
// Otherwise use the controller with the spring.
- shooter_.set_controller_index(0);
+ shooter_.set_index(0);
}
- if (shooter_.controller_index() != last_controller_index) {
+ if (shooter_.index() != last_index) {
shooter_.RecalculatePowerGoal();
}
}
diff --git a/y2014/control_loops/shooter/shooter.h b/y2014/control_loops/shooter/shooter.h
index fb991e3..f2ddbc8 100644
--- a/y2014/control_loops/shooter/shooter.h
+++ b/y2014/control_loops/shooter/shooter.h
@@ -83,8 +83,9 @@
desired_velocity);
mutable_R() << desired_position - kPositionOffset, desired_velocity,
- (-A(1, 0) / A(1, 2) * (desired_position - kPositionOffset) -
- A(1, 1) / A(1, 2) * desired_velocity);
+ (-plant().A(1, 0) / plant().A(1, 2) *
+ (desired_position - kPositionOffset) -
+ plant().A(1, 1) / plant().A(1, 2) * desired_velocity);
}
double position() const { return X_hat(0, 0) - offset_ + kPositionOffset; }
diff --git a/y2014/control_loops/shooter/shooter_lib_test.cc b/y2014/control_loops/shooter/shooter_lib_test.cc
index 5463265..df3f5fd 100644
--- a/y2014/control_loops/shooter/shooter_lib_test.cc
+++ b/y2014/control_loops/shooter/shooter_lib_test.cc
@@ -102,12 +102,12 @@
// Only disengage the spring if we are greater than 0, which is where the
// latch will take the load off the pusher.
if (GetAbsolutePosition() > 0.0) {
- shooter_plant_->set_plant_index(1);
+ shooter_plant_->set_index(1);
} else {
- shooter_plant_->set_plant_index(0);
+ shooter_plant_->set_index(0);
}
} else {
- shooter_plant_->set_plant_index(0);
+ shooter_plant_->set_index(0);
position->plunger =
CheckRange(GetAbsolutePosition(), values.shooter.plunger_back);
}
@@ -228,7 +228,7 @@
U << last_voltage_;
shooter_plant_->Update(U);
}
- LOG(DEBUG, "Plant index is %d\n", shooter_plant_->plant_index());
+ LOG(DEBUG, "Plant index is %d\n", shooter_plant_->index());
// Handle latch hall effect
if (!latch_piston_state_ && latch_delay_count_ > 0) {