zeroing: Add API to flag errors.
The zeroing class itself won't poke any other processes or classes.
Whatever code is using the zeroing class needs to poll for an error
state using the `error()' function.
Change-Id: Ie872d923c6c85419e4ca55680ab6c7437e284c89
diff --git a/frc971/zeroing/zeroing.cc b/frc971/zeroing/zeroing.cc
index 92f2875..d45c49f 100644
--- a/frc971/zeroing/zeroing.cc
+++ b/frc971/zeroing/zeroing.cc
@@ -24,6 +24,14 @@
zeroed_ = false;
wait_for_index_pulse_ = true;
last_used_index_pulse_count_ = 0;
+ error_ = false;
+}
+
+void ZeroingEstimator::TriggerError() {
+ if (!error_) {
+ LOG(ERROR, "Manually triggered zeroing error.\n");
+ error_ = true;
+ }
}
double ZeroingEstimator::CalculateStartPosition(double start_average,
diff --git a/frc971/zeroing/zeroing.h b/frc971/zeroing/zeroing.h
index 9fa335a..9f9329a 100644
--- a/frc971/zeroing/zeroing.h
+++ b/frc971/zeroing/zeroing.h
@@ -7,8 +7,6 @@
#include "frc971/control_loops/control_loops.q.h"
#include "frc971/constants.h"
-// TODO(pschrader): Create an error API to flag faults/errors etc..
-//
// TODO(pschrader): Flag an error if encoder index pulse is not n revolutions
// away from the last one (i.e. got extra counts from noise, etc..)
//
@@ -33,6 +31,14 @@
// Reset the internal logic so it needs to be re-zeroed.
void Reset();
+ // Manually trigger an internal error. This is used for testing the error
+ // logic.
+ void TriggerError();
+
+ // Returns true if an error has occurred, false otherwise. This gets reset to
+ // false when the Reset() function is called.
+ bool error() const { return error_; }
+
// Returns true if the logic considers the corresponding mechanism to be
// zeroed. It return false otherwise. For example, right after a call to
// Reset() this returns false.
@@ -93,6 +99,9 @@
uint32_t last_used_index_pulse_count_;
// Marker to track whether we're fully zeroed yet or not.
bool zeroed_;
+ // Marker to track whether an error has occurred. This gets reset to false
+ // whenever Reset() is called.
+ bool error_;
};
} // namespace zeroing
diff --git a/frc971/zeroing/zeroing_test.cc b/frc971/zeroing/zeroing_test.cc
index 6798123..2ef6865 100644
--- a/frc971/zeroing/zeroing_test.cc
+++ b/frc971/zeroing/zeroing_test.cc
@@ -237,5 +237,28 @@
ASSERT_DOUBLE_EQ(4.7 * index_diff, estimator.position());
}
+TEST_F(ZeroingTest, BasicErrorAPITest) {
+ const double index_diff = 1.0;
+ ZeroingEstimator estimator(
+ Values::ZeroingConstants{kSampleSize, index_diff, 0.0});
+ PositionSensorSimulator sim(index_diff);
+ sim.Initialize(1.5 * index_diff, index_diff / 3.0, 0.0);
+
+ // Perform a simple move and make sure that no error occured.
+ MoveTo(&sim, &estimator, 3.5 * index_diff);
+ ASSERT_FALSE(estimator.error());
+
+ // Trigger an error and make sure it's reported.
+ estimator.TriggerError();
+ ASSERT_TRUE(estimator.error());
+
+ // Make sure that it can recover after a reset.
+ estimator.Reset();
+ ASSERT_FALSE(estimator.error());
+ MoveTo(&sim, &estimator, 4.5 * index_diff);
+ MoveTo(&sim, &estimator, 5.5 * index_diff);
+ ASSERT_FALSE(estimator.error());
+}
+
} // namespace zeroing
} // namespace frc971