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/BUILD b/y2017/control_loops/superstructure/indexer/BUILD
index 3df0810..10b726f 100644
--- a/y2017/control_loops/superstructure/indexer/BUILD
+++ b/y2017/control_loops/superstructure/indexer/BUILD
@@ -9,6 +9,8 @@
'indexer_plant.cc',
'indexer_integral_plant.h',
'indexer_integral_plant.cc',
+ 'stuck_indexer_integral_plant.h',
+ 'stuck_indexer_integral_plant.cc',
],
)
@@ -18,12 +20,31 @@
srcs = [
'indexer_plant.cc',
'indexer_integral_plant.cc',
+ 'stuck_indexer_integral_plant.cc',
],
hdrs = [
'indexer_plant.h',
'indexer_integral_plant.h',
+ 'stuck_indexer_integral_plant.h',
],
deps = [
'//frc971/control_loops:state_feedback_loop',
],
)
+
+cc_library(
+ name = 'indexer',
+ visibility = ['//visibility:public'],
+ srcs = [
+ 'indexer.cc',
+ ],
+ hdrs = [
+ 'indexer.h',
+ ],
+ deps = [
+ ':indexer_plants',
+ '//aos/common/controls:control_loop',
+ '//aos/common:math',
+ '//y2017/control_loops/superstructure:superstructure_queue',
+ ],
+)
diff --git a/y2017/control_loops/superstructure/indexer/indexer.cc b/y2017/control_loops/superstructure/indexer/indexer.cc
new file mode 100644
index 0000000..cab9864
--- /dev/null
+++ b/y2017/control_loops/superstructure/indexer/indexer.cc
@@ -0,0 +1,206 @@
+#include "y2017/control_loops/superstructure/indexer/indexer.h"
+
+#include <chrono>
+
+#include "aos/common/commonmath.h"
+#include "aos/common/controls/control_loops.q.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/logging/queue_logging.h"
+#include "aos/common/time.h"
+#include "y2017/control_loops/superstructure/indexer/indexer_integral_plant.h"
+#include "y2017/control_loops/superstructure/indexer/stuck_indexer_integral_plant.h"
+#include "y2017/control_loops/superstructure/superstructure.q.h"
+
+namespace y2017 {
+namespace control_loops {
+namespace superstructure {
+namespace indexer {
+
+namespace chrono = ::std::chrono;
+using ::aos::monotonic_clock;
+
+namespace {
+constexpr double kTolerance = 10.0;
+constexpr double kMinStuckVoltage = 3.0;
+constexpr chrono::milliseconds kForwardTimeout{500};
+constexpr chrono::milliseconds kReverseTimeout{500};
+constexpr chrono::milliseconds kReverseMinTimeout{100};
+} // namespace
+
+// TODO(austin): Pseudo current limit?
+
+IndexerController::IndexerController()
+ : loop_(new StateFeedbackLoop<3, 1, 1>(
+ superstructure::indexer::MakeIntegralIndexerLoop())),
+ stuck_indexer_detector_(new StateFeedbackLoop<3, 1, 1>(
+ superstructure::indexer::MakeStuckIntegralIndexerLoop())) {
+ history_.fill(0);
+ Y_.setZero();
+ X_hat_current_.setZero();
+ stuck_indexer_X_hat_current_.setZero();
+}
+
+void IndexerController::set_goal(double angular_velocity_goal) {
+ loop_->mutable_next_R() << 0.0, angular_velocity_goal, 0.0;
+}
+
+void IndexerController::set_position(double current_position) {
+ // Update position in the model.
+ Y_ << current_position;
+
+ // Add the position to the history.
+ history_[history_position_] = current_position;
+ history_position_ = (history_position_ + 1) % kHistoryLength;
+
+ dt_velocity_ = (current_position - last_position_) /
+ chrono::duration_cast<chrono::duration<double>>(
+ ::aos::controls::kLoopFrequency)
+ .count();
+ last_position_ = current_position;
+}
+
+double IndexerController::voltage() const { return loop_->U(0, 0); }
+
+double IndexerController::StuckRatio() const {
+ double applied_voltage = voltage();
+ if (applied_voltage < 0) {
+ applied_voltage = ::std::min(applied_voltage, -kMinStuckVoltage);
+ } else {
+ applied_voltage = ::std::max(applied_voltage, kMinStuckVoltage);
+ }
+ // Look at the ratio of the current controller power to the voltage_error
+ // term. If our output is dominated by the voltage_error, then we are likely
+ // pretty stuck and should try reversing.
+ // We don't want to worry about dividing by zero, so keep the applied voltage
+ // away from 0 though a min/max.
+ return -stuck_indexer_X_hat_current_(2, 0) / applied_voltage;
+}
+bool IndexerController::IsStuck() const { return StuckRatio() > 0.6; }
+
+void IndexerController::Reset() { reset_ = true; }
+
+void IndexerController::PartialReset() { loop_->mutable_X_hat(2, 0) = 0.0; }
+
+void IndexerController::Update(bool disabled) {
+ loop_->mutable_R() = loop_->next_R();
+ if (::std::abs(loop_->R(1, 0)) < 0.1) {
+ // Kill power at low angular velocities.
+ disabled = true;
+ }
+
+ if (reset_) {
+ loop_->mutable_X_hat(0, 0) = Y_(0, 0);
+ loop_->mutable_X_hat(1, 0) = 0.0;
+ loop_->mutable_X_hat(2, 0) = 0.0;
+ stuck_indexer_detector_->mutable_X_hat(0, 0) = Y_(0, 0);
+ stuck_indexer_detector_->mutable_X_hat(1, 0) = 0.0;
+ stuck_indexer_detector_->mutable_X_hat(2, 0) = 0.0;
+ reset_ = false;
+ }
+
+ loop_->Correct(Y_);
+ stuck_indexer_detector_->Correct(Y_);
+
+ // Compute the oldest point in the history.
+ const int oldest_history_position =
+ ((history_position_ == 0) ? kHistoryLength : history_position_) - 1;
+
+ // Compute the distance moved over that time period.
+ average_angular_velocity_ =
+ (history_[oldest_history_position] - history_[history_position_]) /
+ (chrono::duration_cast<chrono::duration<double>>(
+ ::aos::controls::kLoopFrequency)
+ .count() *
+ static_cast<double>(kHistoryLength - 1));
+
+ // Ready if average angular velocity is close to the goal.
+ error_ = average_angular_velocity_ - loop_->next_R(1, 0);
+
+ ready_ = std::abs(error_) < kTolerance && loop_->next_R(1, 0) > 1.0;
+
+ X_hat_current_ = loop_->X_hat();
+ stuck_indexer_X_hat_current_ = stuck_indexer_detector_->X_hat();
+ position_error_ = X_hat_current_(0, 0) - Y_(0, 0);
+
+ loop_->Update(disabled);
+ stuck_indexer_detector_->UpdateObserver(loop_->U());
+}
+
+void IndexerController::SetStatus(IndexerStatus *status) {
+ status->avg_angular_velocity = average_angular_velocity_;
+
+ status->angular_velocity = X_hat_current_(1, 0);
+ status->ready = ready_;
+
+ status->voltage_error = X_hat_current_(2, 0);
+ status->stuck_voltage_error = stuck_indexer_X_hat_current_(2, 0);
+ status->position_error = position_error_;
+ status->instantaneous_velocity = dt_velocity_;
+
+ status->stuck = IsStuck();
+
+ status->stuck_ratio = StuckRatio();
+}
+
+void Indexer::Reset() { indexer_.Reset(); }
+
+void Indexer::Iterate(const control_loops::IndexerGoal *goal,
+ const double *position, double *output,
+ control_loops::IndexerStatus *status) {
+ if (goal) {
+ // Start indexing at the suggested velocity.
+ // If a "stuck" event is detected, reverse. Stay reversed until either
+ // unstuck, or 0.5 seconds have elapsed.
+ // Then, start going forwards. Don't detect stuck for 0.5 seconds.
+
+ monotonic_clock::time_point monotonic_now = monotonic_clock::now();
+ switch (state_) {
+ case State::RUNNING:
+ // Pass the velocity goal through.
+ indexer_.set_goal(goal->angular_velocity);
+ // If we are stuck and weren't just reversing, try reversing to unstick
+ // us. We don't want to chatter back and forth too fast if reversing
+ // isn't working.
+ if (indexer_.IsStuck() &&
+ monotonic_now > kForwardTimeout + last_transition_time_) {
+ state_ = State::REVERSING;
+ last_transition_time_ = monotonic_now;
+ indexer_.Reset();
+ }
+ break;
+ case State::REVERSING:
+ // "Reverse" "slowly".
+ indexer_.set_goal(-5.0 * aos::sign(goal->angular_velocity));
+
+ // If we've timed out or are no longer stuck, try running again.
+ if ((!indexer_.IsStuck() &&
+ monotonic_now > last_transition_time_ + kReverseMinTimeout) ||
+ monotonic_now > kReverseTimeout + last_transition_time_) {
+ state_ = State::RUNNING;
+ last_transition_time_ = monotonic_now;
+
+ // Only reset if we got stuck going this way too.
+ if (monotonic_now > kReverseTimeout + last_transition_time_) {
+ indexer_.Reset();
+ }
+ }
+ break;
+ }
+ }
+
+ indexer_.set_position(*position);
+
+ indexer_.Update(output == nullptr);
+
+ indexer_.SetStatus(status);
+ status->state = static_cast<int32_t>(state_);
+
+ if (output) {
+ *output = indexer_.voltage();
+ }
+}
+
+} // namespace indexer
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2017
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_