Indexer cuts the power after being on at a low power for some time.
diff --git a/frc971/control_loops/index/index.cc b/frc971/control_loops/index/index.cc
index f17164c..11f930f 100644
--- a/frc971/control_loops/index/index.cc
+++ b/frc971/control_loops/index/index.cc
@@ -20,7 +20,7 @@
IndexMotor::IndexMotor(control_loops::IndexLoop *my_index)
: aos::control_loops::ControlLoop<control_loops::IndexLoop>(my_index),
- wrist_loop_(new StateFeedbackLoop<2, 1, 1>(MakeIndexLoop())),
+ wrist_loop_(new IndexStateFeedbackLoop(MakeIndexLoop())),
hopper_disc_count_(0),
total_disc_count_(0),
safe_goal_(Goal::HOLD),
@@ -68,6 +68,12 @@
/*static*/ const int IndexMotor::kShootingDelay = 5;
/*static*/ const int IndexMotor::kLoweringDelay = 20;
+// TODO(aschuh): Tune these.
+/*static*/ const double
+ IndexMotor::IndexStateFeedbackLoop::kMinMotionVoltage = 5.0;
+/*static*/ const double
+ IndexMotor::IndexStateFeedbackLoop::kNoMotionCuttoffCount = 30;
+
// Distance to move the indexer when grabbing a disc.
const double kNextPosition = 10.0;
@@ -141,6 +147,31 @@
return found_start;
}
+void IndexMotor::IndexStateFeedbackLoop::CapU() {
+ // If the voltage has been low for a large number of cycles, cut the motor
+ // power. This is generally very bad controls practice since this isn't LTI,
+ // but we don't really care about tracking anything other than large step
+ // inputs, and the loader doesn't need to be that accurate.
+ if (::std::abs(U(0, 0)) < kMinMotionVoltage) {
+ ++low_voltage_count_;
+ if (low_voltage_count_ > kNoMotionCuttoffCount) {
+ printf("Limiting power from %f to 0\n", U(0, 0));
+ U(0, 0) = 0.0;
+ }
+ } else {
+ low_voltage_count_ = 0;
+ }
+
+ for (int i = 0; i < kNumOutputs; ++i) {
+ if (U[i] > plant.U_max[i]) {
+ U[i] = plant.U_max[i];
+ } else if (U[i] < plant.U_min[i]) {
+ U[i] = plant.U_min[i];
+ }
+ }
+}
+
+
// Positive angle is towards the shooter, and positive power is towards the
// shooter.
void IndexMotor::RunIteration(
@@ -518,9 +549,6 @@
if (output) {
output->intake_voltage = intake_voltage;
output->transfer_voltage = transfer_voltage;
- // TODO(aschuh): Count the number of cycles with power below
- // kFrictionVoltage and if it is too high, turn the motor off.
- // 50 cycles, 5 volts? Need data...
output->index_voltage = wrist_loop_->U(0, 0);
output->loader_up = loader_up_;
output->disc_clamped = disc_clamped_;
diff --git a/frc971/control_loops/index/index.h b/frc971/control_loops/index/index.h
index d0ee666..e581faa 100644
--- a/frc971/control_loops/index/index.h
+++ b/frc971/control_loops/index/index.h
@@ -148,6 +148,28 @@
private:
friend class testing::IndexTest_InvalidStateTest_Test;
+
+ // This class implements the CapU function correctly given all the extra
+ // information that we know about from the wrist motor.
+ class IndexStateFeedbackLoop : public StateFeedbackLoop<2, 1, 1> {
+ public:
+ IndexStateFeedbackLoop(StateFeedbackLoop<2, 1, 1> loop)
+ : StateFeedbackLoop<2, 1, 1>(loop),
+ low_voltage_count_(0) {
+ }
+
+ // Voltage below which the indexer won't move with a disc in it.
+ static const double kMinMotionVoltage;
+ // Maximum number of cycles to apply a low voltage to the motor.
+ static const double kNoMotionCuttoffCount;
+
+ // Caps U, but disables the motor after a number of cycles of inactivity.
+ virtual void CapU();
+ private:
+ // Number of cycles that we have seen a small voltage being applied.
+ uint32_t low_voltage_count_;
+ };
+
// Sets disc_position to the minimum or maximum disc position.
// Returns true if there were discs, and false if there weren't.
// On false, disc_position is left unmodified.
@@ -155,7 +177,7 @@
bool MaxDiscPosition(double *disc_position);
// The state feedback control loop to talk to for the index.
- ::std::unique_ptr<StateFeedbackLoop<2, 1, 1>> wrist_loop_;
+ ::std::unique_ptr<IndexStateFeedbackLoop> wrist_loop_;
// Count of the number of discs that we have collected.
uint32_t hopper_disc_count_;
diff --git a/frc971/control_loops/index/index_lib_test.cc b/frc971/control_loops/index/index_lib_test.cc
index 01f34c8..dd67ae1 100644
--- a/frc971/control_loops/index/index_lib_test.cc
+++ b/frc971/control_loops/index/index_lib_test.cc
@@ -775,6 +775,17 @@
EXPECT_LE(0, static_cast<int>(index_motor_.safe_goal_));
}
+// Tests that the motor is turned off after a number of cycles of low power.
+TEST_F(IndexTest, ZeroPowerAfterTimeout) {
+ LoadNDiscs(4);
+ SimulateNCycles(100);
+
+ // Verify that the motor is hard off. This relies on floating point math
+ // never really getting to 0 unless you set it explicitly.
+ my_index_loop_.output.FetchLatest();
+ EXPECT_EQ(my_index_loop_.output->index_voltage, 0.0);
+}
+
} // namespace testing
} // namespace control_loops
} // namespace frc971