Add Drivetrain Encoder fault detector
The Drivetrain encoder Fault detector will add a faults message to drivetrain status in order to display whether the left or right sides of the drivetrain are faulted.
Signed-off-by: Niko Sohmers <nikolai@sohmers.com>
Change-Id: Ieb13e9d236fbea591a6062b1733ed808c7648ace
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index 6f98839..316c12f 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -42,7 +42,10 @@
static_flatbuffer(
name = "drivetrain_status_fbs",
srcs = ["drivetrain_status.fbs"],
- deps = ["//frc971/control_loops:control_loops_fbs"],
+ deps = [
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops:encoder_fault_status_fbs",
+ ],
)
static_flatbuffer(
@@ -62,7 +65,10 @@
name = "drivetrain_status_ts_fbs",
srcs = ["drivetrain_status.fbs"],
target_compatible_with = ["@platforms//os:linux"],
- deps = ["//frc971/control_loops:control_loops_ts_fbs"],
+ deps = [
+ "//frc971/control_loops:control_loops_ts_fbs",
+ "//frc971/control_loops:encoder_fault_status_ts_fbs",
+ ],
)
flatbuffer_ts_library(
@@ -128,7 +134,10 @@
static_flatbuffer(
name = "drivetrain_status_float_fbs",
srcs = ["drivetrain_status_float.fbs"],
- deps = ["//frc971/control_loops:control_loops_fbs"],
+ deps = [
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops:encoder_fault_status_fbs",
+ ],
)
aos_config(
@@ -136,6 +145,7 @@
src = "drivetrain_simulation_channels.json",
flatbuffers = [
":drivetrain_status_fbs",
+ "//frc971/control_loops:encoder_fault_status_fbs",
],
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
@@ -162,6 +172,7 @@
":drivetrain_output_fbs",
":drivetrain_status_fbs",
":drivetrain_position_fbs",
+ ":drivetrain_can_position_fbs",
":localizer_fbs",
"//frc971/queues:gyro_fbs",
"//frc971/queues:gyro_uid_fbs",
@@ -789,6 +800,34 @@
],
)
+cc_library(
+ name = "drivetrain_encoder_fault_detector",
+ srcs = ["drivetrain_encoder_fault_detector.cc"],
+ hdrs = ["drivetrain_encoder_fault_detector.h"],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ "//aos/events:event_loop",
+ "//frc971/control_loops:encoder_fault_detector",
+ "//frc971/control_loops:encoder_fault_status_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_can_position_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_position_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
+ ],
+)
+
+cc_test(
+ name = "drivetrain_encoder_fault_detector_test",
+ srcs = ["drivetrain_encoder_fault_detector_test.cc"],
+ data = [":simulation_config"],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":drivetrain_encoder_fault_detector",
+ "//aos:json_to_flatbuffer",
+ "//aos/events:simulated_event_loop",
+ "//aos/testing:googletest",
+ ],
+)
+
cc_test(
name = "camera_test",
srcs = ["camera_test.cc"],
diff --git a/frc971/control_loops/drivetrain/drivetrain_can_position.fbs b/frc971/control_loops/drivetrain/drivetrain_can_position.fbs
index b451ea9..c86b4c1 100644
--- a/frc971/control_loops/drivetrain/drivetrain_can_position.fbs
+++ b/frc971/control_loops/drivetrain/drivetrain_can_position.fbs
@@ -13,6 +13,11 @@
// The ctre::phoenix::StatusCode of the measurement
// Should be OK = 0
status:int (id: 2);
+
+ // These values aren't used yet but are
+ // arrays that represent falcons on left/right sides of the drivetrain
+ left_falcons: [CANTalonFX] (id: 3);
+ right_falcons: [CANTalonFX] (id: 4);
}
root_type CANPosition;
diff --git a/frc971/control_loops/drivetrain/drivetrain_config.json b/frc971/control_loops/drivetrain/drivetrain_config.json
index a74d416..71c7628 100644
--- a/frc971/control_loops/drivetrain/drivetrain_config.json
+++ b/frc971/control_loops/drivetrain/drivetrain_config.json
@@ -43,6 +43,11 @@
},
{
"name": "/drivetrain",
+ "type": "frc971.control_loops.drivetrain.CANPosition",
+ "frequency": 200
+ },
+ {
+ "name": "/drivetrain",
"type": "frc971.control_loops.drivetrain.Status",
"frequency": 200,
"max_size": 2000
diff --git a/frc971/control_loops/drivetrain/drivetrain_encoder_fault_detector.cc b/frc971/control_loops/drivetrain/drivetrain_encoder_fault_detector.cc
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/frc971/control_loops/drivetrain/drivetrain_encoder_fault_detector.cc
diff --git a/frc971/control_loops/drivetrain/drivetrain_encoder_fault_detector.h b/frc971/control_loops/drivetrain/drivetrain_encoder_fault_detector.h
new file mode 100644
index 0000000..7513033
--- /dev/null
+++ b/frc971/control_loops/drivetrain/drivetrain_encoder_fault_detector.h
@@ -0,0 +1,115 @@
+#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_ENCODER_FAULT_DETECTOR_H_
+#define FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_ENCODER_FAULT_DETECTOR_H_
+
+#include "aos/events/event_loop.h"
+#include "frc971/control_loops/drivetrain/drivetrain_can_position_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
+#include "frc971/control_loops/encoder_fault_detector.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace drivetrain {
+
+// This class will be used to detect any encoder faults within the drivetrain
+// using values from the left/right encoders and motors
+
+template <uint8_t NumberofDrivetrainMotors>
+class DrivetrainEncoderFaultDetector {
+ public:
+ DrivetrainEncoderFaultDetector(aos::EventLoop *event_loop);
+
+ void UpdateDetectors();
+
+ void SendMessage();
+
+ flatbuffers::Offset<drivetrain::Faults> PopulateFaults(
+ flatbuffers::FlatBufferBuilder *fbb);
+
+ private:
+ aos::EventLoop *event_loop_;
+ EncoderFaultDetector<NumberofDrivetrainMotors> left_encoder_fault_detector_;
+ EncoderFaultDetector<NumberofDrivetrainMotors> right_encoder_fault_detector_;
+ aos::Sender<frc971::control_loops::drivetrain::Status>
+ drivetrain_status_sender_;
+ aos::Fetcher<control_loops::drivetrain::Position>
+ drivetrain_position_fetcher_;
+ aos::Fetcher<control_loops::drivetrain::CANPosition>
+ drivetrain_can_position_fetcher_;
+};
+
+template <uint8_t NumberofDrivetrainMotors>
+DrivetrainEncoderFaultDetector<NumberofDrivetrainMotors>::
+ DrivetrainEncoderFaultDetector(aos::EventLoop *event_loop)
+ : event_loop_(event_loop),
+ drivetrain_status_sender_(
+ event_loop->MakeSender<drivetrain::Status>("/drivetrain")),
+ drivetrain_position_fetcher_(
+ event_loop->MakeFetcher<drivetrain::Position>("/drivetrain")),
+ drivetrain_can_position_fetcher_(
+ event_loop->MakeFetcher<drivetrain::CANPosition>("/drivetrain")) {
+ event_loop
+ ->AddTimer([this]() {
+ UpdateDetectors();
+ SendMessage();
+ })
+ ->Schedule(event_loop->monotonic_now(),
+ std::chrono::duration_cast<aos::monotonic_clock::duration>(
+ std::chrono::milliseconds(5)));
+}
+
+template <uint8_t NumberofDrivetrainMotors>
+void DrivetrainEncoderFaultDetector<
+ NumberofDrivetrainMotors>::UpdateDetectors() {
+ drivetrain_position_fetcher_.Fetch();
+ drivetrain_can_position_fetcher_.Fetch();
+
+ if (drivetrain_position_fetcher_.get() &&
+ drivetrain_can_position_fetcher_.get()) {
+ left_encoder_fault_detector_.Iterate(
+ drivetrain_position_fetcher_->left_encoder(),
+ drivetrain_can_position_fetcher_->left_falcons(),
+ event_loop_->monotonic_now());
+ right_encoder_fault_detector_.Iterate(
+ drivetrain_position_fetcher_->right_encoder(),
+ drivetrain_can_position_fetcher_->right_falcons(),
+ event_loop_->monotonic_now());
+ }
+}
+
+template <uint8_t NumberofDrivetrainMotors>
+void DrivetrainEncoderFaultDetector<NumberofDrivetrainMotors>::SendMessage() {
+ auto builder = drivetrain_status_sender_.MakeBuilder();
+ auto offset = PopulateFaults(builder.fbb());
+
+ drivetrain::Status::Builder drivetrain_status_builder =
+ builder.MakeBuilder<drivetrain::Status>();
+
+ drivetrain_status_builder.add_encoder_faults(offset);
+ builder.CheckOk(builder.Send(drivetrain_status_builder.Finish()));
+}
+
+template <uint8_t NumberofDrivetrainMotors>
+flatbuffers::Offset<drivetrain::Faults>
+DrivetrainEncoderFaultDetector<NumberofDrivetrainMotors>::PopulateFaults(
+ flatbuffers::FlatBufferBuilder *fbb) {
+ flatbuffers::Offset<EncoderFaultStatus> left_encoder_fault_detector_offset =
+ left_encoder_fault_detector_.PopulateStatus(fbb);
+ flatbuffers::Offset<EncoderFaultStatus> right_encoder_fault_detector_offset =
+ right_encoder_fault_detector_.PopulateStatus(fbb);
+
+ drivetrain::Faults::Builder drivetrain_faults_builder(*fbb);
+
+ drivetrain_faults_builder.add_left_faulted(
+ left_encoder_fault_detector_offset);
+ drivetrain_faults_builder.add_right_faulted(
+ right_encoder_fault_detector_offset);
+
+ return drivetrain_faults_builder.Finish();
+}
+
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace frc971
+
+#endif
\ No newline at end of file
diff --git a/frc971/control_loops/drivetrain/drivetrain_encoder_fault_detector_test.cc b/frc971/control_loops/drivetrain/drivetrain_encoder_fault_detector_test.cc
new file mode 100644
index 0000000..adc8972
--- /dev/null
+++ b/frc971/control_loops/drivetrain/drivetrain_encoder_fault_detector_test.cc
@@ -0,0 +1,416 @@
+#include "frc971/control_loops/drivetrain/drivetrain_encoder_fault_detector.h"
+
+#include "gtest/gtest.h"
+
+#include "aos/events/simulated_event_loop.h"
+#include "aos/json_to_flatbuffer.h"
+
+namespace frc971::control_loops::drivetrain::testing {
+
+class EncoderFaultDetectorTest : public ::testing::Test {
+ public:
+ EncoderFaultDetectorTest()
+ : config_(aos::configuration::ReadConfig(
+ "frc971/control_loops/drivetrain/simulation_config.json")),
+ event_loop_factory_(&config_.message()),
+ event_loop_(event_loop_factory_.MakeEventLoop(
+ "drivetrain_encoder_fault_detector", nullptr)),
+ drivetrain_status_fetcher_(
+ event_loop_->MakeFetcher<frc971::control_loops::drivetrain::Status>(
+ "/drivetrain")),
+ drivetrain_position_sender_(
+ event_loop_->MakeSender<drivetrain::Position>("/drivetrain")),
+ drivetrain_can_position_sender_(
+ event_loop_->MakeSender<drivetrain::CANPosition>("/drivetrain")),
+ fault_detector_(event_loop_.get()) {}
+ void ResetEncoders() {
+ left_encoder_ = 0.0;
+ right_encoder_ = 0.0;
+ right_falcons_ = {0.0, 0.0};
+ left_falcons_ = {0.0, 0.0};
+ }
+ aos::FlatbufferDetachedBuffer<control_loops::drivetrain::Position>
+ ConstructPositionFlatBuffer(double left_encoder, double right_encoder) {
+ return aos::JsonToFlatbuffer<control_loops::drivetrain::Position>(
+ absl::StrFormat("{ \"left_encoder\": %f, \"right_encoder\": %f }",
+ left_encoder, right_encoder));
+ }
+
+ aos::FlatbufferDetachedBuffer<control_loops::drivetrain::CANPosition>
+ ConstructCANPositionFlatBuffer(const std::vector<double> &left_falcons,
+ const std::vector<double> &right_falcons) {
+ if (left_falcons.size() == right_falcons.size()) {
+ const size_t num_falcons = left_falcons.size();
+ std::string json = "{ \"left_falcons\":[";
+
+ for (size_t i = 0; i < num_falcons; ++i) {
+ json += absl::StrFormat("{ \"position\": %f }", left_falcons[i]);
+ if (i + 1 < num_falcons) {
+ json += ", ";
+ }
+ }
+
+ json += "], \"right_falcons\":[";
+
+ for (size_t i = 0; i < num_falcons; ++i) {
+ json += absl::StrFormat("{ \"position\": %f }", right_falcons[i]);
+ if (i + 1 < num_falcons) {
+ json += ", ";
+ }
+ }
+
+ json += "]}";
+ return aos::JsonToFlatbuffer<control_loops::drivetrain::CANPosition>(
+ json);
+ }
+ LOG(FATAL) << "You must provide two falcon arrays of equal length";
+ return aos::JsonToFlatbuffer<control_loops::drivetrain::CANPosition>("");
+ }
+
+ void SendPositionMessages() {
+ drivetrain_position_sender_.CheckOk(drivetrain_position_sender_.Send(
+ ConstructPositionFlatBuffer(left_encoder_, right_encoder_)));
+ drivetrain_can_position_sender_.CheckOk(
+ drivetrain_can_position_sender_.Send(
+ ConstructCANPositionFlatBuffer(left_falcons_, right_falcons_)));
+ }
+
+ protected:
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config_;
+ aos::SimulatedEventLoopFactory event_loop_factory_;
+ std::unique_ptr<aos::EventLoop> event_loop_;
+
+ aos::Fetcher<frc971::control_loops::drivetrain::Status>
+ drivetrain_status_fetcher_;
+ aos::Sender<control_loops::drivetrain::Position> drivetrain_position_sender_;
+ aos::Sender<control_loops::drivetrain::CANPosition>
+ drivetrain_can_position_sender_;
+
+ DrivetrainEncoderFaultDetector<2> fault_detector_;
+ double left_encoder_ = 0.0;
+ double right_encoder_ = 0.0;
+ std::vector<double> right_falcons_;
+ std::vector<double> left_falcons_;
+};
+
+// Test simulates if drivetrain encoders are idle
+TEST_F(EncoderFaultDetectorTest, Idle) {
+ ResetEncoders();
+
+ event_loop_->AddTimer([this]() { SendPositionMessages(); })
+ ->Schedule(event_loop_->monotonic_now(),
+ std::chrono::duration_cast<aos::monotonic_clock::duration>(
+ std::chrono::milliseconds(5)));
+ event_loop_factory_.RunFor(std::chrono::seconds(1));
+
+ CHECK(drivetrain_status_fetcher_.Fetch());
+
+ EXPECT_EQ(
+ false,
+ drivetrain_status_fetcher_->encoder_faults()->right_faulted()->faulted());
+ EXPECT_EQ(
+ false,
+ drivetrain_status_fetcher_->encoder_faults()->left_faulted()->faulted());
+}
+
+// Test simulates if drivetrain encoders are increasing
+TEST_F(EncoderFaultDetectorTest, Increasing) {
+ ResetEncoders();
+
+ event_loop_
+ ->AddTimer([this]() {
+ SendPositionMessages();
+ left_encoder_ += 0.1;
+ right_encoder_ += 0.1;
+ for (double &falcon : left_falcons_) {
+ falcon += 0.1;
+ }
+ for (double &falcon : right_falcons_) {
+ falcon += 0.1;
+ }
+ })
+ ->Schedule(event_loop_->monotonic_now(),
+ std::chrono::duration_cast<aos::monotonic_clock::duration>(
+ std::chrono::milliseconds(5)));
+ event_loop_factory_.RunFor(std::chrono::seconds(5));
+
+ CHECK(drivetrain_status_fetcher_.Fetch());
+
+ EXPECT_EQ(
+ false,
+ drivetrain_status_fetcher_->encoder_faults()->right_faulted()->faulted());
+ EXPECT_EQ(
+ false,
+ drivetrain_status_fetcher_->encoder_faults()->left_faulted()->faulted());
+}
+
+// Test simulates if drivetrain encoders are decreasing
+TEST_F(EncoderFaultDetectorTest, Decreasing) {
+ ResetEncoders();
+
+ event_loop_
+ ->AddTimer([this]() {
+ SendPositionMessages();
+ left_encoder_ -= 0.1;
+ right_encoder_ -= 0.1;
+ for (double &falcon : left_falcons_) {
+ falcon -= 0.1;
+ }
+ for (double &falcon : right_falcons_) {
+ falcon -= 0.1;
+ }
+ })
+ ->Schedule(event_loop_->monotonic_now(),
+ std::chrono::duration_cast<aos::monotonic_clock::duration>(
+ std::chrono::milliseconds(5)));
+ event_loop_factory_.RunFor(std::chrono::seconds(5));
+
+ CHECK(drivetrain_status_fetcher_.Fetch());
+
+ EXPECT_EQ(
+ false,
+ drivetrain_status_fetcher_->encoder_faults()->right_faulted()->faulted());
+ EXPECT_EQ(
+ false,
+ drivetrain_status_fetcher_->encoder_faults()->left_faulted()->faulted());
+}
+
+// Test simulates if only the right drivetrain encoders are increasing
+TEST_F(EncoderFaultDetectorTest, OnlyIncreaseRightSide) {
+ ResetEncoders();
+
+ event_loop_
+ ->AddTimer([this]() {
+ SendPositionMessages();
+ right_encoder_ += 0.1;
+ for (double &falcon : right_falcons_) {
+ falcon += 0.1;
+ }
+ })
+ ->Schedule(event_loop_->monotonic_now(),
+ std::chrono::duration_cast<aos::monotonic_clock::duration>(
+ std::chrono::milliseconds(5)));
+ event_loop_factory_.RunFor(std::chrono::seconds(5));
+
+ CHECK(drivetrain_status_fetcher_.Fetch());
+
+ EXPECT_EQ(
+ false,
+ drivetrain_status_fetcher_->encoder_faults()->right_faulted()->faulted());
+ EXPECT_EQ(
+ false,
+ drivetrain_status_fetcher_->encoder_faults()->left_faulted()->faulted());
+}
+
+// Test simulates if only the left drivetrain encoders are increasing
+TEST_F(EncoderFaultDetectorTest, OnlyIncreaseLeftSide) {
+ ResetEncoders();
+
+ event_loop_
+ ->AddTimer([this]() {
+ SendPositionMessages();
+ left_encoder_ += 0.1;
+ for (double &falcon : left_falcons_) {
+ falcon += 0.1;
+ }
+ })
+ ->Schedule(event_loop_->monotonic_now(),
+ std::chrono::duration_cast<aos::monotonic_clock::duration>(
+ std::chrono::milliseconds(5)));
+ event_loop_factory_.RunFor(std::chrono::seconds(5));
+
+ CHECK(drivetrain_status_fetcher_.Fetch());
+
+ EXPECT_EQ(
+ false,
+ drivetrain_status_fetcher_->encoder_faults()->right_faulted()->faulted());
+ EXPECT_EQ(
+ false,
+ drivetrain_status_fetcher_->encoder_faults()->left_faulted()->faulted());
+}
+
+// Test simulates if only the right drivetrain encoders are decreasing
+TEST_F(EncoderFaultDetectorTest, OnlyDecreaseRightSide) {
+ ResetEncoders();
+
+ event_loop_
+ ->AddTimer([this]() {
+ SendPositionMessages();
+ right_encoder_ -= 0.1;
+ for (double &falcon : right_falcons_) {
+ falcon -= 0.1;
+ }
+ })
+ ->Schedule(event_loop_->monotonic_now(),
+ std::chrono::duration_cast<aos::monotonic_clock::duration>(
+ std::chrono::milliseconds(5)));
+ event_loop_factory_.RunFor(std::chrono::seconds(5));
+
+ CHECK(drivetrain_status_fetcher_.Fetch());
+
+ EXPECT_EQ(
+ false,
+ drivetrain_status_fetcher_->encoder_faults()->right_faulted()->faulted());
+ EXPECT_EQ(
+ false,
+ drivetrain_status_fetcher_->encoder_faults()->left_faulted()->faulted());
+}
+// Test simulates if only the left drivetrain encoders are decreasing
+TEST_F(EncoderFaultDetectorTest, OnlyDecreaseLeftSide) {
+ ResetEncoders();
+
+ event_loop_
+ ->AddTimer([this]() {
+ SendPositionMessages();
+ left_encoder_ -= 0.1;
+ for (double &falcon : left_falcons_) {
+ falcon -= 0.1;
+ }
+ })
+ ->Schedule(event_loop_->monotonic_now(),
+ std::chrono::duration_cast<aos::monotonic_clock::duration>(
+ std::chrono::milliseconds(5)));
+ event_loop_factory_.RunFor(std::chrono::seconds(5));
+
+ CHECK(drivetrain_status_fetcher_.Fetch());
+
+ EXPECT_EQ(
+ false,
+ drivetrain_status_fetcher_->encoder_faults()->right_faulted()->faulted());
+ EXPECT_EQ(
+ false,
+ drivetrain_status_fetcher_->encoder_faults()->left_faulted()->faulted());
+}
+
+// Test simulates that if there is no data for one second that their will be no
+// faults
+TEST_F(EncoderFaultDetectorTest, NoDataForOneSecond) {
+ ResetEncoders();
+
+ SendPositionMessages();
+
+ event_loop_factory_.RunFor(std::chrono::seconds(1));
+
+ left_encoder_ = 1.0;
+ right_encoder_ = 2.0;
+ for (double &falcon : left_falcons_) {
+ falcon = 3.0;
+ }
+ for (double &falcon : right_falcons_) {
+ falcon = 4.0;
+ }
+
+ SendPositionMessages();
+
+ event_loop_factory_.RunFor(std::chrono::seconds(1));
+
+ CHECK(drivetrain_status_fetcher_.Fetch());
+
+ EXPECT_EQ(
+ false,
+ drivetrain_status_fetcher_->encoder_faults()->right_faulted()->faulted());
+ EXPECT_EQ(
+ false,
+ drivetrain_status_fetcher_->encoder_faults()->left_faulted()->faulted());
+}
+
+// Test simulates that only the left encoder is increasing
+TEST_F(EncoderFaultDetectorTest, LeftEncoderFaulted) {
+ ResetEncoders();
+
+ event_loop_
+ ->AddTimer([this]() {
+ SendPositionMessages();
+ left_encoder_ += 0.1;
+ })
+ ->Schedule(event_loop_->monotonic_now(),
+ std::chrono::duration_cast<aos::monotonic_clock::duration>(
+ std::chrono::milliseconds(5)));
+ event_loop_factory_.RunFor(std::chrono::seconds(5));
+
+ CHECK(drivetrain_status_fetcher_.Fetch());
+
+ EXPECT_EQ(
+ false,
+ drivetrain_status_fetcher_->encoder_faults()->right_faulted()->faulted());
+ EXPECT_EQ(
+ true,
+ drivetrain_status_fetcher_->encoder_faults()->left_faulted()->faulted());
+}
+// Test simulates that only the right encoder is increasing
+TEST_F(EncoderFaultDetectorTest, RightEncoderFaulted) {
+ ResetEncoders();
+
+ event_loop_
+ ->AddTimer([this]() {
+ SendPositionMessages();
+ right_encoder_ += 0.1;
+ })
+ ->Schedule(event_loop_->monotonic_now(),
+ std::chrono::duration_cast<aos::monotonic_clock::duration>(
+ std::chrono::milliseconds(5)));
+ event_loop_factory_.RunFor(std::chrono::seconds(5));
+
+ CHECK(drivetrain_status_fetcher_.Fetch());
+
+ EXPECT_EQ(
+ true,
+ drivetrain_status_fetcher_->encoder_faults()->right_faulted()->faulted());
+ EXPECT_EQ(
+ false,
+ drivetrain_status_fetcher_->encoder_faults()->left_faulted()->faulted());
+}
+
+// Test simulates that only the left falcons are increasing
+TEST_F(EncoderFaultDetectorTest, LeftFalconsFaulted) {
+ ResetEncoders();
+
+ event_loop_
+ ->AddTimer([this]() {
+ SendPositionMessages();
+ for (double &falcon : left_falcons_) {
+ falcon += 0.1;
+ }
+ })
+ ->Schedule(event_loop_->monotonic_now(),
+ std::chrono::duration_cast<aos::monotonic_clock::duration>(
+ std::chrono::milliseconds(5)));
+ event_loop_factory_.RunFor(std::chrono::seconds(5));
+
+ CHECK(drivetrain_status_fetcher_.Fetch());
+
+ EXPECT_EQ(
+ false,
+ drivetrain_status_fetcher_->encoder_faults()->right_faulted()->faulted());
+ EXPECT_EQ(
+ true,
+ drivetrain_status_fetcher_->encoder_faults()->left_faulted()->faulted());
+}
+
+// Test simulates that only the right falcons are increasing
+TEST_F(EncoderFaultDetectorTest, RightFalconsFaulted) {
+ ResetEncoders();
+
+ event_loop_
+ ->AddTimer([this]() {
+ SendPositionMessages();
+ for (double &falcon : right_falcons_) {
+ falcon += 0.1;
+ }
+ })
+ ->Schedule(event_loop_->monotonic_now(),
+ std::chrono::duration_cast<aos::monotonic_clock::duration>(
+ std::chrono::milliseconds(5)));
+ event_loop_factory_.RunFor(std::chrono::seconds(5));
+
+ CHECK(drivetrain_status_fetcher_.Fetch());
+
+ EXPECT_EQ(
+ true,
+ drivetrain_status_fetcher_->encoder_faults()->right_faulted()->faulted());
+ EXPECT_EQ(
+ false,
+ drivetrain_status_fetcher_->encoder_faults()->left_faulted()->faulted());
+}
+
+} // namespace frc971::control_loops::drivetrain::testing
\ No newline at end of file
diff --git a/frc971/control_loops/drivetrain/drivetrain_status.fbs b/frc971/control_loops/drivetrain/drivetrain_status.fbs
index 8158e75..8e18915 100644
--- a/frc971/control_loops/drivetrain/drivetrain_status.fbs
+++ b/frc971/control_loops/drivetrain/drivetrain_status.fbs
@@ -1,4 +1,5 @@
include "frc971/control_loops/control_loops.fbs";
+include "frc971/control_loops/encoder_fault_status.fbs";
namespace frc971.control_loops.drivetrain;
@@ -210,6 +211,11 @@
accel_z_average:float (id: 8);
}
+table Faults {
+ right_faulted:EncoderFaultStatus (id: 0);
+ left_faulted:EncoderFaultStatus (id: 1);
+}
+
table Status {
// Estimated speed of the center of the robot in m/s (positive forwards).
robot_speed:double (id: 0);
@@ -271,6 +277,8 @@
// Total number of status send failures.
send_failures:uint64 (id: 28);
+
+ encoder_faults:Faults (id: 29);
}
root_type Status;