Add Encoder Fault Detector to frc971 Control Loop
The Encoder Fault Detector will take in a Position and CANPosition message to test whether either encoder or CAN encoders in a subsystem are faulted
Signed-off-by: Niko Sohmers <nikolai@sohmers.com>
Change-Id: I544772a809fbe4dc26dc0e37ef3c4b2ae4def75e
diff --git a/frc971/control_loops/BUILD b/frc971/control_loops/BUILD
index 925dd43..9986fef 100644
--- a/frc971/control_loops/BUILD
+++ b/frc971/control_loops/BUILD
@@ -176,6 +176,30 @@
)
cc_library(
+ name = "encoder_fault_detector",
+ srcs = ["encoder_fault_detector.cc"],
+ hdrs = ["encoder_fault_detector.h"],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":encoder_fault_status_fbs",
+ "//aos/containers:sized_array",
+ "//aos/time",
+ "//frc971/control_loops:can_talonfx_fbs",
+ ],
+)
+
+cc_test(
+ name = "encoder_fault_detector_test",
+ srcs = ["encoder_fault_detector_test.cc"],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":encoder_fault_detector",
+ "//aos:json_to_flatbuffer",
+ "//aos/testing:googletest",
+ ],
+)
+
+cc_library(
name = "hall_effect_tracker",
hdrs = [
"hall_effect_tracker.h",
@@ -201,6 +225,20 @@
)
flatbuffer_ts_library(
+ name = "encoder_fault_status_ts_fbs",
+ srcs = [
+ "encoder_fault_status.fbs",
+ ],
+)
+
+static_flatbuffer(
+ name = "encoder_fault_status_fbs",
+ srcs = [
+ "encoder_fault_status.fbs",
+ ],
+)
+
+flatbuffer_ts_library(
name = "can_talonfx_ts_fbs",
srcs = [
"can_talonfx.fbs",
diff --git a/frc971/control_loops/encoder_fault_detector.cc b/frc971/control_loops/encoder_fault_detector.cc
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/frc971/control_loops/encoder_fault_detector.cc
diff --git a/frc971/control_loops/encoder_fault_detector.h b/frc971/control_loops/encoder_fault_detector.h
new file mode 100644
index 0000000..0be3464
--- /dev/null
+++ b/frc971/control_loops/encoder_fault_detector.h
@@ -0,0 +1,127 @@
+#ifndef FRC971_CONTROL_LOOPS_ENCODER_FAULT_DETECTOR_H_
+#define FRC971_CONTROL_LOOPS_ENCODER_FAULT_DETECTOR_H_
+
+#include "aos/containers/sized_array.h"
+#include "aos/time/time.h"
+#include "frc971/control_loops/can_talonfx_generated.h"
+#include "frc971/control_loops/encoder_fault_status_generated.h"
+
+namespace frc971 {
+namespace control_loops {
+// The EncoderFaultDetector class will check for faults within a subsystem by
+// taking in an array of CAN encoder positions and an encoder position. When any
+// encoder gives a value that does not align with the others, the class will set
+// faulted to true
+template <uint8_t NumberofMotors>
+class EncoderFaultDetector {
+ public:
+ enum EncoderState {
+ SAME,
+ INCREASING,
+ DECREASING,
+ };
+
+ void Iterate(double encoder_position,
+ const flatbuffers::Vector<
+ flatbuffers::Offset<frc971::control_loops::CANTalonFX>>
+ *falcon_positions,
+ aos::monotonic_clock::time_point current_time);
+
+ void Iterate(double encoder_position,
+ aos::SizedArray<double, NumberofMotors> motor_positions,
+ aos::monotonic_clock::time_point current_time);
+
+ flatbuffers::Offset<EncoderFaultStatus> PopulateStatus(
+ flatbuffers::FlatBufferBuilder *fbb);
+
+ bool isfaulted() { return faulted_; }
+
+ private:
+ // The amount of time in milliseconds it will take before new data is ignored
+ static constexpr std::chrono::milliseconds kMaxTimeWithNoUpdate{10};
+ // Whether the encoders are faulted or not
+ bool faulted_ = false;
+ // The previous encoder position
+ double prev_encoder_position_;
+ // The previous motor positions
+ aos::SizedArray<double, NumberofMotors> prev_motor_positions_;
+ // A timestamp representing the last time data was updated
+ aos::monotonic_clock::time_point last_accepted_time_{
+ aos::monotonic_clock::min_time};
+};
+
+template <uint8_t NumberofMotors>
+void EncoderFaultDetector<NumberofMotors>::Iterate(
+ double encoder_position,
+ const flatbuffers::Vector<
+ flatbuffers::Offset<frc971::control_loops::CANTalonFX>>
+ *falcon_positions,
+ aos::monotonic_clock::time_point current_time) {
+ aos::SizedArray<double, NumberofMotors> motors;
+
+ for (const auto &motor : *falcon_positions) {
+ motors.push_back(motor->position());
+ }
+
+ Iterate(encoder_position, motors, current_time);
+}
+
+template <uint8_t NumberofMotors>
+void EncoderFaultDetector<NumberofMotors>::Iterate(
+ double encoder_position,
+ aos::SizedArray<double, NumberofMotors> motor_positions,
+ aos::monotonic_clock::time_point current_time) {
+ if (prev_motor_positions_.empty()) {
+ prev_encoder_position_ = encoder_position;
+ prev_motor_positions_ = motor_positions;
+ last_accepted_time_ = current_time;
+ return;
+ }
+
+ aos::monotonic_clock::duration time_elapsed =
+ current_time - last_accepted_time_;
+
+ // If more than 10 milliseconds has passed, then do not check for a fault
+ if (time_elapsed <= EncoderFaultDetector::kMaxTimeWithNoUpdate) {
+ constexpr auto get_encoder_state =
+ [](const double now, const double previous) -> EncoderState {
+ if (now > previous) {
+ return INCREASING;
+ }
+ if (now < previous) {
+ return DECREASING;
+ }
+ return SAME;
+ };
+
+ for (uint64_t i = 0; i < motor_positions.size(); i++) {
+ const EncoderState encoder =
+ get_encoder_state(encoder_position, prev_encoder_position_);
+ const EncoderState motor =
+ get_encoder_state(motor_positions[i], prev_motor_positions_[i]);
+
+ if (encoder != motor) { // If the encoder and the motor states are not
+ // the same set faulted to true
+ faulted_ = true;
+ }
+ }
+ }
+
+ prev_encoder_position_ = encoder_position;
+ prev_motor_positions_ = motor_positions;
+ last_accepted_time_ = current_time;
+}
+
+template <uint8_t NumberofMotors>
+flatbuffers::Offset<EncoderFaultStatus>
+EncoderFaultDetector<NumberofMotors>::PopulateStatus(
+ flatbuffers::FlatBufferBuilder *fbb) {
+ EncoderFaultStatus::Builder builder(*fbb);
+ builder.add_faulted(faulted_);
+ return builder.Finish();
+}
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_ENCODER_FAULT_DETECTOR_H_
\ No newline at end of file
diff --git a/frc971/control_loops/encoder_fault_detector_test.cc b/frc971/control_loops/encoder_fault_detector_test.cc
new file mode 100644
index 0000000..a4974ef
--- /dev/null
+++ b/frc971/control_loops/encoder_fault_detector_test.cc
@@ -0,0 +1,244 @@
+#include "frc971/control_loops/encoder_fault_detector.h"
+
+#include "gtest/gtest.h"
+
+#include "aos/json_to_flatbuffer.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+
+// Test for simulating if encoder values are idle.
+TEST(EncoderFaultDetector, Idle) {
+ EncoderFaultDetector<2> detector;
+ aos::monotonic_clock::time_point t;
+
+ double encoder_position = 0.0;
+ aos::SizedArray<double, 2> falcon_positions = {0.0, 0.0};
+
+ for (int i = 0; i < 10; i++) {
+ detector.Iterate(encoder_position, falcon_positions, t);
+ t += std::chrono::milliseconds(5);
+ }
+
+ EXPECT_FALSE(detector.isfaulted());
+}
+
+// Test for simulating if we have three motors
+TEST(EncoderFaultDetector, ThreeMotors) {
+ EncoderFaultDetector<3> detector;
+ aos::monotonic_clock::time_point t;
+
+ double encoder_position = 0.0;
+ aos::SizedArray<double, 3> falcon_positions = {0.0, 0.0, 0.0};
+
+ for (int i = 0; i < 10; i++) {
+ detector.Iterate(encoder_position, falcon_positions, t);
+ t += std::chrono::milliseconds(5);
+ }
+
+ EXPECT_FALSE(detector.isfaulted());
+}
+
+// Test for simulating faulting with three motors
+TEST(EncoderFaultDetector, FaultThreeMotors) {
+ EncoderFaultDetector<3> detector;
+ aos::monotonic_clock::time_point t;
+
+ double encoder_position = 0.0;
+ aos::SizedArray<double, 3> falcon_positions = {0.0, 0.0, 0.0};
+
+ for (int i = 0; i < 10; i++) {
+ for (double &falcon : falcon_positions) {
+ falcon++;
+ }
+ detector.Iterate(encoder_position, falcon_positions, t);
+ t += std::chrono::milliseconds(5);
+ }
+
+ EXPECT_TRUE(detector.isfaulted());
+}
+
+// Test for simulating if encoder values are increasing.
+TEST(EncoderFaultDetector, Increasing) {
+ EncoderFaultDetector<2> detector;
+ aos::monotonic_clock::time_point t;
+
+ double encoder_position = 0.0;
+ aos::SizedArray<double, 2> falcon_positions = {0.0, 0.0};
+
+ for (int i = 0; i < 10; ++i) {
+ detector.Iterate(encoder_position, falcon_positions, t);
+ encoder_position++;
+ for (double &falcon : falcon_positions) {
+ falcon++;
+ }
+ t += std::chrono::milliseconds(5);
+ }
+
+ EXPECT_FALSE(detector.isfaulted());
+}
+
+// Test for simulating if encoder values are decreasing.
+TEST(EncoderFaultDetector, Decreasing) {
+ EncoderFaultDetector<2> detector;
+ aos::monotonic_clock::time_point t;
+
+ double encoder_position = 0.0;
+ aos::SizedArray<double, 2> falcon_positions = {0.0, 0.0};
+
+ for (int i = 0; i < 10; ++i) {
+ detector.Iterate(encoder_position, falcon_positions, t);
+ encoder_position--;
+ for (double &falcon : falcon_positions) {
+ falcon--;
+ }
+ t += std::chrono::milliseconds(5);
+ }
+
+ EXPECT_FALSE(detector.isfaulted());
+}
+
+// Test for simulating if only falcon values are increasing.
+TEST(EncoderFaultDetector, FalconsOnlyIncrease) {
+ EncoderFaultDetector<2> detector;
+ aos::monotonic_clock::time_point t;
+
+ double encoder_position = 0.0;
+ aos::SizedArray<double, 2> falcon_positions = {0.0, 0.0};
+
+ for (int i = 0; i < 5; ++i) {
+ detector.Iterate(encoder_position, falcon_positions, t);
+ for (double &falcon : falcon_positions) {
+ falcon++;
+ }
+ t += std::chrono::milliseconds(5);
+ }
+
+ EXPECT_TRUE(detector.isfaulted());
+}
+
+// Test for simulating if only encoder value is increasing.
+TEST(EncoderFaultDetector, EncoderOnlyIncrease) {
+ EncoderFaultDetector<2> detector;
+ aos::monotonic_clock::time_point t;
+
+ double encoder_position = 0.0;
+ aos::SizedArray<double, 2> falcon_positions = {0.0, 0.0};
+
+ for (int i = 0; i < 5; ++i) {
+ detector.Iterate(encoder_position, falcon_positions, t);
+ encoder_position++;
+ t += std::chrono::milliseconds(5);
+ }
+
+ EXPECT_TRUE(detector.isfaulted());
+}
+
+// Test for simulating if only one falcon value is increasing at a time.
+TEST(EncoderFaultDetector, OnlyOneFalconIncreases) {
+ EncoderFaultDetector<2> detector;
+ aos::monotonic_clock::time_point t;
+
+ double encoder_position = 0.0;
+ aos::SizedArray<double, 2> falcon_positions = {0.0, 0.0};
+
+ for (int i = 0; i < 5; ++i) {
+ detector.Iterate(encoder_position, falcon_positions, t);
+ falcon_positions[0] += 0.1;
+ t += std::chrono::milliseconds(5);
+ }
+
+ for (int i = 0; i < 5; ++i) {
+ detector.Iterate(encoder_position, falcon_positions, t);
+ falcon_positions[1] += 0.1;
+ t += std::chrono::milliseconds(5);
+ }
+
+ EXPECT_TRUE(detector.isfaulted());
+}
+
+// Test checks that the detector stays faulted after a fault if the encoder
+// positions align again
+TEST(EncoderFaultDetector, StaysFaulted) {
+ EncoderFaultDetector<2> detector;
+ aos::monotonic_clock::time_point t;
+
+ double encoder_position = 0.0;
+ aos::SizedArray<double, 2> falcon_positions = {0.0, 0.0};
+
+ for (int i = 0; i < 5; ++i) {
+ detector.Iterate(encoder_position, falcon_positions, t);
+ encoder_position += 0.1;
+ t += std::chrono::milliseconds(5);
+ }
+
+ EXPECT_TRUE(detector.isfaulted());
+
+ for (int i = 0; i < 5; ++i) {
+ detector.Iterate(encoder_position, falcon_positions, t);
+ encoder_position += 0.1;
+ for (double &falcon : falcon_positions) {
+ falcon += 0.1;
+ }
+ t += std::chrono::milliseconds(5);
+ }
+
+ EXPECT_TRUE(detector.isfaulted());
+}
+
+// Tests that after 10 milliseconds updates will not register
+TEST(EncoderFaultDetector, NoUpdateForTooLong) {
+ EncoderFaultDetector<2> detector;
+ aos::monotonic_clock::time_point t;
+
+ double encoder_position = 0.0;
+ aos::SizedArray<double, 2> falcon_positions = {0.0, 0.0};
+
+ for (int i = 0; i < 5; ++i) {
+ detector.Iterate(encoder_position, falcon_positions, t);
+ encoder_position += 0.1;
+ t += std::chrono::milliseconds(11);
+ }
+
+ EXPECT_FALSE(detector.isfaulted());
+}
+
+// Tests if populate status function is working as expected
+TEST(EncoderFaultDetector, PopulateStatus) {
+ EncoderFaultDetector<2> detector;
+ aos::monotonic_clock::time_point t;
+
+ double encoder_position = 0.0;
+ aos::SizedArray<double, 2> falcon_positions = {0.0, 0.0};
+
+ for (int i = 0; i < 10; ++i) {
+ detector.Iterate(encoder_position, falcon_positions, t);
+ encoder_position++;
+ for (double &falcon : falcon_positions) {
+ falcon++;
+ }
+ t += std::chrono::milliseconds(5);
+ }
+
+ flatbuffers::FlatBufferBuilder fbb;
+ fbb.Finish(detector.PopulateStatus(&fbb));
+ aos::FlatbufferDetachedBuffer<EncoderFaultStatus> result = fbb.Release();
+
+ EXPECT_EQ("{ \"faulted\": false }", aos::FlatbufferToJson(result));
+
+ for (int i = 0; i < 5; ++i) {
+ detector.Iterate(encoder_position, falcon_positions, t);
+ encoder_position++;
+ t += std::chrono::milliseconds(5);
+ }
+
+ fbb.Finish(detector.PopulateStatus(&fbb));
+ result = fbb.Release();
+
+ EXPECT_EQ("{ \"faulted\": true }", aos::FlatbufferToJson(result));
+}
+
+} // namespace testing
+} // namespace control_loops
+} // namespace frc971
\ No newline at end of file
diff --git a/frc971/control_loops/encoder_fault_status.fbs b/frc971/control_loops/encoder_fault_status.fbs
new file mode 100644
index 0000000..ad3d360
--- /dev/null
+++ b/frc971/control_loops/encoder_fault_status.fbs
@@ -0,0 +1,11 @@
+namespace frc971.control_loops;
+
+// EncoderFaultStatus table contains boolean for when subsystem is faulted
+
+// TODO (niko): Add a boolean representing whether new date is "stale"
+// meaning that it is currently being ignored due to it
+// coming in slower than the 10 millisecond timeframe
+
+table EncoderFaultStatus {
+ faulted:bool (id : 0);
+}
\ No newline at end of file