Merge "Remove a dead dependency"
diff --git a/aos/analysis/BUILD b/aos/analysis/BUILD
new file mode 100644
index 0000000..63f2d57
--- /dev/null
+++ b/aos/analysis/BUILD
@@ -0,0 +1,108 @@
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
+load("//tools/build_rules:js.bzl", "ts_project")
+load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
+load("//aos:config.bzl", "aos_config")
+
+package(default_visibility = ["//visibility:public"])
+
+cc_binary(
+ name = "py_log_reader.so",
+ srcs = ["py_log_reader.cc"],
+ linkshared = True,
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ "//aos:configuration",
+ "//aos:json_to_flatbuffer",
+ "//aos/events:shm_event_loop",
+ "//aos/events:simulated_event_loop",
+ "//aos/events/logging:log_reader",
+ "//third_party/python",
+ "@com_github_google_glog//:glog",
+ ],
+)
+
+py_test(
+ name = "log_reader_test",
+ srcs = ["log_reader_test.py"],
+ data = [
+ ":py_log_reader.so",
+ "@sample_logfile//file",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = ["//aos:configuration_fbs_python"],
+)
+
+static_flatbuffer(
+ name = "plot_data_fbs",
+ srcs = [
+ "plot_data.fbs",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+)
+
+flatbuffer_ts_library(
+ name = "plot_data_ts_fbs",
+ srcs = [
+ "plot_data.fbs",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+)
+
+ts_project(
+ name = "plot_data_utils",
+ srcs = ["plot_data_utils.ts"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":plot_data_ts_fbs",
+ "//aos:configuration_ts_fbs",
+ "//aos/network/www:aos_plotter",
+ "//aos/network/www:plotter",
+ "//aos/network/www:proxy",
+ "@com_github_google_flatbuffers//reflection:reflection_ts_fbs",
+ "@com_github_google_flatbuffers//ts:flatbuffers_ts",
+ ],
+)
+
+aos_config(
+ name = "plotter",
+ src = "plotter_config.json",
+ flatbuffers = [":plot_data_fbs"],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = ["//aos/events:aos_config"],
+)
+
+cc_library(
+ name = "in_process_plotter",
+ srcs = ["in_process_plotter.cc"],
+ hdrs = ["in_process_plotter.h"],
+ data = [
+ ":plotter",
+ "//aos/analysis/cpp_plot:cpp_plot_files",
+ ],
+ deps = [
+ ":plot_data_fbs",
+ "//aos/events:simulated_event_loop",
+ "//aos/network:web_proxy",
+ ],
+)
+
+cc_binary(
+ name = "in_process_plotter_demo",
+ srcs = ["in_process_plotter_demo.cc"],
+ deps = [
+ ":in_process_plotter",
+ "//aos:init",
+ ],
+)
+
+cc_binary(
+ name = "local_foxglove",
+ srcs = ["local_foxglove.cc"],
+ data = ["@foxglove_studio"],
+ deps = [
+ "//aos:init",
+ "//aos/network:gen_embedded",
+ "//aos/seasocks:seasocks_logger",
+ "//third_party/seasocks",
+ ],
+)
diff --git a/frc971/analysis/cpp_plot/BUILD b/aos/analysis/cpp_plot/BUILD
similarity index 94%
rename from frc971/analysis/cpp_plot/BUILD
rename to aos/analysis/cpp_plot/BUILD
index 7f34afe..7286aaf 100644
--- a/frc971/analysis/cpp_plot/BUILD
+++ b/aos/analysis/cpp_plot/BUILD
@@ -8,8 +8,8 @@
target_compatible_with = ["@platforms//os:linux"],
deps = [
"//aos:configuration_ts_fbs",
+ "//aos/analysis:plot_data_utils",
"//aos/network/www:proxy",
- "//frc971/analysis:plot_data_utils",
],
)
diff --git a/frc971/analysis/cpp_plot/cpp_plot.ts b/aos/analysis/cpp_plot/cpp_plot.ts
similarity index 100%
rename from frc971/analysis/cpp_plot/cpp_plot.ts
rename to aos/analysis/cpp_plot/cpp_plot.ts
diff --git a/frc971/analysis/cpp_plot/index.html b/aos/analysis/cpp_plot/index.html
similarity index 100%
rename from frc971/analysis/cpp_plot/index.html
rename to aos/analysis/cpp_plot/index.html
diff --git a/frc971/analysis/in_process_plotter.cc b/aos/analysis/in_process_plotter.cc
similarity index 98%
rename from frc971/analysis/in_process_plotter.cc
rename to aos/analysis/in_process_plotter.cc
index b537aa4..a2ed68c 100644
--- a/frc971/analysis/in_process_plotter.cc
+++ b/aos/analysis/in_process_plotter.cc
@@ -1,4 +1,4 @@
-#include "frc971/analysis/in_process_plotter.h"
+#include "aos/analysis/in_process_plotter.h"
#include "aos/configuration.h"
diff --git a/frc971/analysis/in_process_plotter.h b/aos/analysis/in_process_plotter.h
similarity index 98%
rename from frc971/analysis/in_process_plotter.h
rename to aos/analysis/in_process_plotter.h
index df81041..fdfde8c 100644
--- a/frc971/analysis/in_process_plotter.h
+++ b/aos/analysis/in_process_plotter.h
@@ -3,9 +3,9 @@
#include <vector>
+#include "aos/analysis/plot_data_generated.h"
#include "aos/events/simulated_event_loop.h"
#include "aos/network/web_proxy.h"
-#include "frc971/analysis/plot_data_generated.h"
namespace frc971::analysis {
diff --git a/frc971/analysis/in_process_plotter_demo.cc b/aos/analysis/in_process_plotter_demo.cc
similarity index 96%
rename from frc971/analysis/in_process_plotter_demo.cc
rename to aos/analysis/in_process_plotter_demo.cc
index 99e0c1d..a02451a 100644
--- a/frc971/analysis/in_process_plotter_demo.cc
+++ b/aos/analysis/in_process_plotter_demo.cc
@@ -1,5 +1,5 @@
+#include "aos/analysis/in_process_plotter.h"
#include "aos/init.h"
-#include "frc971/analysis/in_process_plotter.h"
// To run this example, do:
// bazel run -c opt //frc971/analysis:in_process_plotter_demo
diff --git a/frc971/analysis/local_foxglove.cc b/aos/analysis/local_foxglove.cc
similarity index 100%
rename from frc971/analysis/local_foxglove.cc
rename to aos/analysis/local_foxglove.cc
diff --git a/frc971/analysis/log_reader_test.py b/aos/analysis/log_reader_test.py
similarity index 98%
rename from frc971/analysis/log_reader_test.py
rename to aos/analysis/log_reader_test.py
index 7af08e6..69627aa 100644
--- a/frc971/analysis/log_reader_test.py
+++ b/aos/analysis/log_reader_test.py
@@ -2,7 +2,7 @@
import json
import unittest
-from frc971.analysis.py_log_reader import LogReader
+from aos.analysis.py_log_reader import LogReader
class LogReaderTest(unittest.TestCase):
diff --git a/frc971/analysis/plot_data.fbs b/aos/analysis/plot_data.fbs
similarity index 100%
rename from frc971/analysis/plot_data.fbs
rename to aos/analysis/plot_data.fbs
diff --git a/frc971/analysis/plot_data_utils.ts b/aos/analysis/plot_data_utils.ts
similarity index 100%
rename from frc971/analysis/plot_data_utils.ts
rename to aos/analysis/plot_data_utils.ts
diff --git a/frc971/analysis/plotter_config.json b/aos/analysis/plotter_config.json
similarity index 100%
rename from frc971/analysis/plotter_config.json
rename to aos/analysis/plotter_config.json
diff --git a/frc971/analysis/py_log_reader.cc b/aos/analysis/py_log_reader.cc
similarity index 100%
rename from frc971/analysis/py_log_reader.cc
rename to aos/analysis/py_log_reader.cc
diff --git a/aos/events/logging/BUILD b/aos/events/logging/BUILD
index 1743f88..21754bb 100644
--- a/aos/events/logging/BUILD
+++ b/aos/events/logging/BUILD
@@ -965,7 +965,7 @@
srcs = ["timestamp_plot.cc"],
deps = [
"//aos:init",
- "//frc971/analysis:in_process_plotter",
+ "//aos/analysis:in_process_plotter",
"@com_google_absl//absl/strings",
],
)
diff --git a/aos/events/logging/timestamp_plot.cc b/aos/events/logging/timestamp_plot.cc
index e266716..8aee90d 100644
--- a/aos/events/logging/timestamp_plot.cc
+++ b/aos/events/logging/timestamp_plot.cc
@@ -1,9 +1,9 @@
#include "absl/strings/str_cat.h"
#include "absl/strings/str_split.h"
+#include "aos/analysis/in_process_plotter.h"
#include "aos/init.h"
#include "aos/util/file.h"
-#include "frc971/analysis/in_process_plotter.h"
using frc971::analysis::Plotter;
diff --git a/frc971/analysis/BUILD b/frc971/analysis/BUILD
index 93f50f4..b179ad9 100644
--- a/frc971/analysis/BUILD
+++ b/frc971/analysis/BUILD
@@ -1,37 +1,7 @@
-load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
load("//tools/build_rules:js.bzl", "rollup_bundle", "ts_project")
-load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
-load("//aos:config.bzl", "aos_config")
package(default_visibility = ["//visibility:public"])
-cc_binary(
- name = "py_log_reader.so",
- srcs = ["py_log_reader.cc"],
- linkshared = True,
- target_compatible_with = ["@platforms//os:linux"],
- deps = [
- "//aos:configuration",
- "//aos:json_to_flatbuffer",
- "//aos/events:shm_event_loop",
- "//aos/events:simulated_event_loop",
- "//aos/events/logging:log_reader",
- "//third_party/python",
- "@com_github_google_glog//:glog",
- ],
-)
-
-py_test(
- name = "log_reader_test",
- srcs = ["log_reader_test.py"],
- data = [
- ":py_log_reader.so",
- "@sample_logfile//file",
- ],
- target_compatible_with = ["@platforms//os:linux"],
- deps = ["//aos:configuration_fbs_python"],
-)
-
ts_project(
name = "plot_index",
srcs = ["plot_index.ts"],
@@ -73,15 +43,6 @@
],
)
-genrule(
- name = "copy_css",
- srcs = [
- "//aos/network/www:styles.css",
- ],
- outs = ["styles.css"],
- cmd = "cp $< $@",
-)
-
filegroup(
name = "plotter_files",
srcs = [
@@ -112,67 +73,13 @@
target_compatible_with = ["@platforms//os:linux"],
)
-static_flatbuffer(
- name = "plot_data_fbs",
+genrule(
+ name = "copy_css",
srcs = [
- "plot_data.fbs",
+ "//aos/network/www:styles.css",
],
- target_compatible_with = ["@platforms//os:linux"],
-)
-
-flatbuffer_ts_library(
- name = "plot_data_ts_fbs",
- srcs = [
- "plot_data.fbs",
- ],
- target_compatible_with = ["@platforms//os:linux"],
-)
-
-ts_project(
- name = "plot_data_utils",
- srcs = ["plot_data_utils.ts"],
- visibility = ["//visibility:public"],
- deps = [
- ":plot_data_ts_fbs",
- "//aos:configuration_ts_fbs",
- "//aos/network/www:aos_plotter",
- "//aos/network/www:plotter",
- "//aos/network/www:proxy",
- "@com_github_google_flatbuffers//reflection:reflection_ts_fbs",
- "@com_github_google_flatbuffers//ts:flatbuffers_ts",
- ],
-)
-
-aos_config(
- name = "plotter",
- src = "plotter_config.json",
- flatbuffers = [":plot_data_fbs"],
- target_compatible_with = ["@platforms//os:linux"],
- deps = ["//aos/events:aos_config"],
-)
-
-cc_library(
- name = "in_process_plotter",
- srcs = ["in_process_plotter.cc"],
- hdrs = ["in_process_plotter.h"],
- data = [
- ":plotter",
- "//frc971/analysis/cpp_plot:cpp_plot_files",
- ],
- deps = [
- ":plot_data_fbs",
- "//aos/events:simulated_event_loop",
- "//aos/network:web_proxy",
- ],
-)
-
-cc_binary(
- name = "in_process_plotter_demo",
- srcs = ["in_process_plotter_demo.cc"],
- deps = [
- ":in_process_plotter",
- "//aos:init",
- ],
+ outs = ["styles.css"],
+ cmd = "cp $< $@",
)
cc_binary(
@@ -204,18 +111,6 @@
],
)
-cc_binary(
- name = "local_foxglove",
- srcs = ["local_foxglove.cc"],
- data = ["@foxglove_studio"],
- deps = [
- "//aos:init",
- "//aos/network:gen_embedded",
- "//aos/seasocks:seasocks_logger",
- "//third_party/seasocks",
- ],
-)
-
py_binary(
name = "trim_and_plot_foxglove",
srcs = ["trim_and_plot_foxglove.py"],
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/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index 6f98839..afbce9b 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -606,8 +606,8 @@
target_compatible_with = ["@platforms//os:linux"],
deps = [
":spline",
+ "//aos/analysis:in_process_plotter",
"//aos/testing:googletest",
- "//frc971/analysis:in_process_plotter",
"@com_github_gflags_gflags//:gflags",
],
)
diff --git a/frc971/control_loops/drivetrain/drivetrain_can_position.fbs b/frc971/control_loops/drivetrain/drivetrain_can_position.fbs
index b451ea9..29c0b85 100644
--- a/frc971/control_loops/drivetrain/drivetrain_can_position.fbs
+++ b/frc971/control_loops/drivetrain/drivetrain_can_position.fbs
@@ -1,9 +1,10 @@
include "frc971/control_loops/can_talonfx.fbs";
namespace frc971.control_loops.drivetrain;
+attribute "static_length";
// CAN readings from the CAN sensor reader loop
table CANPosition {
- talonfxs: [CANTalonFX] (id: 0);
+ talonfxs: [CANTalonFX] (id: 0, static_length: 6);
// The timestamp of the measurement on the canivore clock in nanoseconds
// This will have less jitter than the
diff --git a/frc971/control_loops/drivetrain/spline_test.cc b/frc971/control_loops/drivetrain/spline_test.cc
index 6d84d53..fcd0030 100644
--- a/frc971/control_loops/drivetrain/spline_test.cc
+++ b/frc971/control_loops/drivetrain/spline_test.cc
@@ -5,7 +5,7 @@
#include "gflags/gflags.h"
#include "gtest/gtest.h"
-#include "frc971/analysis/in_process_plotter.h"
+#include "aos/analysis/in_process_plotter.h"
DEFINE_bool(plot, false, "If true, plot");
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
diff --git a/frc971/vision/BUILD b/frc971/vision/BUILD
index 5d8bf7c..6cee780 100644
--- a/frc971/vision/BUILD
+++ b/frc971/vision/BUILD
@@ -131,7 +131,6 @@
":foxglove_image_converter_lib",
"//aos:init",
"//aos/events/logging:log_reader",
- "//frc971/analysis:in_process_plotter",
"//frc971/control_loops/drivetrain:improved_down_estimator",
"//frc971/vision:visualize_robot",
"//frc971/wpilib:imu_batch_fbs",
@@ -142,6 +141,12 @@
"@com_google_absl//absl/strings:str_format",
"@com_google_ceres_solver//:ceres",
"@org_tuxfamily_eigen//:eigen",
+ ] + [
+ # TODO(Stephan): This is a whacky hack. If we include this
+ # in the proper spot above (alphabetically), then we get a
+ # linker error: duplicate symbol: crc32.
+ # It's part of both @zlib and @com_github_rawrtc_re.
+ "//aos/analysis:in_process_plotter",
],
)
diff --git a/frc971/vision/extrinsics_calibration.cc b/frc971/vision/extrinsics_calibration.cc
index 6017258..5c4ae31 100644
--- a/frc971/vision/extrinsics_calibration.cc
+++ b/frc971/vision/extrinsics_calibration.cc
@@ -7,8 +7,8 @@
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc.hpp>
+#include "aos/analysis/in_process_plotter.h"
#include "aos/time/time.h"
-#include "frc971/analysis/in_process_plotter.h"
#include "frc971/control_loops/runge_kutta.h"
#include "frc971/vision/calibration_accumulator.h"
#include "frc971/vision/charuco_lib.h"
diff --git a/y2018/control_loops/superstructure/arm/BUILD b/y2018/control_loops/superstructure/arm/BUILD
index ec18f9d..319e591 100644
--- a/y2018/control_loops/superstructure/arm/BUILD
+++ b/y2018/control_loops/superstructure/arm/BUILD
@@ -73,7 +73,7 @@
deps = [
":arm_constants",
":generated_graph",
- "//frc971/analysis:in_process_plotter",
+ "//aos/analysis:in_process_plotter",
"//frc971/control_loops/double_jointed_arm:ekf",
"//frc971/control_loops/double_jointed_arm:trajectory",
"@com_github_gflags_gflags//:gflags",
diff --git a/y2018/control_loops/superstructure/arm/trajectory_plot.cc b/y2018/control_loops/superstructure/arm/trajectory_plot.cc
index 6739b07..29dc085 100644
--- a/y2018/control_loops/superstructure/arm/trajectory_plot.cc
+++ b/y2018/control_loops/superstructure/arm/trajectory_plot.cc
@@ -1,7 +1,7 @@
#include "gflags/gflags.h"
+#include "aos/analysis/in_process_plotter.h"
#include "aos/init.h"
-#include "frc971/analysis/in_process_plotter.h"
#include "frc971/control_loops/double_jointed_arm/dynamics.h"
#include "frc971/control_loops/double_jointed_arm/ekf.h"
#include "frc971/control_loops/double_jointed_arm/trajectory.h"
diff --git a/y2023/control_loops/superstructure/arm/BUILD b/y2023/control_loops/superstructure/arm/BUILD
index 2f4e7d6..a54f72c 100644
--- a/y2023/control_loops/superstructure/arm/BUILD
+++ b/y2023/control_loops/superstructure/arm/BUILD
@@ -119,7 +119,7 @@
deps = [
":arm_constants",
"//aos:init",
- "//frc971/analysis:in_process_plotter",
+ "//aos/analysis:in_process_plotter",
"//frc971/control_loops:dlqr",
"//frc971/control_loops:jacobian",
"//frc971/control_loops/double_jointed_arm:dynamics",
@@ -166,7 +166,7 @@
":arm_constants",
":generated_graph",
":trajectory",
- "//frc971/analysis:in_process_plotter",
+ "//aos/analysis:in_process_plotter",
"//frc971/control_loops:binomial",
"//frc971/control_loops:fixed_quadrature",
"//frc971/control_loops/double_jointed_arm:ekf",
diff --git a/y2023/control_loops/superstructure/arm/arm_design.cc b/y2023/control_loops/superstructure/arm/arm_design.cc
index e918746..d17535a 100644
--- a/y2023/control_loops/superstructure/arm/arm_design.cc
+++ b/y2023/control_loops/superstructure/arm/arm_design.cc
@@ -1,5 +1,5 @@
+#include "aos/analysis/in_process_plotter.h"
#include "aos/init.h"
-#include "frc971/analysis/in_process_plotter.h"
#include "frc971/control_loops/dlqr.h"
#include "frc971/control_loops/double_jointed_arm/dynamics.h"
#include "frc971/control_loops/jacobian.h"
diff --git a/y2023/control_loops/superstructure/arm/trajectory_plot.cc b/y2023/control_loops/superstructure/arm/trajectory_plot.cc
index 4db5080..9b99660 100644
--- a/y2023/control_loops/superstructure/arm/trajectory_plot.cc
+++ b/y2023/control_loops/superstructure/arm/trajectory_plot.cc
@@ -1,7 +1,7 @@
#include "gflags/gflags.h"
+#include "aos/analysis/in_process_plotter.h"
#include "aos/init.h"
-#include "frc971/analysis/in_process_plotter.h"
#include "frc971/control_loops/binomial.h"
#include "frc971/control_loops/double_jointed_arm/dynamics.h"
#include "frc971/control_loops/double_jointed_arm/ekf.h"
diff --git a/y2024/constants.h b/y2024/constants.h
index ddcf0a4..2462bd5 100644
--- a/y2024/constants.h
+++ b/y2024/constants.h
@@ -38,9 +38,6 @@
kDrivetrainEncoderCountsPerRevolution();
}
- static constexpr double kDrivetrainSupplyCurrentLimit() { return 35.0; }
- static constexpr double kDrivetrainStatorCurrentLimit() { return 60.0; }
-
static double DrivetrainEncoderToMeters(int32_t in) {
return ((static_cast<double>(in) /
kDrivetrainEncoderCountsPerRevolution()) *
diff --git a/y2024/constants/common.json b/y2024/constants/common.json
index 2e89bfa..3da88fe 100644
--- a/y2024/constants/common.json
+++ b/y2024/constants/common.json
@@ -45,7 +45,9 @@
"intake_roller_supply_current_limit": 35,
"intake_roller_stator_current_limit": 60,
"transfer_roller_supply_current_limit": 35,
- "transfer_roller_stator_current_limit": 60
+ "transfer_roller_stator_current_limit": 60,
+ "drivetrain_supply_current_limit": 35,
+ "drivetrain_stator_current_limit": 60
},
"transfer_roller_voltages": {
"transfer_in": 12.0,
diff --git a/y2024/constants/constants.fbs b/y2024/constants/constants.fbs
index 54e6925..4a40487 100644
--- a/y2024/constants/constants.fbs
+++ b/y2024/constants/constants.fbs
@@ -50,6 +50,8 @@
intake_roller_stator_current_limit:double (id: 3);
transfer_roller_supply_current_limit:double (id: 4);
transfer_roller_stator_current_limit:double (id: 5);
+ drivetrain_supply_current_limit:double (id: 6);
+ drivetrain_stator_current_limit:double (id: 7);
}
table TransferRollerVoltages {
diff --git a/y2024/wpilib_interface.cc b/y2024/wpilib_interface.cc
index b708266..8130795 100644
--- a/y2024/wpilib_interface.cc
+++ b/y2024/wpilib_interface.cc
@@ -90,6 +90,7 @@
constexpr double kMaxFastEncoderPulsesPerSecond = std::max({
Values::kMaxDrivetrainEncoderPulsesPerSecond(),
+ Values::kMaxIntakePivotEncoderPulsesPerSecond(),
});
static_assert(kMaxFastEncoderPulsesPerSecond <= 1300000,
"fast encoders are too fast");
@@ -198,18 +199,12 @@
}
}
- void set_intake_pivot_encoder(::std::unique_ptr<frc::Encoder> encoder) {
+ void set_intake_pivot(::std::unique_ptr<frc::Encoder> encoder,
+ ::std::unique_ptr<frc::DigitalInput> absolute_pwm,
+ ::std::unique_ptr<frc::AnalogInput> potentiometer) {
fast_encoder_filter_.Add(encoder.get());
intake_pivot_encoder_.set_encoder(::std::move(encoder));
- }
-
- void set_intake_pivot_absolute_pwm(
- ::std::unique_ptr<frc::DigitalInput> absolute_pwm) {
intake_pivot_encoder_.set_absolute_pwm(::std::move(absolute_pwm));
- }
-
- void set_intake_pivot_potentiometer(
- ::std::unique_ptr<frc::AnalogInput> potentiometer) {
intake_pivot_encoder_.set_potentiometer(::std::move(potentiometer));
}
@@ -272,11 +267,9 @@
sensor_reader.set_drivetrain_right_encoder(make_encoder(0));
sensor_reader.set_yaw_rate_input(make_unique<frc::DigitalInput>(0));
// TODO: (niko) change values once robot is wired
- sensor_reader.set_intake_pivot_encoder(make_encoder(4));
- sensor_reader.set_intake_pivot_absolute_pwm(
- make_unique<frc::DigitalInput>(4));
- sensor_reader.set_intake_pivot_potentiometer(
- make_unique<frc::AnalogInput>(4));
+ sensor_reader.set_intake_pivot(make_encoder(4),
+ make_unique<frc::DigitalInput>(4),
+ make_unique<frc::AnalogInput>(4));
sensor_reader.set_transfer_beambreak(make_unique<frc::DigitalInput>(7));
AddLoop(&sensor_reader_event_loop);
@@ -290,46 +283,38 @@
std::vector<ctre::phoenix6::BaseStatusSignal *> signals_registry;
+ const CurrentLimits *current_limits =
+ robot_constants->common()->current_limits();
+
std::shared_ptr<TalonFX> right_front = std::make_shared<TalonFX>(
0, false, "Drivetrain Bus", &signals_registry,
- constants::Values::kDrivetrainStatorCurrentLimit(),
- constants::Values::kDrivetrainSupplyCurrentLimit());
+ current_limits->drivetrain_supply_current_limit(),
+ current_limits->drivetrain_stator_current_limit());
std::shared_ptr<TalonFX> right_back = std::make_shared<TalonFX>(
1, false, "Drivetrain Bus", &signals_registry,
- constants::Values::kDrivetrainStatorCurrentLimit(),
- constants::Values::kDrivetrainSupplyCurrentLimit());
+ current_limits->drivetrain_supply_current_limit(),
+ current_limits->drivetrain_stator_current_limit());
std::shared_ptr<TalonFX> left_front = std::make_shared<TalonFX>(
2, false, "Drivetrain Bus", &signals_registry,
- constants::Values::kDrivetrainStatorCurrentLimit(),
- constants::Values::kDrivetrainSupplyCurrentLimit());
+ current_limits->drivetrain_supply_current_limit(),
+ current_limits->drivetrain_stator_current_limit());
std::shared_ptr<TalonFX> left_back = std::make_shared<TalonFX>(
3, false, "Drivetrain Bus", &signals_registry,
- constants::Values::kDrivetrainStatorCurrentLimit(),
- constants::Values::kDrivetrainSupplyCurrentLimit());
- std::shared_ptr<TalonFX> intake_pivot =
- std::make_shared<TalonFX>(4, false, "Drivetrain Bus", &signals_registry,
- robot_constants->common()
- ->current_limits()
- ->intake_pivot_stator_current_limit(),
- robot_constants->common()
- ->current_limits()
- ->intake_pivot_supply_current_limit());
- std::shared_ptr<TalonFX> intake_roller =
- std::make_shared<TalonFX>(5, false, "Drivetrain Bus", &signals_registry,
- robot_constants->common()
- ->current_limits()
- ->intake_roller_stator_current_limit(),
- robot_constants->common()
- ->current_limits()
- ->intake_roller_supply_current_limit());
- std::shared_ptr<TalonFX> transfer_roller =
- std::make_shared<TalonFX>(6, false, "Drivetrain Bus", &signals_registry,
- robot_constants->common()
- ->current_limits()
- ->transfer_roller_stator_current_limit(),
- robot_constants->common()
- ->current_limits()
- ->transfer_roller_supply_current_limit());
+ current_limits->drivetrain_supply_current_limit(),
+ current_limits->drivetrain_stator_current_limit());
+ std::shared_ptr<TalonFX> intake_pivot = std::make_shared<TalonFX>(
+ 4, false, "Drivetrain Bus", &signals_registry,
+ current_limits->intake_pivot_stator_current_limit(),
+ current_limits->intake_pivot_supply_current_limit());
+ std::shared_ptr<TalonFX> intake_roller = std::make_shared<TalonFX>(
+ 5, false, "Drivetrain Bus", &signals_registry,
+ current_limits->intake_roller_stator_current_limit(),
+ current_limits->intake_roller_supply_current_limit());
+ std::shared_ptr<TalonFX> transfer_roller = std::make_shared<TalonFX>(
+ 6, false, "Drivetrain Bus", &signals_registry,
+ current_limits->transfer_roller_stator_current_limit(),
+ current_limits->transfer_roller_supply_current_limit());
+
ctre::phoenix::platform::can::CANComm_SetRxSchedPriority(
constants::Values::kDrivetrainRxPriority, true, "Drivetrain Bus");
ctre::phoenix::platform::can::CANComm_SetTxSchedPriority(
@@ -375,6 +360,8 @@
auto drivetrain_falcon_vector =
drivetrain_can_builder->add_talonfxs();
+ CHECK(drivetrain_falcon_vector->reserve(drivetrain_talonfxs.size()));
+
for (auto talonfx : drivetrain_talonfxs) {
talonfx->SerializePosition(
drivetrain_falcon_vector->emplace_back(),
diff --git a/y2024_defense/wpilib_interface.cc b/y2024_defense/wpilib_interface.cc
index 1584dbd..6dec3c9 100644
--- a/y2024_defense/wpilib_interface.cc
+++ b/y2024_defense/wpilib_interface.cc
@@ -324,6 +324,8 @@
auto falcon_vector = builder->add_talonfxs();
+ CHECK(falcon_vector->reserve(falcons.size()));
+
for (auto falcon : falcons) {
falcon->SerializePosition(
falcon_vector->emplace_back(),