Added column and tests
The column works! We can also shut the intake down for hanging.
Change-Id: I4369d489d1a07a688f204fd9bb00ef7ad787f5a3
diff --git a/frc971/control_loops/profiled_subsystem.h b/frc971/control_loops/profiled_subsystem.h
index 6ebba72..84803af 100644
--- a/frc971/control_loops/profiled_subsystem.h
+++ b/frc971/control_loops/profiled_subsystem.h
@@ -55,6 +55,7 @@
for (auto &estimator : estimators_) {
estimator.Reset();
}
+ should_reset_ = true;
}
// Returns the controller.
@@ -98,6 +99,9 @@
return loop_->X_hat();
}
double X_hat(int row, int col) const { return loop_->X_hat(row, col); }
+ double &mutable_X_hat(int row, int col) const {
+ return loop_->mutable_X_hat(row, col);
+ }
// Returns the current internal estimator state for logging.
typename ZeroingEstimator::State EstimatorState(int index) {
@@ -114,8 +118,6 @@
protected:
void set_zeroed(int index, bool val) { zeroed_[index] = val; }
- // TODO(austin): It's a bold assumption to assume that we will have the same
- // number of sensors as axes. So far, that's been fine.
::std::unique_ptr<::frc971::control_loops::SimpleCappedStateFeedbackLoop<
number_of_states, number_of_inputs, number_of_outputs>>
loop_;
@@ -125,6 +127,10 @@
bool initialized_ = false;
+ // If true, the subclass should reset in Update. It should then clear this
+ // flag.
+ bool should_reset_ = true;
+
::std::array<ZeroingEstimator, number_of_axes> estimators_;
private:
@@ -334,6 +340,15 @@
template <class ZeroingEstimator>
void SingleDOFProfiledSubsystem<ZeroingEstimator>::Update(bool disable) {
+ // TODO(austin): What do we want to do with the profile on reset? Also, we
+ // should probably reset R, the offset, the profile, etc.
+ if (this->should_reset_) {
+ this->loop_->mutable_X_hat(0, 0) = Y_(0, 0);
+ this->loop_->mutable_X_hat(1, 0) = 0.0;
+ this->loop_->mutable_X_hat(2, 0) = 0.0;
+ this->should_reset_ = false;
+ }
+
if (!disable) {
::Eigen::Matrix<double, 2, 1> goal_state = profile_.Update(
this->unprofiled_goal_(0, 0), this->unprofiled_goal_(1, 0));