Merge "Improve error message in invalid Sender"
diff --git a/WORKSPACE b/WORKSPACE
index d76d359..cd344fa 100644
--- a/WORKSPACE
+++ b/WORKSPACE
@@ -703,9 +703,9 @@
deps = ["@//third_party/allwpilib/wpimath"],
)
""",
- sha256 = "8b7a64cf71d83f55492a4896d05e7cb3c75c64357b58059e529f59eab7c54ca4",
+ sha256 = "e1254dea273c2951f8418c9651f9c3e6fd387c4dab833858502997b3b00f3b80",
urls = [
- "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/api-cpp/24.0.0-beta-5/api-cpp-24.0.0-beta-5-headers.zip",
+ "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/api-cpp/24.0.0-beta-7/api-cpp-24.0.0-beta-7-headers.zip",
],
)
@@ -727,9 +727,9 @@
target_compatible_with = ['@//tools/platforms/hardware:roborio'],
)
""",
- sha256 = "cdf64c40418fb4ddedcaca70d2c847f2412c1e1242b5b53ec79272c354ab7084",
+ sha256 = "864e8c2c9ef0af45205f0e8cbbcb0ae27de7fa9c4b1004b3dc2ac5e2dbeb2e33",
urls = [
- "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/api-cpp/24.0.0-beta-5/api-cpp-24.0.0-beta-5-linuxathena.zip",
+ "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/api-cpp/24.0.0-beta-7/api-cpp-24.0.0-beta-7-linuxathena.zip",
],
)
@@ -742,9 +742,9 @@
hdrs = glob(['ctre/**/*.h', 'ctre/**/*.hpp']),
)
""",
- sha256 = "c5a9da5b4e3d3d886971a88b97d2bcb8dc41ba61dd9a7af65ab80b8c3eb3c296",
+ sha256 = "a2d9f361b7547ef99ec4306c73e6199cae1815b66e283f68e6cbb91bcdd35727",
urls = [
- "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/tools/24.0.0-beta-5/tools-24.0.0-beta-5-headers.zip",
+ "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/tools/24.0.0-beta-7/tools-24.0.0-beta-7-headers.zip",
],
)
@@ -766,9 +766,9 @@
target_compatible_with = ['@//tools/platforms/hardware:roborio'],
)
""",
- sha256 = "a119d3a1346e3427293237b9780f5063263e5b2803d682a919f6d37ae03837a1",
+ sha256 = "fe54d1883496270612529ba2d72c58fef8199491159b0d2caf5cf9fec33343c6",
urls = [
- "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/tools/24.0.0-beta-5/tools-24.0.0-beta-5-linuxathena.zip",
+ "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/tools/24.0.0-beta-7/tools-24.0.0-beta-7-linuxathena.zip",
],
)
@@ -781,9 +781,9 @@
hdrs = glob(['ctre/phoenix/**/*.h']),
)
""",
- sha256 = "58e32fa7f08aff009703233ba8cd5379c9f7ec82ba2dc41ff94364573141e462",
+ sha256 = "2f6cf6c9edca8a6ec14a29a52974947285758a2cdade1055d8f6162c5a4ecbee",
urls = [
- "https://maven.ctr-electronics.com/release/com/ctre/phoenix/api-cpp/5.32.0-beta-4/api-cpp-5.32.0-beta-4-headers.zip",
+ "https://maven.ctr-electronics.com/release/com/ctre/phoenix/api-cpp/5.32.0-beta-5/api-cpp-5.32.0-beta-5-headers.zip",
],
)
@@ -805,9 +805,9 @@
target_compatible_with = ['@//tools/platforms/hardware:roborio'],
)
""",
- sha256 = "8fd0c6c147dfaf71b309c6293c2a69ea0b125c694943aab9cb16acfbd2764b4d",
+ sha256 = "a7a98bf85aecda23bdd578cebbab9a471c3954de78dda480190028e02e0db9d9",
urls = [
- "https://maven.ctr-electronics.com/release/com/ctre/phoenix/api-cpp/5.32.0-beta-4/api-cpp-5.32.0-beta-4-linuxathena.zip",
+ "https://maven.ctr-electronics.com/release/com/ctre/phoenix/api-cpp/5.32.0-beta-5/api-cpp-5.32.0-beta-5-linuxathena.zip",
],
)
@@ -820,9 +820,9 @@
hdrs = glob(['ctre/phoenix/**/*.h']),
)
""",
- sha256 = "6addc983287221f6af94ea96ef30c4767fa9d02eced0333156deed6bf6f4888e",
+ sha256 = "1b2b5375c7ca57148165ea1adfe6726d7e91ff3f2250cfa846732ee98de68b6c",
urls = [
- "https://maven.ctr-electronics.com/release/com/ctre/phoenix/cci/5.32.0-beta-4/cci-5.32.0-beta-4-headers.zip",
+ "https://maven.ctr-electronics.com/release/com/ctre/phoenix/cci/5.32.0-beta-5/cci-5.32.0-beta-5-headers.zip",
],
)
@@ -844,9 +844,9 @@
target_compatible_with = ['@//tools/platforms/hardware:roborio'],
)
""",
- sha256 = "072cefb9caf71340e01b4847be493864efce1820cf38fdbfff8bf2122547cf52",
+ sha256 = "09b1485e635b4190e5386d416979890a62c29741d153db15567a3d0a80abb05e",
urls = [
- "https://maven.ctr-electronics.com/release/com/ctre/phoenix/cci/5.32.0-beta-4/cci-5.32.0-beta-4-linuxathena.zip",
+ "https://maven.ctr-electronics.com/release/com/ctre/phoenix/cci/5.32.0-beta-5/cci-5.32.0-beta-5-linuxathena.zip",
],
)
diff --git a/aos/network/message_bridge_server_status.cc b/aos/network/message_bridge_server_status.cc
index 22270d9..f540b1e 100644
--- a/aos/network/message_bridge_server_status.cc
+++ b/aos/network/message_bridge_server_status.cc
@@ -126,6 +126,11 @@
configuration::GetChannel(event_loop_->configuration(), "/aos",
Timestamp::GetFullyQualifiedName(),
event_loop_->name(), destination_node);
+ CHECK(other_timestamp_channel)
+ << "Failed to find other timestamp channel \"/aos\" type "
+ << Timestamp::GetFullyQualifiedName() << " for destination node "
+ << destination_node->name()->string_view() << " from node "
+ << event_loop_->node()->name()->string_view();
ServerConnection *const server_connection =
FindServerConnection(destination_node->name()->string_view());
diff --git a/frc971/control_loops/flywheel/BUILD b/frc971/control_loops/flywheel/BUILD
new file mode 100644
index 0000000..6399abe
--- /dev/null
+++ b/frc971/control_loops/flywheel/BUILD
@@ -0,0 +1,49 @@
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
+
+flatbuffer_cc_library(
+ name = "flywheel_controller_status_fbs",
+ srcs = [
+ "flywheel_controller_status.fbs",
+ ],
+ gen_reflections = 1,
+ visibility = ["//visibility:public"],
+)
+
+flatbuffer_ts_library(
+ name = "flywheel_controller_status_ts_fbs",
+ srcs = [
+ "flywheel_controller_status.fbs",
+ ],
+ visibility = ["//visibility:public"],
+)
+
+cc_library(
+ name = "flywheel_test_plant",
+ hdrs = [
+ "flywheel_test_plant.h",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":flywheel_controller",
+ ],
+)
+
+cc_library(
+ name = "flywheel_controller",
+ srcs = [
+ "flywheel_controller.cc",
+ ],
+ hdrs = [
+ "flywheel_controller.h",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":flywheel_controller_status_fbs",
+ "//frc971/control_loops:control_loop",
+ "//frc971/control_loops:hybrid_state_feedback_loop",
+ "//frc971/control_loops:profiled_subsystem",
+ ],
+)
diff --git a/y2020/control_loops/superstructure/shooter/flywheel_controller.cc b/frc971/control_loops/flywheel/flywheel_controller.cc
similarity index 93%
rename from y2020/control_loops/superstructure/shooter/flywheel_controller.cc
rename to frc971/control_loops/flywheel/flywheel_controller.cc
index a25ed53..adea0e8 100644
--- a/y2020/control_loops/superstructure/shooter/flywheel_controller.cc
+++ b/frc971/control_loops/flywheel/flywheel_controller.cc
@@ -1,15 +1,11 @@
-#include "y2020/control_loops/superstructure/shooter/flywheel_controller.h"
+#include "frc971/control_loops/flywheel/flywheel_controller.h"
#include <chrono>
#include "aos/logging/logging.h"
-#include "y2020/control_loops/superstructure/accelerator/accelerator_plant.h"
-#include "y2020/control_loops/superstructure/finisher/finisher_plant.h"
-
-namespace y2020 {
+namespace frc971 {
namespace control_loops {
-namespace superstructure {
-namespace shooter {
+namespace flywheel {
// Class to current limit battery current for a flywheel controller.
class CurrentLimitedStateFeedbackController
@@ -169,7 +165,6 @@
double FlywheelController::velocity() const { return loop_->X_hat(1, 0); }
-} // namespace shooter
-} // namespace superstructure
+} // namespace flywheel
} // namespace control_loops
-} // namespace y2020
+} // namespace frc971
diff --git a/y2020/control_loops/superstructure/shooter/flywheel_controller.h b/frc971/control_loops/flywheel/flywheel_controller.h
similarity index 78%
rename from y2020/control_loops/superstructure/shooter/flywheel_controller.h
rename to frc971/control_loops/flywheel/flywheel_controller.h
index 1d56407..db8baeb 100644
--- a/y2020/control_loops/superstructure/shooter/flywheel_controller.h
+++ b/frc971/control_loops/flywheel/flywheel_controller.h
@@ -1,19 +1,17 @@
-#ifndef Y2020_CONTROL_LOOPS_SHOOTER_FLYWHEEL_CONTROLLER_H_
-#define Y2020_CONTROL_LOOPS_SHOOTER_FLYWHEEL_CONTROLLER_H_
+#ifndef FRC971_CONTROL_LOOPS_SHOOTER_FLYWHEEL_CONTROLLER_H_
+#define FRC971_CONTROL_LOOPS_SHOOTER_FLYWHEEL_CONTROLLER_H_
#include <memory>
#include "aos/time/time.h"
#include "frc971/control_loops/control_loop.h"
+#include "frc971/control_loops/flywheel/flywheel_controller_status_generated.h"
+#include "frc971/control_loops/hybrid_state_feedback_loop.h"
#include "frc971/control_loops/state_feedback_loop.h"
-#include "y2020/control_loops/superstructure/accelerator/integral_accelerator_plant.h"
-#include "y2020/control_loops/superstructure/finisher/integral_finisher_plant.h"
-#include "y2020/control_loops/superstructure/superstructure_status_generated.h"
-namespace y2020 {
+namespace frc971 {
namespace control_loops {
-namespace superstructure {
-namespace shooter {
+namespace flywheel {
class CurrentLimitedStateFeedbackController;
@@ -75,9 +73,8 @@
DISALLOW_COPY_AND_ASSIGN(FlywheelController);
};
-} // namespace shooter
-} // namespace superstructure
+} // namespace flywheel
} // namespace control_loops
-} // namespace y2020
+} // namespace frc971
-#endif // Y2020_CONTROL_LOOPS_SHOOTER_FLYWHEEL_CONTROLLER_H_
+#endif // FRC971_CONTROL_LOOPS_SHOOTER_FLYWHEEL_CONTROLLER_H_
diff --git a/frc971/control_loops/flywheel/flywheel_controller_status.fbs b/frc971/control_loops/flywheel/flywheel_controller_status.fbs
new file mode 100644
index 0000000..0f95818
--- /dev/null
+++ b/frc971/control_loops/flywheel/flywheel_controller_status.fbs
@@ -0,0 +1,22 @@
+namespace frc971.control_loops.flywheel;
+
+table FlywheelControllerStatus {
+ // The current average velocity in radians/second
+ avg_angular_velocity:double (id: 0);
+
+ // The current instantaneous filtered velocity in radians/second.
+ angular_velocity:double (id: 1);
+
+ // The target speed selected by the lookup table or from manual override
+ // Can be compared to velocity to determine if ready.
+ angular_velocity_goal:double (id: 2);
+
+ // Voltage error.
+ voltage_error:double (id: 3);
+
+ // The commanded battery current.
+ commanded_current:double (id: 4);
+
+ // The angular velocity of the flywheel computed using delta x / delta t
+ dt_angular_velocity:double (id: 5);
+}
diff --git a/frc971/control_loops/flywheel/flywheel_test_plant.h b/frc971/control_loops/flywheel/flywheel_test_plant.h
new file mode 100644
index 0000000..01e347d
--- /dev/null
+++ b/frc971/control_loops/flywheel/flywheel_test_plant.h
@@ -0,0 +1,47 @@
+#ifndef FRC971_CONTROL_LOOPS_FLYWHEEL_FLYWHEEL_TEST_PLANT_H_
+#define FRC971_CONTROL_LOOPS_FLYWHEEL_FLYWHEEL_TEST_PLANT_H_
+
+#include "frc971/control_loops/flywheel/flywheel_controller.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace flywheel {
+
+class FlywheelPlant : public StateFeedbackPlant<2, 1, 1> {
+ public:
+ explicit FlywheelPlant(StateFeedbackPlant<2, 1, 1> &&other, double bemf,
+ double resistance)
+ : StateFeedbackPlant<2, 1, 1>(::std::move(other)),
+ bemf_(bemf),
+ resistance_(resistance) {}
+
+ void CheckU(const Eigen::Matrix<double, 1, 1> &U) override {
+ EXPECT_LE(U(0, 0), U_max(0, 0) + 0.00001 + voltage_offset_);
+ EXPECT_GE(U(0, 0), U_min(0, 0) - 0.00001 + voltage_offset_);
+ }
+
+ double motor_current(const Eigen::Matrix<double, 1, 1> U) const {
+ return (U(0) - X(1) / bemf_) / resistance_;
+ }
+
+ double battery_current(const Eigen::Matrix<double, 1, 1> U) const {
+ return motor_current(U) * U(0) / 12.0;
+ }
+
+ double voltage_offset() const { return voltage_offset_; }
+ void set_voltage_offset(double voltage_offset) {
+ voltage_offset_ = voltage_offset;
+ }
+
+ private:
+ double voltage_offset_ = 0.0;
+
+ double bemf_;
+ double resistance_;
+};
+
+} // namespace flywheel
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_FLYWHEEL_FLYWHEEL_TEST_PLANT_H_
diff --git a/frc971/control_loops/python/BUILD b/frc971/control_loops/python/BUILD
index 0066b93..699eb5a 100644
--- a/frc971/control_loops/python/BUILD
+++ b/frc971/control_loops/python/BUILD
@@ -247,6 +247,21 @@
)
py_binary(
+ name = "flywheel",
+ srcs = [
+ "flywheel.py",
+ ],
+ legacy_create_init = False,
+ target_compatible_with = ["@platforms//cpu:x86_64"],
+ deps = [
+ ":python_init",
+ "//frc971/control_loops/python:controls",
+ "@pip//matplotlib",
+ "@pip//pygobject",
+ ],
+)
+
+py_binary(
name = "static_zeroing_single_dof_profiled_subsystem_test",
srcs = [
"static_zeroing_single_dof_profiled_subsystem_test.py",
diff --git a/y2020/control_loops/python/flywheel.py b/frc971/control_loops/python/flywheel.py
old mode 100755
new mode 100644
similarity index 96%
rename from y2020/control_loops/python/flywheel.py
rename to frc971/control_loops/python/flywheel.py
index 9153bc6..1280b36
--- a/y2020/control_loops/python/flywheel.py
+++ b/frc971/control_loops/python/flywheel.py
@@ -1,7 +1,9 @@
from frc971.control_loops.python import control_loop
from frc971.control_loops.python import controls
import numpy
-from matplotlib import pylab
+
+import matplotlib
+import matplotlib.pyplot as plt
import glog
@@ -251,21 +253,23 @@
t.append(initial_t + i * flywheel.dt)
u.append(U[0, 0])
- pylab.subplot(3, 1, 1)
- pylab.plot(t, v, label='x')
- pylab.plot(t, x_hat, label='x_hat')
- pylab.legend()
+ matplotlib.use("GTK3Agg")
- pylab.subplot(3, 1, 2)
- pylab.plot(t, u, label='u')
- pylab.plot(t, offset, label='voltage_offset')
- pylab.legend()
+ plt.subplot(3, 1, 1)
+ plt.plot(t, v, label='x')
+ plt.plot(t, x_hat, label='x_hat')
+ plt.legend()
- pylab.subplot(3, 1, 3)
- pylab.plot(t, a, label='a')
- pylab.legend()
+ plt.subplot(3, 1, 2)
+ plt.plot(t, u, label='u')
+ plt.plot(t, offset, label='voltage_offset')
+ plt.legend()
- pylab.show()
+ plt.subplot(3, 1, 3)
+ plt.plot(t, a, label='a')
+ plt.legend()
+
+ plt.show()
def WriteFlywheel(params, plant_files, controller_files, namespace):
diff --git a/frc971/wpilib/BUILD b/frc971/wpilib/BUILD
index 7ce36e4..ac9446e 100644
--- a/frc971/wpilib/BUILD
+++ b/frc971/wpilib/BUILD
@@ -535,6 +535,22 @@
)
cc_library(
+ name = "generic_can_writer",
+ srcs = [
+ "generic_can_writer.cc",
+ ],
+ hdrs = [
+ "generic_can_writer.h",
+ ],
+ target_compatible_with = ["//tools/platforms/hardware:roborio"],
+ deps = [
+ ":falcon",
+ ":loop_output_handler",
+ "//frc971:can_configuration_fbs",
+ ],
+)
+
+cc_library(
name = "can_sensor_reader",
srcs = ["can_sensor_reader.cc"],
hdrs = ["can_sensor_reader.h"],
diff --git a/frc971/wpilib/generic_can_writer.cc b/frc971/wpilib/generic_can_writer.cc
new file mode 100644
index 0000000..8658180
--- /dev/null
+++ b/frc971/wpilib/generic_can_writer.cc
@@ -0,0 +1 @@
+#include "frc971/wpilib/generic_can_writer.h"
diff --git a/frc971/wpilib/generic_can_writer.h b/frc971/wpilib/generic_can_writer.h
new file mode 100644
index 0000000..8bacfbd
--- /dev/null
+++ b/frc971/wpilib/generic_can_writer.h
@@ -0,0 +1,70 @@
+#include <map>
+#include <string_view>
+
+#include "frc971/can_configuration_generated.h"
+#include "frc971/wpilib/falcon.h"
+#include "frc971/wpilib/loop_output_handler.h"
+
+namespace frc971 {
+namespace wpilib {
+
+/// This class uses a callback whenever it writes so that the caller can use any
+/// flatbuffer to write to the falcon.
+template <typename T>
+class GenericCANWriter : public LoopOutputHandler<T> {
+ public:
+ GenericCANWriter(
+ ::aos::EventLoop *event_loop,
+ std::function<
+ void(const T &output,
+ std::map<std::string, std::shared_ptr<Falcon>> falcon_map)>
+ write_callback)
+ : LoopOutputHandler<T>(event_loop, "/superstructure"),
+ write_callback_(write_callback) {
+ event_loop->SetRuntimeRealtimePriority(kGenericCANWriterPriority);
+
+ event_loop->OnRun([this]() { WriteConfigs(); });
+ }
+
+ void HandleCANConfiguration(const CANConfiguration &configuration) {
+ for (auto &[_, falcon] : falcon_map_) {
+ falcon->PrintConfigs();
+ }
+
+ if (configuration.reapply()) {
+ WriteConfigs();
+ }
+ }
+
+ void add_falcon(std::string_view name, std::shared_ptr<Falcon> falcon) {
+ falcon_map_.insert({name, std::move(falcon)});
+ }
+
+ static constexpr int kGenericCANWriterPriority = 35;
+
+ private:
+ void WriteConfigs() {
+ for (auto &[_, falcon] : falcon_map_) {
+ falcon->WriteConfigs();
+ }
+ }
+
+ void Write(const T &output) override { write_callback_(output, falcon_map_); }
+
+ void Stop() override {
+ AOS_LOG(WARNING, "Generic CAN output too old.\n");
+ for (auto &[_, falcon] : falcon_map_) {
+ falcon->WriteVoltage(0);
+ }
+ }
+
+ // Maps each name to a falcon to let the caller retreive them when writing
+ std::map<std::string_view, std::shared_ptr<Falcon>> falcon_map_;
+
+ std::function<void(const T &output,
+ std::map<std::string, std::shared_ptr<Falcon>> falcon_map)>
+ write_callback_;
+};
+
+} // namespace wpilib
+} // namespace frc971
diff --git a/y2020/control_loops/python/BUILD b/y2020/control_loops/python/BUILD
index 3833536..0650421 100644
--- a/y2020/control_loops/python/BUILD
+++ b/y2020/control_loops/python/BUILD
@@ -113,19 +113,6 @@
],
)
-py_library(
- name = "flywheel",
- srcs = [
- "flywheel.py",
- ],
- target_compatible_with = ["@platforms//cpu:x86_64"],
- deps = [
- "//frc971/control_loops/python:controls",
- "@pip//matplotlib",
- "@pip//pygobject",
- ],
-)
-
py_binary(
name = "accelerator",
srcs = [
@@ -134,8 +121,8 @@
legacy_create_init = False,
target_compatible_with = ["@platforms//cpu:x86_64"],
deps = [
- ":flywheel",
":python_init",
+ "//frc971/control_loops/python:flywheel",
"@pip//glog",
"@pip//python_gflags",
],
@@ -149,8 +136,8 @@
legacy_create_init = False,
target_compatible_with = ["@platforms//cpu:x86_64"],
deps = [
- ":flywheel",
":python_init",
+ "//frc971/control_loops/python:flywheel",
"@pip//glog",
"@pip//python_gflags",
],
diff --git a/y2020/control_loops/python/accelerator.py b/y2020/control_loops/python/accelerator.py
index 54ec9d3..752f8b5 100644
--- a/y2020/control_loops/python/accelerator.py
+++ b/y2020/control_loops/python/accelerator.py
@@ -1,7 +1,7 @@
#!/usr/bin/python3
from frc971.control_loops.python import control_loop
-from y2020.control_loops.python import flywheel
+from frc971.control_loops.python import flywheel
import numpy
import sys
diff --git a/y2020/control_loops/python/finisher.py b/y2020/control_loops/python/finisher.py
index 7e703fc..29af431 100644
--- a/y2020/control_loops/python/finisher.py
+++ b/y2020/control_loops/python/finisher.py
@@ -1,7 +1,7 @@
#!/usr/bin/python3
from frc971.control_loops.python import control_loop
-from y2020.control_loops.python import flywheel
+from frc971.control_loops.python import flywheel
import numpy
import sys
diff --git a/y2020/control_loops/superstructure/BUILD b/y2020/control_loops/superstructure/BUILD
index 83af834..fc2c0b4 100644
--- a/y2020/control_loops/superstructure/BUILD
+++ b/y2020/control_loops/superstructure/BUILD
@@ -35,6 +35,7 @@
deps = [
"//frc971/control_loops:control_loops_ts_fbs",
"//frc971/control_loops:profiled_subsystem_ts_fbs",
+ "//frc971/control_loops/flywheel:flywheel_controller_status_ts_fbs",
],
)
@@ -44,11 +45,12 @@
"superstructure_status.fbs",
],
gen_reflections = 1,
- includes = [
- "//frc971/control_loops:control_loops_fbs_includes",
- "//frc971/control_loops:profiled_subsystem_fbs_includes",
- ],
target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops:profiled_subsystem_fbs",
+ "//frc971/control_loops/flywheel:flywheel_controller_status_fbs",
+ ],
)
flatbuffer_cc_library(
@@ -133,6 +135,7 @@
"//frc971/control_loops:position_sensor_sim",
"//frc971/control_loops:team_number_test_environment",
"//frc971/control_loops/drivetrain:drivetrain_status_fbs",
+ "//frc971/control_loops/flywheel:flywheel_test_plant",
"//y2020/control_loops/superstructure/hood:hood_plants",
"//y2020/control_loops/superstructure/intake:intake_plants",
"//y2020/control_loops/superstructure/shooter:shooter_plants",
diff --git a/y2020/control_loops/superstructure/shooter/BUILD b/y2020/control_loops/superstructure/shooter/BUILD
index 4b0bf03..82e9fec 100644
--- a/y2020/control_loops/superstructure/shooter/BUILD
+++ b/y2020/control_loops/superstructure/shooter/BUILD
@@ -20,30 +20,13 @@
],
target_compatible_with = ["@platforms//os:linux"],
deps = [
- ":flywheel_controller",
"//frc971/control_loops:control_loop",
"//frc971/control_loops:profiled_subsystem",
+ "//frc971/control_loops/flywheel:flywheel_controller",
"//y2020/control_loops/superstructure:superstructure_goal_fbs",
"//y2020/control_loops/superstructure:superstructure_output_fbs",
"//y2020/control_loops/superstructure:superstructure_position_fbs",
"//y2020/control_loops/superstructure:superstructure_status_fbs",
- ],
-)
-
-cc_library(
- name = "flywheel_controller",
- srcs = [
- "flywheel_controller.cc",
- ],
- hdrs = [
- "flywheel_controller.h",
- ],
- target_compatible_with = ["@platforms//os:linux"],
- deps = [
- "//frc971/control_loops:control_loop",
- "//frc971/control_loops:profiled_subsystem",
- "//y2020/control_loops/superstructure:superstructure_goal_fbs",
- "//y2020/control_loops/superstructure:superstructure_status_fbs",
"//y2020/control_loops/superstructure/accelerator:accelerator_plants",
"//y2020/control_loops/superstructure/finisher:finisher_plants",
],
diff --git a/y2020/control_loops/superstructure/shooter/shooter.cc b/y2020/control_loops/superstructure/shooter/shooter.cc
index 9be6273..c400e34 100644
--- a/y2020/control_loops/superstructure/shooter/shooter.cc
+++ b/y2020/control_loops/superstructure/shooter/shooter.cc
@@ -5,7 +5,9 @@
#include "aos/logging/logging.h"
#include "y2020/control_loops/superstructure/accelerator/accelerator_plant.h"
+#include "y2020/control_loops/superstructure/accelerator/integral_accelerator_plant.h"
#include "y2020/control_loops/superstructure/finisher/finisher_plant.h"
+#include "y2020/control_loops/superstructure/finisher/integral_finisher_plant.h"
namespace y2020 {
namespace control_loops {
@@ -78,11 +80,11 @@
UpToSpeed(goal);
}
- flatbuffers::Offset<FlywheelControllerStatus> finisher_status_offset =
- finisher_.SetStatus(fbb);
- flatbuffers::Offset<FlywheelControllerStatus> accelerator_left_status_offset =
- accelerator_left_.SetStatus(fbb);
- flatbuffers::Offset<FlywheelControllerStatus>
+ flatbuffers::Offset<frc971::control_loops::flywheel::FlywheelControllerStatus>
+ finisher_status_offset = finisher_.SetStatus(fbb);
+ flatbuffers::Offset<frc971::control_loops::flywheel::FlywheelControllerStatus>
+ accelerator_left_status_offset = accelerator_left_.SetStatus(fbb);
+ flatbuffers::Offset<frc971::control_loops::flywheel::FlywheelControllerStatus>
accelerator_right_status_offset = accelerator_right_.SetStatus(fbb);
ShooterStatusBuilder status_builder(*fbb);
diff --git a/y2020/control_loops/superstructure/shooter/shooter.h b/y2020/control_loops/superstructure/shooter/shooter.h
index ccbb326..a1d7be3 100644
--- a/y2020/control_loops/superstructure/shooter/shooter.h
+++ b/y2020/control_loops/superstructure/shooter/shooter.h
@@ -2,8 +2,8 @@
#define Y2020_CONTROL_LOOPS_SHOOTER_SHOOTER_H_
#include "frc971/control_loops/control_loop.h"
+#include "frc971/control_loops/flywheel/flywheel_controller.h"
#include "frc971/control_loops/state_feedback_loop.h"
-#include "y2020/control_loops/superstructure/shooter/flywheel_controller.h"
#include "y2020/control_loops/superstructure/superstructure_goal_generated.h"
#include "y2020/control_loops/superstructure/superstructure_output_generated.h"
#include "y2020/control_loops/superstructure/superstructure_position_generated.h"
@@ -42,7 +42,8 @@
// flywheel and when it gets shot.
static constexpr double kMinFinisherVelocityDipWithBall = 5.0;
- FlywheelController finisher_, accelerator_left_, accelerator_right_;
+ frc971::control_loops::flywheel::FlywheelController finisher_,
+ accelerator_left_, accelerator_right_;
void UpToSpeed(const ShooterGoal *goal);
bool finisher_ready_ = false;
diff --git a/y2020/control_loops/superstructure/superstructure_lib_test.cc b/y2020/control_loops/superstructure/superstructure_lib_test.cc
index d2051df..8b34fc9 100644
--- a/y2020/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2020/control_loops/superstructure/superstructure_lib_test.cc
@@ -11,6 +11,7 @@
#include "aos/network/team_number.h"
#include "frc971/control_loops/capped_test_plant.h"
#include "frc971/control_loops/control_loop_test.h"
+#include "frc971/control_loops/flywheel/flywheel_test_plant.h"
#include "frc971/control_loops/position_sensor_sim.h"
#include "frc971/control_loops/team_number_test_environment.h"
#include "y2020/constants.h"
@@ -51,39 +52,6 @@
typedef Superstructure::PotAndAbsoluteEncoderSubsystem
PotAndAbsoluteEncoderSubsystem;
-class FlywheelPlant : public StateFeedbackPlant<2, 1, 1> {
- public:
- explicit FlywheelPlant(StateFeedbackPlant<2, 1, 1> &&other, double bemf,
- double resistance)
- : StateFeedbackPlant<2, 1, 1>(::std::move(other)),
- bemf_(bemf),
- resistance_(resistance) {}
-
- void CheckU(const Eigen::Matrix<double, 1, 1> &U) override {
- EXPECT_LE(U(0, 0), U_max(0, 0) + 0.00001 + voltage_offset_);
- EXPECT_GE(U(0, 0), U_min(0, 0) - 0.00001 + voltage_offset_);
- }
-
- double motor_current(const Eigen::Matrix<double, 1, 1> U) const {
- return (U(0) - X(1) / bemf_) / resistance_;
- }
-
- double battery_current(const Eigen::Matrix<double, 1, 1> U) const {
- return motor_current(U) * U(0) / 12.0;
- }
-
- double voltage_offset() const { return voltage_offset_; }
- void set_voltage_offset(double voltage_offset) {
- voltage_offset_ = voltage_offset;
- }
-
- private:
- double voltage_offset_ = 0.0;
-
- double bemf_;
- double resistance_;
-};
-
// Class which simulates the superstructure and sends out queue messages with
// the position.
class SuperstructureSimulation {
@@ -111,14 +79,16 @@
.turret.subsystem_params.zeroing_constants
.one_revolution_distance),
accelerator_left_plant_(
- new FlywheelPlant(accelerator::MakeAcceleratorPlant(),
- accelerator::kBemf, accelerator::kResistance)),
+ new frc971::control_loops::flywheel::FlywheelPlant(
+ accelerator::MakeAcceleratorPlant(), accelerator::kBemf,
+ accelerator::kResistance)),
accelerator_right_plant_(
- new FlywheelPlant(accelerator::MakeAcceleratorPlant(),
- accelerator::kBemf, accelerator::kResistance)),
- finisher_plant_(new FlywheelPlant(finisher::MakeFinisherPlant(),
- finisher::kBemf,
- finisher::kResistance)) {
+ new frc971::control_loops::flywheel::FlywheelPlant(
+ accelerator::MakeAcceleratorPlant(), accelerator::kBemf,
+ accelerator::kResistance)),
+ finisher_plant_(new frc971::control_loops::flywheel::FlywheelPlant(
+ finisher::MakeFinisherPlant(), finisher::kBemf,
+ finisher::kResistance)) {
InitializeHoodPosition(constants::Values::kHoodRange().upper);
InitializeIntakePosition(constants::Values::kIntakeRange().upper);
InitializeTurretPosition(constants::Values::kTurretRange().middle());
@@ -413,9 +383,12 @@
::std::unique_ptr<CappedTestPlant> turret_plant_;
PositionSensorSimulator turret_encoder_;
- ::std::unique_ptr<FlywheelPlant> accelerator_left_plant_;
- ::std::unique_ptr<FlywheelPlant> accelerator_right_plant_;
- ::std::unique_ptr<FlywheelPlant> finisher_plant_;
+ ::std::unique_ptr<frc971::control_loops::flywheel::FlywheelPlant>
+ accelerator_left_plant_;
+ ::std::unique_ptr<frc971::control_loops::flywheel::FlywheelPlant>
+ accelerator_right_plant_;
+ ::std::unique_ptr<frc971::control_loops::flywheel::FlywheelPlant>
+ finisher_plant_;
// The acceleration limits to check for while moving.
double peak_hood_acceleration_ = 1e10;
diff --git a/y2020/control_loops/superstructure/superstructure_status.fbs b/y2020/control_loops/superstructure/superstructure_status.fbs
index 611661c..d3f98e5 100644
--- a/y2020/control_loops/superstructure/superstructure_status.fbs
+++ b/y2020/control_loops/superstructure/superstructure_status.fbs
@@ -1,5 +1,6 @@
include "frc971/control_loops/control_loops.fbs";
include "frc971/control_loops/profiled_subsystem.fbs";
+include "frc971/control_loops/flywheel/flywheel_controller_status.fbs";
namespace y2020.control_loops.superstructure;
@@ -9,36 +10,14 @@
TURRET
}
-table FlywheelControllerStatus {
- // The current average velocity in radians/second over the last kHistoryLength
- // in shooter.h
- avg_angular_velocity:double (id: 0);
-
- // The current instantaneous filtered velocity in radians/second.
- angular_velocity:double (id: 1);
-
- // The target speed selected by the lookup table or from manual override
- // Can be compared to velocity to determine if ready.
- angular_velocity_goal:double (id: 2);
-
- // Voltage error.
- voltage_error:double (id: 3);
-
- // The commanded battery current.
- commanded_current:double (id: 4);
-
- // The angular velocity of the flywheel computed using delta x / delta t
- dt_angular_velocity:double (id: 5);
-}
-
table ShooterStatus {
// The final wheel shooting the ball
- finisher:FlywheelControllerStatus (id: 0);
+ finisher:frc971.control_loops.flywheel.FlywheelControllerStatus (id: 0);
// The subsystem to accelerate the ball before the finisher
// Velocity is the fastest (top) wheel
- accelerator_left:FlywheelControllerStatus (id: 1);
- accelerator_right:FlywheelControllerStatus (id: 2);
+ accelerator_left:frc971.control_loops.flywheel.FlywheelControllerStatus (id: 1);
+ accelerator_right:frc971.control_loops.flywheel.FlywheelControllerStatus (id: 2);
// If the shooter is ready, this is true. Note: don't use this for status
// checking, only for plotting. Changes in goal take time to impact this.
diff --git a/y2020/www/field_handler.ts b/y2020/www/field_handler.ts
index 28c35aa..5783b4f 100644
--- a/y2020/www/field_handler.ts
+++ b/y2020/www/field_handler.ts
@@ -2,7 +2,8 @@
import {Connection} from '../../aos/network/www/proxy';
import {Status as DrivetrainStatus} from '../../frc971/control_loops/drivetrain/drivetrain_status_generated';
import {LocalizerDebug, RejectionReason, ImageMatchDebug} from '../control_loops/drivetrain/localizer_debug_generated';
-import {Status as SuperstructureStatus, FlywheelControllerStatus} from '../control_loops/superstructure/superstructure_status_generated'
+import {Status as SuperstructureStatus} from '../control_loops/superstructure/superstructure_status_generated'
+import {FlywheelControllerStatus} from '../../frc971/control_loops/flywheel/flywheel_controller_status_generated'
import {ImageMatchResult} from '../vision/sift/sift_generated';
import {FIELD_LENGTH, FIELD_WIDTH, FT_TO_M, IN_TO_M} from './constants';