Tuned indexer stall detection and controller.
Change-Id: I93332461253457730b3051c3e96735b314f1405d
diff --git a/y2017/control_loops/superstructure/hood/hood.h b/y2017/control_loops/superstructure/hood/hood.h
index 9c25cb9..f5daa42 100644
--- a/y2017/control_loops/superstructure/hood/hood.h
+++ b/y2017/control_loops/superstructure/hood/hood.h
@@ -32,7 +32,7 @@
// The zeroing and operating voltages.
static constexpr double kZeroingVoltage = 2.0;
- static constexpr double kOperatingVoltage = 3.0;
+ static constexpr double kOperatingVoltage = 12.0;
void Iterate(const control_loops::HoodGoal *unsafe_goal,
const ::frc971::IndexPosition *position, double *output,
diff --git a/y2017/control_loops/superstructure/indexer/indexer.cc b/y2017/control_loops/superstructure/indexer/indexer.cc
index 78e9528..6186f7f 100644
--- a/y2017/control_loops/superstructure/indexer/indexer.cc
+++ b/y2017/control_loops/superstructure/indexer/indexer.cc
@@ -21,7 +21,6 @@
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};
@@ -61,21 +60,15 @@
double IndexerController::voltage() const { return loop_->U(0, 0); }
-double IndexerController::StuckRatio() const {
- double applied_voltage = voltage();
+double IndexerController::StuckVoltage() const {
+ const double applied_voltage = voltage() + loop_->X_hat(2, 0);
if (applied_voltage < 0) {
- applied_voltage = ::std::min(applied_voltage, -kMinStuckVoltage);
+ return +stuck_indexer_X_hat_current_(2, 0) + applied_voltage;
} else {
- applied_voltage = ::std::max(applied_voltage, kMinStuckVoltage);
+ return -stuck_indexer_X_hat_current_(2, 0) - applied_voltage;
}
- // 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; }
+bool IndexerController::IsStuck() const { return StuckVoltage() > 1.5; }
void IndexerController::Reset() { reset_ = true; }
@@ -139,7 +132,7 @@
status->stuck = IsStuck();
- status->stuck_ratio = StuckRatio();
+ status->stuck_voltage = StuckVoltage();
}
void Indexer::Reset() { indexer_.Reset(); }
diff --git a/y2017/control_loops/superstructure/indexer/indexer.h b/y2017/control_loops/superstructure/indexer/indexer.h
index 5c1cc1b..9d71eed 100644
--- a/y2017/control_loops/superstructure/indexer/indexer.h
+++ b/y2017/control_loops/superstructure/indexer/indexer.h
@@ -39,7 +39,7 @@
// Returns true if the indexer is stuck.
bool IsStuck() const;
- double StuckRatio() const;
+ double StuckVoltage() const;
// Executes the control loop for a cycle.
void Update(bool disabled);
diff --git a/y2017/control_loops/superstructure/superstructure.q b/y2017/control_loops/superstructure/superstructure.q
index b4e6c9b..427dadb 100644
--- a/y2017/control_loops/superstructure/superstructure.q
+++ b/y2017/control_loops/superstructure/superstructure.q
@@ -69,7 +69,7 @@
// True if the indexer is stuck.
bool stuck;
- float stuck_ratio;
+ float stuck_voltage;
// The state of the indexer state machine.
int32_t state;