Added indexer support, with stall detection.
Re-tuned some other loops while I was learning more about the indexer.
Change-Id: I893f1f273b31357ae9137601ca27322fc3b09d98
diff --git a/y2017/control_loops/superstructure/indexer/indexer.h b/y2017/control_loops/superstructure/indexer/indexer.h
new file mode 100644
index 0000000..5c1cc1b
--- /dev/null
+++ b/y2017/control_loops/superstructure/indexer/indexer.h
@@ -0,0 +1,121 @@
+#ifndef Y2017_CONTROL_LOOPS_INDEXER_INDEXER_H_
+#define Y2017_CONTROL_LOOPS_INDEXER_INDEXER_H_
+
+#include <memory>
+
+#include "aos/common/controls/control_loop.h"
+#include "aos/common/time.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+
+#include "y2017/control_loops/superstructure/indexer/indexer_integral_plant.h"
+#include "y2017/control_loops/superstructure/superstructure.q.h"
+
+namespace y2017 {
+namespace control_loops {
+namespace superstructure {
+namespace indexer {
+
+class IndexerController {
+ public:
+ IndexerController();
+
+ // Sets the velocity goal in radians/sec
+ void set_goal(double angular_velocity_goal);
+ // Sets the current encoder position in radians
+ void set_position(double current_position);
+
+ // Populates the status structure.
+ void SetStatus(control_loops::IndexerStatus *status);
+
+ // Returns the control loop calculated voltage.
+ double voltage() const;
+
+ // Returns the instantaneous velocity.
+ double velocity() const { return loop_->X_hat(1, 0); }
+
+ double dt_velocity() const { return dt_velocity_; }
+
+ double error() const { return error_; }
+
+ // Returns true if the indexer is stuck.
+ bool IsStuck() const;
+ double StuckRatio() const;
+
+ // Executes the control loop for a cycle.
+ void Update(bool disabled);
+
+ // Resets the kalman filter and any other internal state.
+ void Reset();
+ // Resets the voltage error for when we reverse directions.
+ void PartialReset();
+
+ private:
+ // The current sensor measurement.
+ Eigen::Matrix<double, 1, 1> Y_;
+ // The control loop.
+ ::std::unique_ptr<StateFeedbackLoop<3, 1, 1>> loop_;
+
+ // The stuck indexer detector
+ ::std::unique_ptr<StateFeedbackLoop<3, 1, 1>> stuck_indexer_detector_;
+
+ // History array for calculating a filtered angular velocity.
+ static constexpr int kHistoryLength = 5;
+ ::std::array<double, kHistoryLength> history_;
+ ptrdiff_t history_position_ = 0;
+
+ double error_ = 0.0;
+ double dt_velocity_ = 0.0;
+ double last_position_ = 0.0;
+ double average_angular_velocity_ = 0.0;
+ double position_error_ = 0.0;
+
+ Eigen::Matrix<double, 3, 1> X_hat_current_;
+ Eigen::Matrix<double, 3, 1> stuck_indexer_X_hat_current_;
+
+ bool ready_ = false;
+ bool reset_ = false;
+
+ DISALLOW_COPY_AND_ASSIGN(IndexerController);
+};
+
+class Indexer {
+ public:
+ Indexer() {}
+
+ enum class State {
+ // The system is running correctly, no stuck condition detected.
+ RUNNING = 0,
+ // The system is reversing to unjam.
+ REVERSING = 1
+ };
+
+ // Iterates the indexer control loop one cycle. position and status must
+ // never be nullptr. goal can be nullptr if no goal exists, and output should
+ // be nullptr if disabled.
+ void Iterate(const control_loops::IndexerGoal *goal, const double *position,
+ double *output, control_loops::IndexerStatus *status);
+
+ // Sets the indexer up to reset the kalman filter next time Iterate is called.
+ void Reset();
+
+ State state() const { return state_; }
+
+ private:
+ IndexerController indexer_;
+
+ State state_ = State::RUNNING;
+
+ // The last time that we transitioned from RUNNING to REVERSING or the
+ // reverse. Used to implement the timeouts.
+ ::aos::monotonic_clock::time_point last_transition_time_ =
+ ::aos::monotonic_clock::min_time;
+
+ DISALLOW_COPY_AND_ASSIGN(Indexer);
+};
+
+} // namespace indexer
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2017
+
+#endif // Y2017_CONTROL_LOOPS_INDEXER_INDEXER_H_