Merge "Switch to relative imports in scouting/**/*.ts files"
diff --git a/WORKSPACE b/WORKSPACE
index d6a50c7..2b3531c 100644
--- a/WORKSPACE
+++ b/WORKSPACE
@@ -750,9 +750,9 @@
deps = ["@//third_party/allwpilib/wpimath"],
)
""",
- sha256 = "d9d6c48df9318cf106237a44bf7ad95e4092618bc3ab731092e9b733cacb1ffc",
+ sha256 = "7ffc54bf40814a5c101ea3159af15215f15087298cfc2ae65826f987ccf65499",
urls = [
- "https://maven.ctr-electronics.com/release/com/ctre/phoenixpro/api-cpp/23.0.1/api-cpp-23.0.1-headers.zip",
+ "https://maven.ctr-electronics.com/release/com/ctre/phoenixpro/api-cpp/23.0.5/api-cpp-23.0.5-headers.zip",
],
)
@@ -774,9 +774,9 @@
target_compatible_with = ['@//tools/platforms/hardware:roborio'],
)
""",
- sha256 = "3d228fdf8565de5411a739fa2670d4ef5390acb15ceb4d3cbef8c76b5adc7682",
+ sha256 = "1e8a487cb538388de437d04985512533a9dea79e6c56ee0f319c5eb80260fcab",
urls = [
- "https://maven.ctr-electronics.com/release/com/ctre/phoenixpro/api-cpp/23.0.1/api-cpp-23.0.1-linuxathena.zip",
+ "https://maven.ctr-electronics.com/release/com/ctre/phoenixpro/api-cpp/23.0.5/api-cpp-23.0.5-linuxathena.zip",
],
)
@@ -789,9 +789,9 @@
hdrs = glob(['ctre/**/*.h', 'ctre/**/*.hpp']),
)
""",
- sha256 = "74d79bb3e739d9d6b87311656b0530aaefc211952cc647a3d57776a0cee9efce",
+ sha256 = "51c52dfce4c2491887a7b7380e2f17e93a4092b6ac9f60d716738447a8ebedd7",
urls = [
- "https://maven.ctr-electronics.com/release/com/ctre/phoenixpro/tools/23.0.1/tools-23.0.1-headers.zip",
+ "https://maven.ctr-electronics.com/release/com/ctre/phoenixpro/tools/23.0.5/tools-23.0.5-headers.zip",
],
)
@@ -813,9 +813,9 @@
target_compatible_with = ['@//tools/platforms/hardware:roborio'],
)
""",
- sha256 = "1791b35fdf76aa08ad120e4d689d9440bd386542f63f5c44e4047a06e2e05b9a",
+ sha256 = "9fb137321745c1eff63bdcfe486806afb46ede11ea4d4c59461320845698cc1e",
urls = [
- "https://maven.ctr-electronics.com/release/com/ctre/phoenixpro/tools/23.0.1/tools-23.0.1-linuxathena.zip",
+ "https://maven.ctr-electronics.com/release/com/ctre/phoenixpro/tools/23.0.5/tools-23.0.5-linuxathena.zip",
],
)
diff --git a/aos/BUILD b/aos/BUILD
index b8468bd..0792e67 100644
--- a/aos/BUILD
+++ b/aos/BUILD
@@ -411,9 +411,11 @@
],
data = [
":json_to_flatbuffer_fbs_reflection_out",
+ ":json_to_flatbuffer_test_spaces.json",
],
target_compatible_with = ["@platforms//os:linux"],
deps = [
+ ":flatbuffer_merge",
":json_to_flatbuffer",
":json_to_flatbuffer_fbs",
"//aos/testing:googletest",
diff --git a/aos/json_to_flatbuffer.h b/aos/json_to_flatbuffer.h
index d5e1039..6eab22a 100644
--- a/aos/json_to_flatbuffer.h
+++ b/aos/json_to_flatbuffer.h
@@ -109,10 +109,8 @@
template <typename T>
inline FlatbufferDetachedBuffer<T> JsonFileToFlatbuffer(
const std::string_view path) {
- std::ifstream t{std::string(path)};
- std::istream_iterator<char> start(t), end;
- std::string result(start, end);
- return FlatbufferDetachedBuffer<T>(JsonToFlatbuffer<T>(result));
+ return FlatbufferDetachedBuffer<T>(
+ JsonToFlatbuffer<T>(util::ReadFileToStringOrDie(path)));
}
// Parses a file as a binary flatbuffer or dies.
diff --git a/aos/json_to_flatbuffer_test.cc b/aos/json_to_flatbuffer_test.cc
index 6772826..fddb431 100644
--- a/aos/json_to_flatbuffer_test.cc
+++ b/aos/json_to_flatbuffer_test.cc
@@ -1,5 +1,6 @@
#include "aos/json_to_flatbuffer.h"
+#include "aos/flatbuffer_merge.h"
#include "aos/json_to_flatbuffer_generated.h"
#include "aos/testing/path.h"
#include "flatbuffers/minireflect.h"
@@ -301,5 +302,20 @@
ConfigurationTypeTable()));
}
+TEST_F(JsonToFlatbufferTest, SpacedData) {
+ EXPECT_TRUE(CompareFlatBuffer(
+ FlatbufferDetachedBuffer<VectorOfStrings>(
+ JsonToFlatbuffer<VectorOfStrings>(R"json({
+ "str": [
+ "f o o",
+ "b a r",
+ "foo bar",
+ "bar foo"
+ ]
+})json")),
+ JsonFileToFlatbuffer<VectorOfStrings>(
+ ArtifactPath("aos/json_to_flatbuffer_test_spaces.json"))));
+}
+
} // namespace testing
} // namespace aos
diff --git a/aos/json_to_flatbuffer_test_spaces.json b/aos/json_to_flatbuffer_test_spaces.json
new file mode 100644
index 0000000..c864037
--- /dev/null
+++ b/aos/json_to_flatbuffer_test_spaces.json
@@ -0,0 +1,8 @@
+{
+ "str": [
+ "f o o",
+ "b a r",
+ "foo bar",
+ "bar foo"
+ ]
+}
diff --git a/aos/network/www/BUILD b/aos/network/www/BUILD
index 03f41a5..81b9e16 100644
--- a/aos/network/www/BUILD
+++ b/aos/network/www/BUILD
@@ -1,5 +1,4 @@
-load("@npm//@bazel/typescript:index.bzl", "ts_library")
-load("//tools/build_rules:js.bzl", "rollup_bundle")
+load("//tools/build_rules:js.bzl", "rollup_bundle", "ts_project")
load("//aos:config.bzl", "aos_config")
exports_files(["styles.css"])
@@ -13,7 +12,7 @@
visibility = ["//visibility:public"],
)
-ts_library(
+ts_project(
name = "proxy",
srcs = [
"config_handler.ts",
@@ -31,7 +30,7 @@
],
)
-ts_library(
+ts_project(
name = "main",
srcs = [
"main.ts",
@@ -68,7 +67,7 @@
visibility = ["//aos:__subpackages__"],
)
-ts_library(
+ts_project(
name = "reflection_test_main",
srcs = [
"reflection_test_main.ts",
@@ -83,7 +82,7 @@
],
)
-ts_library(
+ts_project(
name = "reflection_ts",
srcs = ["reflection.ts"],
target_compatible_with = ["@platforms//os:linux"],
@@ -96,7 +95,7 @@
],
)
-ts_library(
+ts_project(
name = "colors",
srcs = [
"colors.ts",
@@ -105,7 +104,7 @@
visibility = ["//visibility:public"],
)
-ts_library(
+ts_project(
name = "plotter",
srcs = [
"plotter.ts",
@@ -115,7 +114,7 @@
deps = [":colors"],
)
-ts_library(
+ts_project(
name = "aos_plotter",
srcs = [
"aos_plotter.ts",
@@ -134,7 +133,7 @@
],
)
-ts_library(
+ts_project(
name = "demo_plot",
srcs = [
"demo_plot.ts",
diff --git a/aos/starter/BUILD b/aos/starter/BUILD
index 5690bda..8d3f4f5 100644
--- a/aos/starter/BUILD
+++ b/aos/starter/BUILD
@@ -226,6 +226,9 @@
srcs = [
"irq_affinity.cc",
],
+ data = [
+ "//aos/starter:rockpi_config.json",
+ ],
visibility = ["//visibility:public"],
deps = [
":irq_affinity_lib",
diff --git a/aos/starter/irq_affinity.cc b/aos/starter/irq_affinity.cc
index 3f32ec9..5c70e9e 100644
--- a/aos/starter/irq_affinity.cc
+++ b/aos/starter/irq_affinity.cc
@@ -14,6 +14,8 @@
DEFINE_string(user, "",
"Starter runs as though this user ran a SUID binary if set.");
+DEFINE_string(irq_config, "rockpi_config.json",
+ "File path of rockpi configuration");
namespace aos {
@@ -265,117 +267,10 @@
aos::FlatbufferDetachedBuffer<aos::Configuration> config =
aos::configuration::ReadConfig(FLAGS_config);
- // TODO(austin): File instead of hard-coded JSON.
aos::FlatbufferDetachedBuffer<aos::starter::IrqAffinityConfig>
irq_affinity_config =
- aos::JsonToFlatbuffer<aos::starter::IrqAffinityConfig>(
- R"json({
- "irqs": [
- {
- "name": "ttyS2",
- "affinity": [1]
- },
- {
- "name": "dw-mci",
- "affinity": [1]
- },
- {
- "name": "mmc1",
- "affinity": [1]
- },
- {
- "name": "rkisp1",
- "affinity": [2]
- },
- {
- "name": "ff3c0000.i2c",
- "affinity": [2]
- },
- {
- "name": "ff3d0000.i2c",
- "affinity": [2]
- },
- {
- "name": "ff6e0000.dma-controller",
- "affinity": [0]
- },
- {
- "name": "ff1d0000.spi",
- "affinity": [0]
- },
- {
- "name": "eth0",
- "affinity": [1]
- }
- ],
- "kthreads": [
- {
- "name": "irq/*-ff940000.hdmi",
- "scheduler": "SCHEDULER_OTHER"
- },
- {
- "name": "irq/*-rockchip_usb2phy",
- "scheduler": "SCHEDULER_OTHER"
- },
- {
- "name": "irq/*-mmc0",
- "scheduler": "SCHEDULER_OTHER"
- },
- {
- "name": "irq/*-mmc1",
- "scheduler": "SCHEDULER_OTHER"
- },
- {
- "name": "irq/*-fe320000.mmc cd",
- "scheduler": "SCHEDULER_OTHER"
- },
- {
- "name": "irq/*-vc4 crtc",
- "scheduler": "SCHEDULER_OTHER"
- },
- {
- "name": "irq/*-rkisp1",
- "scheduler": "SCHEDULER_FIFO",
- "priority": 60,
- "affinity": [2]
- },
- {
- "name": "irq/*-ff3c0000.i2c",
- "scheduler": "SCHEDULER_FIFO",
- "priority": 51,
- "affinity": [2]
- },
- {
- "name": "irq/*-adis16505",
- "scheduler": "SCHEDULER_FIFO",
- "priority": 59,
- "affinity": [0]
- },
- {
- "name": "irq/*-ff6e0000.dma-controller",
- "scheduler": "SCHEDULER_FIFO",
- "priority": 59,
- "affinity": [0]
- },
- {
- "name": "spi0",
- "scheduler": "SCHEDULER_FIFO",
- "priority": 57,
- "affinity": [0]
- },
- {
- "name": "irq/*-eth0",
- "scheduler": "SCHEDULER_FIFO",
- "priority": 10,
- "affinity": [1]
- },
- {
- "name": "irq/*-rockchip_thermal",
- "scheduler": "SCHEDULER_FIFO",
- "priority": 1
- }
- ]
-})json");
+ aos::JsonFileToFlatbuffer<aos::starter::IrqAffinityConfig>(
+ FLAGS_irq_config);
aos::ShmEventLoop shm_event_loop(&config.message());
diff --git a/aos/starter/rockpi_config.json b/aos/starter/rockpi_config.json
new file mode 100644
index 0000000..d4cbaae
--- /dev/null
+++ b/aos/starter/rockpi_config.json
@@ -0,0 +1,107 @@
+{
+ "irqs": [
+ {
+ "name": "ttyS2",
+ "affinity": [1]
+ },
+ {
+ "name": "dw-mci",
+ "affinity": [1]
+ },
+ {
+ "name": "mmc1",
+ "affinity": [1]
+ },
+ {
+ "name": "rkisp1",
+ "affinity": [2]
+ },
+ {
+ "name": "ff3c0000.i2c",
+ "affinity": [2]
+ },
+ {
+ "name": "ff3d0000.i2c",
+ "affinity": [2]
+ },
+ {
+ "name": "ff6e0000.dma-controller",
+ "affinity": [0]
+ },
+ {
+ "name": "ff1d0000.spi",
+ "affinity": [0]
+ },
+ {
+ "name": "eth0",
+ "affinity": [1]
+ }
+ ],
+ "kthreads": [
+ {
+ "name": "irq/*-ff940000.hdmi",
+ "scheduler": "SCHEDULER_OTHER"
+ },
+ {
+ "name": "irq/*-rockchip_usb2phy",
+ "scheduler": "SCHEDULER_OTHER"
+ },
+ {
+ "name": "irq/*-mmc0",
+ "scheduler": "SCHEDULER_OTHER"
+ },
+ {
+ "name": "irq/*-mmc1",
+ "scheduler": "SCHEDULER_OTHER"
+ },
+ {
+ "name": "irq/*-fe320000.mmc cd",
+ "scheduler": "SCHEDULER_OTHER"
+ },
+ {
+ "name": "irq/*-vc4 crtc",
+ "scheduler": "SCHEDULER_OTHER"
+ },
+ {
+ "name": "irq/*-rkisp1",
+ "scheduler": "SCHEDULER_FIFO",
+ "priority": 60,
+ "affinity": [2]
+ },
+ {
+ "name": "irq/*-ff3c0000.i2c",
+ "scheduler": "SCHEDULER_FIFO",
+ "priority": 51,
+ "affinity": [2]
+ },
+ {
+ "name": "irq/*-adis16505",
+ "scheduler": "SCHEDULER_FIFO",
+ "priority": 59,
+ "affinity": [0]
+ },
+ {
+ "name": "irq/*-ff6e0000.dma-controller",
+ "scheduler": "SCHEDULER_FIFO",
+ "priority": 59,
+ "affinity": [0]
+ },
+ {
+ "name": "spi0",
+ "scheduler": "SCHEDULER_FIFO",
+ "priority": 57,
+ "affinity": [0]
+ },
+ {
+ "name": "irq/*-eth0",
+ "scheduler": "SCHEDULER_FIFO",
+ "priority": 10,
+ "affinity": [1]
+ },
+ {
+ "name": "irq/*-rockchip_thermal",
+ "scheduler": "SCHEDULER_FIFO",
+ "priority": 1
+ }
+ ]
+}
diff --git a/build_tests/BUILD b/build_tests/BUILD
index ea05c0b..bfdd76e 100644
--- a/build_tests/BUILD
+++ b/build_tests/BUILD
@@ -1,10 +1,9 @@
+load("//tools/build_rules:js.bzl", "ts_project")
load("@com_google_protobuf//:protobuf.bzl", "cc_proto_library")
load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_go_library", "flatbuffer_py_library")
load("@io_bazel_rules_go//go:def.bzl", "go_binary", "go_library")
load("//tools/build_rules:apache.bzl", "apache_wrapper")
load("@rules_rust//rust:defs.bzl", "rust_binary", "rust_library", "rust_test")
-load("@npm//@bazel/typescript:index.bzl", "ts_library")
-load("@npm//@bazel/concatjs:index.bzl", "karma_web_test_suite")
load("//tools/build_rules:autocxx.bzl", "autocxx_library")
cc_test(
@@ -161,28 +160,11 @@
deps = [":hello_lib"],
)
-ts_library(
+ts_project(
name = "build_tests_ts",
srcs = ["basic.ts"],
)
-ts_library(
- name = "ts_test",
- testonly = True,
- srcs = ["basic_test.ts"],
- deps = [
- ":build_tests_ts",
- "@npm//@types/jasmine",
- ],
-)
-
-karma_web_test_suite(
- name = "karma",
- testonly = True,
- target_compatible_with = ["@platforms//os:linux"],
- deps = [":ts_test"],
-)
-
rust_library(
name = "rust_in_cc_rs",
srcs = ["rust_in_cc.rs"],
diff --git a/frc971/analysis/BUILD b/frc971/analysis/BUILD
index c1ae252..36c450a 100644
--- a/frc971/analysis/BUILD
+++ b/frc971/analysis/BUILD
@@ -1,5 +1,4 @@
-load("@npm//@bazel/typescript:index.bzl", "ts_library")
-load("//tools/build_rules:js.bzl", "rollup_bundle")
+load("//tools/build_rules:js.bzl", "rollup_bundle", "ts_project")
load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
load("//aos:config.bzl", "aos_config")
@@ -33,7 +32,7 @@
deps = ["//aos:configuration_fbs_python"],
)
-ts_library(
+ts_project(
name = "plot_index",
srcs = ["plot_index.ts"],
target_compatible_with = ["@platforms//os:linux"],
@@ -127,7 +126,7 @@
target_compatible_with = ["@platforms//os:linux"],
)
-ts_library(
+ts_project(
name = "plot_data_utils",
srcs = ["plot_data_utils.ts"],
visibility = ["//visibility:public"],
diff --git a/frc971/analysis/cpp_plot/BUILD b/frc971/analysis/cpp_plot/BUILD
index d6e74c3..7f34afe 100644
--- a/frc971/analysis/cpp_plot/BUILD
+++ b/frc971/analysis/cpp_plot/BUILD
@@ -1,9 +1,8 @@
-load("@npm//@bazel/typescript:index.bzl", "ts_library")
-load("//tools/build_rules:js.bzl", "rollup_bundle")
+load("//tools/build_rules:js.bzl", "rollup_bundle", "ts_project")
package(default_visibility = ["//visibility:public"])
-ts_library(
+ts_project(
name = "cpp_plot",
srcs = ["cpp_plot.ts"],
target_compatible_with = ["@platforms//os:linux"],
diff --git a/frc971/constants/BUILD b/frc971/constants/BUILD
index c17dc31..3a7e73a 100644
--- a/frc971/constants/BUILD
+++ b/frc971/constants/BUILD
@@ -45,6 +45,7 @@
],
data = [
"//frc971/constants/testdata:aos_config",
+ "//frc971/constants/testdata:syntax_error.json",
"//frc971/constants/testdata:test_constants.json",
],
target_compatible_with = ["@platforms//os:linux"],
diff --git a/frc971/constants/constants_sender_test.cc b/frc971/constants/constants_sender_test.cc
index 1441767..512cb68 100644
--- a/frc971/constants/constants_sender_test.cc
+++ b/frc971/constants/constants_sender_test.cc
@@ -113,11 +113,11 @@
({
ConstantSender<testdata::ConstantsData, testdata::ConstantsList>
test_syntax(constants_sender_event_loop_.get(),
- "frc971/constants/testdata/syntaxerror.json", 971,
+ "frc971/constants/testdata/syntax_error.json", 971,
"/constants");
event_loop_factory_.RunFor(std::chrono::seconds(1));
}),
- "Error on line 0");
+ "Invalid field name");
}
} // namespace testing
diff --git a/frc971/constants/testdata/BUILD b/frc971/constants/testdata/BUILD
index 04cf8d3..a20264c 100644
--- a/frc971/constants/testdata/BUILD
+++ b/frc971/constants/testdata/BUILD
@@ -1,7 +1,10 @@
load("//aos:config.bzl", "aos_config")
load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
-exports_files(["test_constants.json"])
+exports_files([
+ "test_constants.json",
+ "syntax_error.json",
+])
flatbuffer_cc_library(
name = "constants_list_fbs",
diff --git a/frc971/control_loops/double_jointed_arm/BUILD b/frc971/control_loops/double_jointed_arm/BUILD
new file mode 100644
index 0000000..c7fcefc
--- /dev/null
+++ b/frc971/control_loops/double_jointed_arm/BUILD
@@ -0,0 +1,121 @@
+cc_library(
+ name = "trajectory",
+ srcs = [
+ "trajectory.cc",
+ ],
+ hdrs = [
+ "trajectory.h",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":dynamics",
+ "//aos/logging",
+ "//frc971/control_loops:dlqr",
+ "//frc971/control_loops:jacobian",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)
+
+cc_test(
+ name = "trajectory_test",
+ srcs = [
+ "trajectory_test.cc",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":demo_path",
+ ":dynamics",
+ ":ekf",
+ ":test_constants",
+ ":trajectory",
+ "//aos/testing:googletest",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)
+
+cc_library(
+ name = "dynamics",
+ srcs = [
+ "dynamics.cc",
+ ],
+ hdrs = [
+ "dynamics.h",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops:runge_kutta",
+ "@com_github_gflags_gflags//:gflags",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)
+
+cc_library(
+ name = "demo_path",
+ srcs = [
+ "demo_path.cc",
+ ],
+ hdrs = ["demo_path.h"],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [":trajectory"],
+)
+
+cc_test(
+ name = "dynamics_test",
+ srcs = [
+ "dynamics_test.cc",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":dynamics",
+ ":test_constants",
+ "//aos/testing:googletest",
+ ],
+)
+
+cc_library(
+ name = "ekf",
+ srcs = [
+ "ekf.cc",
+ ],
+ hdrs = [
+ "ekf.h",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":dynamics",
+ "//frc971/control_loops:jacobian",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)
+
+cc_library(
+ name = "graph",
+ srcs = ["graph.cc"],
+ hdrs = ["graph.h"],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+)
+
+cc_test(
+ name = "graph_test",
+ srcs = ["graph_test.cc"],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":graph",
+ "//aos/testing:googletest",
+ ],
+)
+
+cc_library(
+ name = "test_constants",
+ hdrs = ["test_constants.h"],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":dynamics",
+ ],
+)
diff --git a/y2018/control_loops/superstructure/arm/demo_path.cc b/frc971/control_loops/double_jointed_arm/demo_path.cc
similarity index 98%
rename from y2018/control_loops/superstructure/arm/demo_path.cc
rename to frc971/control_loops/double_jointed_arm/demo_path.cc
index 03856c9..8d549d1 100644
--- a/y2018/control_loops/superstructure/arm/demo_path.cc
+++ b/frc971/control_loops/double_jointed_arm/demo_path.cc
@@ -1,12 +1,11 @@
-#include "y2018/control_loops/superstructure/arm/demo_path.h"
+#include "frc971/control_loops/double_jointed_arm/demo_path.h"
#include <array>
#include <initializer_list>
#include <memory>
-namespace y2018 {
+namespace frc971 {
namespace control_loops {
-namespace superstructure {
namespace arm {
::std::vector<::std::array<double, 6>> FlipPath(
@@ -25,7 +24,6 @@
return result;
}
-
::std::unique_ptr<Path> MakeDemoPath() {
return ::std::unique_ptr<Path>(new Path(FlipPath(
{{{1.3583511559969876, 0.99753029519739866, 0.63708920330895369,
@@ -213,6 +211,5 @@
}
} // namespace arm
-} // namespace superstructure
} // namespace control_loops
-} // namespace y2018
+} // namespace frc971
diff --git a/frc971/control_loops/double_jointed_arm/demo_path.h b/frc971/control_loops/double_jointed_arm/demo_path.h
new file mode 100644
index 0000000..9a9d393
--- /dev/null
+++ b/frc971/control_loops/double_jointed_arm/demo_path.h
@@ -0,0 +1,19 @@
+#ifndef FRC971_CONTROL_LOOPS_DOUBLE_JOINTED_ARM_DEMO_PATH_H_
+#define FRC971_CONTROL_LOOPS_DOUBLE_JOINTED_ARM_DEMO_PATH_H_
+
+#include <memory>
+
+#include "frc971/control_loops/double_jointed_arm/trajectory.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace arm {
+
+::std::unique_ptr<Path> MakeDemoPath();
+::std::unique_ptr<Path> MakeReversedDemoPath();
+
+} // namespace arm
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_DOUBLE_JOINTED_ARM_DEMO_PATH_H_
diff --git a/frc971/control_loops/double_jointed_arm/dynamics.cc b/frc971/control_loops/double_jointed_arm/dynamics.cc
new file mode 100644
index 0000000..b5b6c8d
--- /dev/null
+++ b/frc971/control_loops/double_jointed_arm/dynamics.cc
@@ -0,0 +1,37 @@
+#include "frc971/control_loops/double_jointed_arm/dynamics.h"
+
+DEFINE_bool(gravity, true, "If true, enable gravity.");
+
+namespace frc971 {
+namespace control_loops {
+namespace arm {
+
+Dynamics::Dynamics(ArmConstants arm_constants)
+ : arm_constants_(arm_constants),
+
+ K3_((::Eigen::Matrix<double, 2, 2>() << arm_constants_.g1 *
+ arm_constants_.Kt /
+ arm_constants_.resistance,
+ 0.0, 0.0,
+ arm_constants_.g2 * arm_constants_.num_distal_motors *
+ arm_constants_.Kt / arm_constants_.resistance)
+ .finished()),
+
+ K3_inverse_(K3_.inverse()),
+ K4_((::Eigen::Matrix<double, 2, 2>()
+ << arm_constants_.g1 * arm_constants_.g1 * arm_constants_.Kt /
+ (arm_constants_.Kv * arm_constants_.resistance),
+ 0.0, 0.0,
+ arm_constants_.g2 * arm_constants_.g2 * arm_constants_.Kt *
+ arm_constants_.num_distal_motors /
+ (arm_constants_.Kv * arm_constants_.resistance))
+ .finished()),
+ alpha_(arm_constants_.j1 +
+ arm_constants_.r1 * arm_constants_.r1 * arm_constants_.m1 +
+ arm_constants_.l1 * arm_constants_.l1 * arm_constants_.m2),
+ beta_(arm_constants_.l1 * arm_constants_.r2 * arm_constants_.m2),
+ gamma_(arm_constants_.j2 +
+ arm_constants_.r2 * arm_constants_.r2 * arm_constants_.m2) {}
+} // namespace arm
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/double_jointed_arm/dynamics.h b/frc971/control_loops/double_jointed_arm/dynamics.h
new file mode 100644
index 0000000..1e934bc
--- /dev/null
+++ b/frc971/control_loops/double_jointed_arm/dynamics.h
@@ -0,0 +1,195 @@
+#ifndef FRC971_CONTROL_LOOPS_DOUBLE_JOINTED_ARM_DYNAMICS_H_
+#define FRC971_CONTROL_LOOPS_DOUBLE_JOINTED_ARM_DYNAMICS_H_
+
+#include "Eigen/Dense"
+#include "frc971/control_loops/runge_kutta.h"
+#include "gflags/gflags.h"
+
+DECLARE_bool(gravity);
+
+namespace frc971 {
+namespace control_loops {
+namespace arm {
+
+struct ArmConstants {
+ // Below, 1 refers to the proximal joint, and 2 refers to the distal joint.
+ // Length of the joints in meters.
+ double l1;
+ double l2;
+
+ // Mass of the joints in kilograms.
+ double m1;
+ double m2;
+
+ // Moment of inertia of the joints in kg m^2
+ double j1;
+ double j2;
+
+ // Radius of the center of mass of the joints in meters.
+ double r1;
+ double r2;
+
+ // Gear ratios for the two joints.
+ double g1;
+ double g2;
+
+ // motor constants.
+ double efficiency_tweak;
+ double stall_torque;
+ double free_speed;
+ double stall_current;
+ double resistance;
+ double Kv;
+ double Kt;
+
+ // Number of motors on the distal joint.
+ double num_distal_motors;
+};
+
+// This class captures the dynamics of our system. It doesn't actually need to
+// store state yet, so everything can be constexpr and/or static.
+class Dynamics {
+ public:
+ Dynamics(ArmConstants arm_constants);
+ // Generates K1-2 for the arm ODE.
+ // K1 * d^2 theta / dt^2 + K2 * d theta / dt = K3 * V - K4 * d theta/dt
+ // These matricies are missing the velocity factor for K2[1, 0], and K2[0, 1].
+ // You probbaly want MatriciesForState.
+ void NormilizedMatriciesForState(const ::Eigen::Matrix<double, 4, 1> &X,
+ ::Eigen::Matrix<double, 2, 2> *K1_result,
+ ::Eigen::Matrix<double, 2, 2> *K2_result) const {
+ const double angle = X(0, 0) - X(2, 0);
+ const double s = ::std::sin(angle);
+ const double c = ::std::cos(angle);
+ *K1_result << alpha_, c * beta_, c * beta_, gamma_;
+ *K2_result << 0.0, s * beta_, -s * beta_, 0.0;
+ }
+
+ // Generates K1-2 for the arm ODE.
+ // K1 * d^2 theta / dt^2 + K2 * d theta / dt = K3 * V - K4 * d theta/dt
+ void MatriciesForState(const ::Eigen::Matrix<double, 4, 1> &X,
+ ::Eigen::Matrix<double, 2, 2> *K1_result,
+ ::Eigen::Matrix<double, 2, 2> *K2_result) const {
+ NormilizedMatriciesForState(X, K1_result, K2_result);
+ (*K2_result)(1, 0) *= X(1, 0);
+ (*K2_result)(0, 1) *= X(3, 0);
+ }
+
+ // TODO(austin): We may want a way to provide K1 and K2 to save CPU cycles.
+
+ // Calculates the acceleration given the current state and control input.
+ const ::Eigen::Matrix<double, 4, 1> Acceleration(
+ const ::Eigen::Matrix<double, 4, 1> &X,
+ const ::Eigen::Matrix<double, 2, 1> &U) const {
+ ::Eigen::Matrix<double, 2, 2> K1;
+ ::Eigen::Matrix<double, 2, 2> K2;
+
+ MatriciesForState(X, &K1, &K2);
+
+ const ::Eigen::Matrix<double, 2, 1> velocity =
+ (::Eigen::Matrix<double, 2, 1>() << X(1, 0), X(3, 0)).finished();
+
+ const ::Eigen::Matrix<double, 2, 1> torque = K3_ * U - K4_ * velocity;
+ const ::Eigen::Matrix<double, 2, 1> gravity_torque = GravityTorque(X);
+
+ const ::Eigen::Matrix<double, 2, 1> accel =
+ K1.inverse() * (torque + gravity_torque - K2 * velocity);
+
+ return (::Eigen::Matrix<double, 4, 1>() << X(1, 0), accel(0, 0), X(3, 0),
+ accel(1, 0))
+ .finished();
+ }
+
+ // Calculates the acceleration given the current augmented kalman filter state
+ // and control input.
+ const ::Eigen::Matrix<double, 6, 1> EKFAcceleration(
+ const ::Eigen::Matrix<double, 6, 1> &X,
+ const ::Eigen::Matrix<double, 2, 1> &U) const {
+ ::Eigen::Matrix<double, 2, 2> K1;
+ ::Eigen::Matrix<double, 2, 2> K2;
+
+ MatriciesForState(X.block<4, 1>(0, 0), &K1, &K2);
+
+ const ::Eigen::Matrix<double, 2, 1> velocity =
+ (::Eigen::Matrix<double, 2, 1>() << X(1, 0), X(3, 0)).finished();
+
+ const ::Eigen::Matrix<double, 2, 1> torque =
+ K3_ *
+ (U +
+ (::Eigen::Matrix<double, 2, 1>() << X(4, 0), X(5, 0)).finished()) -
+ K4_ * velocity;
+ const ::Eigen::Matrix<double, 2, 1> gravity_torque =
+ GravityTorque(X.block<4, 1>(0, 0));
+
+ const ::Eigen::Matrix<double, 2, 1> accel =
+ K1.inverse() * (torque + gravity_torque - K2 * velocity);
+
+ return (::Eigen::Matrix<double, 6, 1>() << X(1, 0), accel(0, 0), X(3, 0),
+ accel(1, 0), 0.0, 0.0)
+ .finished();
+ }
+
+ // Calculates the voltage required to follow the trajectory. This requires
+ // knowing the current state, desired angular velocity and acceleration.
+ const ::Eigen::Matrix<double, 2, 1> FF_U(
+ const ::Eigen::Matrix<double, 4, 1> &X,
+ const ::Eigen::Matrix<double, 2, 1> &omega_t,
+ const ::Eigen::Matrix<double, 2, 1> &alpha_t) const {
+ ::Eigen::Matrix<double, 2, 2> K1;
+ ::Eigen::Matrix<double, 2, 2> K2;
+
+ MatriciesForState(X, &K1, &K2);
+
+ const ::Eigen::Matrix<double, 2, 1> gravity_torque = GravityTorque(X);
+
+ return K3_inverse_ *
+ (K1 * alpha_t + K2 * omega_t + K4_ * omega_t - gravity_torque);
+ }
+
+ const ::Eigen::Matrix<double, 2, 1> GravityTorque(
+ const ::Eigen::Matrix<double, 4, 1> &X) const {
+ const double accel_due_to_gravity = 9.8 * arm_constants_.efficiency_tweak;
+ return (::Eigen::Matrix<double, 2, 1>()
+ << (arm_constants_.r1 * arm_constants_.m1 +
+ arm_constants_.l1 * arm_constants_.m2) *
+ ::std::sin(X(0)) * accel_due_to_gravity,
+ arm_constants_.r2 * arm_constants_.m2 * ::std::sin(X(2)) *
+ accel_due_to_gravity)
+ .finished() *
+ (FLAGS_gravity ? 1.0 : 0.0);
+ }
+
+ const ::Eigen::Matrix<double, 4, 1> UnboundedDiscreteDynamics(
+ const ::Eigen::Matrix<double, 4, 1> &X,
+ const ::Eigen::Matrix<double, 2, 1> &U, double dt) const {
+ return ::frc971::control_loops::RungeKuttaU(
+ [this](const auto &X, const auto &U) { return Acceleration(X, U); }, X,
+ U, dt);
+ }
+
+ const ::Eigen::Matrix<double, 6, 1> UnboundedEKFDiscreteDynamics(
+ const ::Eigen::Matrix<double, 6, 1> &X,
+ const ::Eigen::Matrix<double, 2, 1> &U, double dt) const {
+ return ::frc971::control_loops::RungeKuttaU(
+ [this](const auto &X, const auto &U) { return EKFAcceleration(X, U); },
+ X, U, dt);
+ }
+
+ const ArmConstants arm_constants_;
+
+ // K3, K4 matricies described above.
+ const ::Eigen::Matrix<double, 2, 2> K3_;
+ const ::Eigen::Matrix<double, 2, 2> K3_inverse_;
+ const ::Eigen::Matrix<double, 2, 2> K4_;
+
+ private:
+ const double alpha_;
+ const double beta_;
+ const double gamma_;
+};
+
+} // namespace arm
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_DOUBLE_JOINTED_ARM_DYNAMICS_H_
diff --git a/frc971/control_loops/double_jointed_arm/dynamics_test.cc b/frc971/control_loops/double_jointed_arm/dynamics_test.cc
new file mode 100644
index 0000000..6e12f75
--- /dev/null
+++ b/frc971/control_loops/double_jointed_arm/dynamics_test.cc
@@ -0,0 +1,53 @@
+#include "frc971/control_loops/double_jointed_arm/dynamics.h"
+#include "frc971/control_loops/double_jointed_arm/test_constants.h"
+
+#include "gtest/gtest.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace arm {
+namespace testing {
+
+// Tests that zero inputs result in no acceleration and no motion.
+// This isn't all that rigerous, but it's a good start.
+TEST(DynamicsTest, Acceleration) {
+ Dynamics dynamics(kArmConstants);
+
+ EXPECT_TRUE(dynamics
+ .Acceleration(::Eigen::Matrix<double, 4, 1>::Zero(),
+ ::Eigen::Matrix<double, 2, 1>::Zero())
+ .isApprox(::Eigen::Matrix<double, 4, 1>::Zero()));
+
+ EXPECT_TRUE(
+ dynamics
+ .UnboundedDiscreteDynamics(::Eigen::Matrix<double, 4, 1>::Zero(),
+ ::Eigen::Matrix<double, 2, 1>::Zero(), 0.1)
+ .isApprox(::Eigen::Matrix<double, 4, 1>::Zero()));
+
+ const ::Eigen::Matrix<double, 4, 1> X =
+ (::Eigen::Matrix<double, 4, 1>() << M_PI / 2.0, 0.0, 0.0, 0.0).finished();
+
+ ::std::cout << dynamics.FF_U(X, ::Eigen::Matrix<double, 2, 1>::Zero(),
+ ::Eigen::Matrix<double, 2, 1>::Zero())
+ << ::std::endl;
+
+ ::std::cout << dynamics.UnboundedDiscreteDynamics(
+ X,
+ dynamics.FF_U(X, ::Eigen::Matrix<double, 2, 1>::Zero(),
+ ::Eigen::Matrix<double, 2, 1>::Zero()),
+ 0.01)
+ << ::std::endl;
+
+ EXPECT_TRUE(dynamics
+ .UnboundedDiscreteDynamics(
+ X,
+ dynamics.FF_U(X, ::Eigen::Matrix<double, 2, 1>::Zero(),
+ ::Eigen::Matrix<double, 2, 1>::Zero()),
+ 0.01)
+ .isApprox(X));
+}
+
+} // namespace testing
+} // namespace arm
+} // namespace control_loops
+} // namespace frc971
diff --git a/y2018/control_loops/superstructure/arm/ekf.cc b/frc971/control_loops/double_jointed_arm/ekf.cc
similarity index 87%
rename from y2018/control_loops/superstructure/arm/ekf.cc
rename to frc971/control_loops/double_jointed_arm/ekf.cc
index 48a71f4..a70e4a6 100644
--- a/y2018/control_loops/superstructure/arm/ekf.cc
+++ b/frc971/control_loops/double_jointed_arm/ekf.cc
@@ -1,19 +1,18 @@
-#include "y2018/control_loops/superstructure/arm/ekf.h"
+#include "frc971/control_loops/double_jointed_arm/ekf.h"
-#include "Eigen/Dense"
#include <iostream>
+#include "Eigen/Dense"
+#include "frc971/control_loops/double_jointed_arm/dynamics.h"
#include "frc971/control_loops/jacobian.h"
-#include "y2018/control_loops/superstructure/arm/dynamics.h"
DEFINE_double(proximal_voltage_error_uncertainty, 8.0,
"Proximal joint voltage error uncertainty.");
DEFINE_double(distal_voltage_error_uncertainty, 2.0,
"Distal joint voltage error uncertainty.");
-namespace y2018 {
+namespace frc971 {
namespace control_loops {
-namespace superstructure {
namespace arm {
namespace {
@@ -29,7 +28,7 @@
.asDiagonal());
} // namespace
-EKF::EKF() {
+EKF::EKF(const Dynamics *dynamics) : dynamics_(dynamics) {
X_hat_.setZero();
Q_covariance =
((::Eigen::DiagonalMatrix<double, 6>().diagonal() << ::std::pow(0.1, 2),
@@ -69,9 +68,12 @@
void EKF::Predict(const ::Eigen::Matrix<double, 2, 1> &U, double dt) {
const ::Eigen::Matrix<double, 6, 6> A =
::frc971::control_loops::NumericalJacobianX<6, 2>(
- Dynamics::UnboundedEKFDiscreteDynamics, X_hat_, U, dt);
+ [this](const auto &X_hat_, const auto &U, double dt) {
+ return dynamics_->UnboundedEKFDiscreteDynamics(X_hat_, U, dt);
+ },
+ X_hat_, U, dt);
- X_hat_ = Dynamics::UnboundedEKFDiscreteDynamics(X_hat_, U, dt);
+ X_hat_ = dynamics_->UnboundedEKFDiscreteDynamics(X_hat_, U, dt);
P_ = A * P_ * A.transpose() + Q_covariance;
}
@@ -83,8 +85,8 @@
.asDiagonal());
// H is the jacobian of the h(x) measurement prediction function
const ::Eigen::Matrix<double, 2, 6> H_jacobian =
- (::Eigen::Matrix<double, 2, 6>() << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0, 1.0, 0.0, 0.0, 0.0)
+ (::Eigen::Matrix<double, 2, 6>() << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+ 0.0, 1.0, 0.0, 0.0, 0.0)
.finished();
// Update step Measurement residual error of proximal and distal joint
@@ -106,6 +108,5 @@
}
} // namespace arm
-} // namespace superstructure
} // namespace control_loops
-} // namespace y2018
+} // namespace frc971
diff --git a/y2018/control_loops/superstructure/arm/ekf.h b/frc971/control_loops/double_jointed_arm/ekf.h
similarity index 81%
rename from y2018/control_loops/superstructure/arm/ekf.h
rename to frc971/control_loops/double_jointed_arm/ekf.h
index 9ae8b47..d969e82 100644
--- a/y2018/control_loops/superstructure/arm/ekf.h
+++ b/frc971/control_loops/double_jointed_arm/ekf.h
@@ -1,11 +1,11 @@
-#ifndef Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_EKF_H_
-#define Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_EKF_H_
+#ifndef FRC971_CONTROL_LOOPS_DOUBLE_JOINTED_ARM_EKF_H_
+#define FRC971_CONTROL_LOOPS_DOUBLE_JOINTED_ARM_EKF_H_
#include "Eigen/Dense"
+#include "frc971/control_loops/double_jointed_arm/dynamics.h"
-namespace y2018 {
+namespace frc971 {
namespace control_loops {
-namespace superstructure {
namespace arm {
// An extended kalman filter for the Arm.
@@ -13,7 +13,7 @@
// [theta0, omega0, theta1, omega1, voltage error0, voltage error1]
class EKF {
public:
- EKF();
+ EKF(const Dynamics *dynamics);
// Resets the internal state back to X. Resets the torque disturbance to 0.
void Reset(const ::Eigen::Matrix<double, 4, 1> &X);
@@ -43,11 +43,12 @@
::Eigen::Matrix<double, 6, 6> P_half_converged_;
::Eigen::Matrix<double, 6, 6> P_converged_;
::Eigen::Matrix<double, 6, 6> P_reset_;
+
+ const Dynamics *dynamics_;
};
} // namespace arm
-} // namespace superstructure
} // namespace control_loops
-} // namespace y2018
+} // namespace frc971
-#endif // Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_EKF_H_
+#endif // FRC971_CONTROL_LOOPS_DOUBLE_JOINTED_ARM_EKF_H_
diff --git a/y2018/control_loops/superstructure/arm/graph.cc b/frc971/control_loops/double_jointed_arm/graph.cc
similarity index 92%
rename from y2018/control_loops/superstructure/arm/graph.cc
rename to frc971/control_loops/double_jointed_arm/graph.cc
index ad6f982..b5a70ab 100644
--- a/y2018/control_loops/superstructure/arm/graph.cc
+++ b/frc971/control_loops/double_jointed_arm/graph.cc
@@ -1,11 +1,10 @@
-#include "y2018/control_loops/superstructure/arm/graph.h"
+#include "frc971/control_loops/double_jointed_arm/graph.h"
#include <algorithm>
#include <cassert>
-namespace y2018 {
+namespace frc971 {
namespace control_loops {
-namespace superstructure {
namespace arm {
SearchGraph::SearchGraph(size_t num_vertexes, std::initializer_list<Edge> edges)
@@ -68,6 +67,5 @@
}
} // namespace arm
-} // namespace superstructure
} // namespace control_loops
-} // namespace y2018
+} // namespace frc971
diff --git a/y2018/control_loops/superstructure/arm/graph.h b/frc971/control_loops/double_jointed_arm/graph.h
similarity index 94%
rename from y2018/control_loops/superstructure/arm/graph.h
rename to frc971/control_loops/double_jointed_arm/graph.h
index 2300d6a..06d396c 100644
--- a/y2018/control_loops/superstructure/arm/graph.h
+++ b/frc971/control_loops/double_jointed_arm/graph.h
@@ -1,5 +1,5 @@
-#ifndef Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_GRAPH_H_
-#define Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_GRAPH_H_
+#ifndef FRC971_CONTROL_LOOPS_DOUBLE_JOINTED_ARM_GRAPH_H_
+#define FRC971_CONTROL_LOOPS_DOUBLE_JOINTED_ARM_GRAPH_H_
#include <algorithm>
#include <cassert>
@@ -8,9 +8,8 @@
#include <memory>
#include <vector>
-namespace y2018 {
+namespace frc971 {
namespace control_loops {
-namespace superstructure {
namespace arm {
// Grr... normal priority queues don't allow modifying the node cost.
@@ -183,8 +182,7 @@
};
} // namespace arm
-} // namespace superstructure
} // namespace control_loops
-} // namespace y2018
+} // namespace frc971
-#endif // Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_GRAPH_H_
+#endif // FRC971_CONTROL_LOOPS_DOUBLE_JOINTED_ARM_GRAPH_H_
diff --git a/y2018/control_loops/superstructure/arm/graph_test.cc b/frc971/control_loops/double_jointed_arm/graph_test.cc
similarity index 90%
rename from y2018/control_loops/superstructure/arm/graph_test.cc
rename to frc971/control_loops/double_jointed_arm/graph_test.cc
index 8f246a5..7b14ced 100644
--- a/y2018/control_loops/superstructure/arm/graph_test.cc
+++ b/frc971/control_loops/double_jointed_arm/graph_test.cc
@@ -1,10 +1,9 @@
-#include "y2018/control_loops/superstructure/arm/graph.h"
+#include "frc971/control_loops/double_jointed_arm/graph.h"
#include "gtest/gtest.h"
-namespace y2018 {
+namespace frc971 {
namespace control_loops {
-namespace superstructure {
namespace arm {
namespace testing {
@@ -68,6 +67,5 @@
} // namespace testing
} // namespace arm
-} // namespace superstructure
} // namespace control_loops
-} // namespace y2018
+} // namespace frc971
diff --git a/frc971/control_loops/double_jointed_arm/test_constants.h b/frc971/control_loops/double_jointed_arm/test_constants.h
new file mode 100644
index 0000000..ea1d3b7
--- /dev/null
+++ b/frc971/control_loops/double_jointed_arm/test_constants.h
@@ -0,0 +1,53 @@
+#ifndef FRC971_CONTROL_LOOPS_DOUBLE_JOINTED_ARM_TEST_CONSTANTS_H_
+#define FRC971_CONTROL_LOOPS_DOUBLE_JOINTED_ARM_TEST_CONSTANTS_H_
+
+#include "frc971/control_loops/double_jointed_arm/dynamics.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace arm {
+namespace testing {
+
+constexpr double kEfficiencyTweak = 0.95;
+constexpr double kStallTorque = 1.41 * kEfficiencyTweak;
+constexpr double kFreeSpeed = (5840.0 / 60.0) * 2.0 * M_PI;
+constexpr double kStallCurrent = 89.0;
+
+constexpr ArmConstants kArmConstants = {
+ .l1 = 46.25 * 0.0254,
+ .l2 = 41.80 * 0.0254,
+ .m1 = 9.34 / 2.2,
+ .m2 = 9.77 / 2.2,
+
+ // Moment of inertia of the joints in kg m^2
+ .j1 = 2957.05 * 0.0002932545454545454,
+ .j2 = 2824.70 * 0.0002932545454545454,
+
+ // Radius of the center of mass of the joints in meters.
+ .r1 = 21.64 * 0.0254,
+ .r2 = 26.70 * 0.0254,
+
+ // Gear ratios for the two joints.
+ .g1 = 140.0,
+ .g2 = 90.0,
+
+ // MiniCIM motor constants.
+ .efficiency_tweak = kEfficiencyTweak,
+ .stall_torque = kStallTorque,
+ .free_speed = kFreeSpeed,
+ .stall_current = kStallCurrent,
+ .resistance = 12.0 / kStallCurrent,
+ .Kv = kFreeSpeed / 12.0,
+ .Kt = kStallTorque / kStallCurrent,
+
+ // Number of motors on the distal joint.
+ .num_distal_motors = 2.0,
+};
+
+} // namespace testing
+} // namespace arm
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_DOUBLE_JOINTED_ARM_TEST_CONSTANTS_H_
+
diff --git a/y2018/control_loops/superstructure/arm/trajectory.cc b/frc971/control_loops/double_jointed_arm/trajectory.cc
similarity index 92%
rename from y2018/control_loops/superstructure/arm/trajectory.cc
rename to frc971/control_loops/double_jointed_arm/trajectory.cc
index c6e09dd..d389eee 100644
--- a/y2018/control_loops/superstructure/arm/trajectory.cc
+++ b/frc971/control_loops/double_jointed_arm/trajectory.cc
@@ -1,20 +1,19 @@
-#include "y2018/control_loops/superstructure/arm/trajectory.h"
+#include "frc971/control_loops/double_jointed_arm/trajectory.h"
#include "Eigen/Dense"
#include "aos/logging/logging.h"
#include "frc971/control_loops/dlqr.h"
+#include "frc971/control_loops/double_jointed_arm/dynamics.h"
#include "frc971/control_loops/jacobian.h"
#include "gflags/gflags.h"
-#include "y2018/control_loops/superstructure/arm/dynamics.h"
DEFINE_double(lqr_proximal_pos, 0.15, "Position LQR gain");
DEFINE_double(lqr_proximal_vel, 4.0, "Velocity LQR gain");
DEFINE_double(lqr_distal_pos, 0.20, "Position LQR gain");
DEFINE_double(lqr_distal_vel, 4.0, "Velocity LQR gain");
-namespace y2018 {
+namespace frc971 {
namespace control_loops {
-namespace superstructure {
namespace arm {
Path Path::Reversed(const Path &p) {
@@ -106,9 +105,9 @@
::Eigen::Matrix<double, 2, 2> K2;
const ::Eigen::Matrix<double, 2, 1> gravity_volts =
- Dynamics::K3_inverse * Dynamics::GravityTorque(X);
+ dynamics_->K3_inverse_ * dynamics_->GravityTorque(X);
- Dynamics::NormilizedMatriciesForState(X, &K1, &K2);
+ dynamics_->NormilizedMatriciesForState(X, &K1, &K2);
const ::Eigen::Matrix<double, 2, 2> omega_square =
(::Eigen::Matrix<double, 2, 2>() << omega(0, 0), 0.0, 0.0, omega(1, 0))
@@ -123,18 +122,18 @@
::std::sqrt(1.0 / ::std::max(0.001, (alpha_unitizer * alpha).norm()));
const ::Eigen::Matrix<double, 2, 1> vk1 =
- Dynamics::K3_inverse * (K1 * alpha + K2 * omega_square * omega);
+ dynamics_->K3_inverse_ * (K1 * alpha + K2 * omega_square * omega);
const ::Eigen::Matrix<double, 2, 1> vk2 =
- Dynamics::K3_inverse * Dynamics::K4 * omega;
+ dynamics_->K3_inverse_ * dynamics_->K4_ * omega;
// Loop through all the various vmin, plan_vmax combinations.
for (const double c : {-plan_vmax, plan_vmax}) {
// Also loop through saturating theta0 and theta1
for (const ::std::tuple<double, double, double> &abgravity :
{::std::tuple<double, double, double>{vk1(0), vk2(0),
- gravity_volts(0)},
+ gravity_volts(0)},
::std::tuple<double, double, double>{vk1(1), vk2(1),
- gravity_volts(1)}}) {
+ gravity_volts(1)}}) {
const double a = ::std::get<0>(abgravity);
const double b = ::std::get<1>(abgravity);
const double gravity = ::std::get<2>(abgravity);
@@ -178,19 +177,19 @@
::Eigen::Matrix<double, 2, 2> K1;
::Eigen::Matrix<double, 2, 2> K2;
- Dynamics::NormilizedMatriciesForState(X, &K1, &K2);
+ dynamics_->NormilizedMatriciesForState(X, &K1, &K2);
const ::Eigen::Matrix<double, 2, 2> omega_square =
(::Eigen::Matrix<double, 2, 2>() << omega(0, 0), 0.0, 0.0, omega(1, 0))
.finished();
const ::Eigen::Matrix<double, 2, 1> k_constant =
- Dynamics::K3_inverse *
+ dynamics_->K3_inverse_ *
((K1 * alpha + K2 * omega_square * omega) * goal_velocity *
goal_velocity +
- Dynamics::K4 * omega * goal_velocity - Dynamics::GravityTorque(X));
+ dynamics_->K4_ * omega * goal_velocity - dynamics_->GravityTorque(X));
const ::Eigen::Matrix<double, 2, 1> k_scalar =
- Dynamics::K3_inverse * K1 * omega;
+ dynamics_->K3_inverse_ * K1 * omega;
const double constraint_goal_acceleration =
::std::sqrt(
@@ -236,19 +235,19 @@
::Eigen::Matrix<double, 2, 2> K1;
::Eigen::Matrix<double, 2, 2> K2;
- Dynamics::NormilizedMatriciesForState(X, &K1, &K2);
+ dynamics_->NormilizedMatriciesForState(X, &K1, &K2);
const ::Eigen::Matrix<double, 2, 2> omega_square =
(::Eigen::Matrix<double, 2, 2>() << omega(0, 0), 0.0, 0.0, omega(1, 0))
.finished();
const ::Eigen::Matrix<double, 2, 1> k_constant =
- Dynamics::K3_inverse *
+ dynamics_->K3_inverse_ *
((K1 * alpha + K2 * omega_square * omega) * goal_velocity *
goal_velocity +
- Dynamics::K4 * omega * goal_velocity - Dynamics::GravityTorque(X));
+ dynamics_->K4_ * omega * goal_velocity - dynamics_->GravityTorque(X));
const ::Eigen::Matrix<double, 2, 1> k_scalar =
- Dynamics::K3_inverse * K1 * omega;
+ dynamics_->K3_inverse_ * K1 * omega;
const double constraint_goal_acceleration =
::std::sqrt(
@@ -390,12 +389,22 @@
.finished()
.asDiagonal();
+ const auto x_blocked = X.block<4, 1>(0, 0);
+
const ::Eigen::Matrix<double, 4, 4> final_A =
::frc971::control_loops::NumericalJacobianX<4, 2>(
- Dynamics::UnboundedDiscreteDynamics, X.block<4, 1>(0, 0), U, 0.00505);
+ [this](const auto &x_blocked, const auto &U, double dt) {
+ return this->dynamics_->UnboundedDiscreteDynamics(
+ x_blocked, U, dt);
+ },
+ x_blocked, U, 0.00505);
+
const ::Eigen::Matrix<double, 4, 2> final_B =
::frc971::control_loops::NumericalJacobianU<4, 2>(
- Dynamics::UnboundedDiscreteDynamics, X.block<4, 1>(0, 0), U, 0.00505);
+ [this](const auto &x_blocked, const auto &U, double dt) {
+ return this->dynamics_->UnboundedDiscreteDynamics(x_blocked, U, dt);
+ },
+ x_blocked, U, 0.00505);
::Eigen::Matrix<double, 4, 4> S;
::Eigen::Matrix<double, 2, 4> sub_K;
@@ -441,7 +450,7 @@
*saturation_goal_acceleration);
const ::Eigen::Matrix<double, 6, 1> R = trajectory.R(theta_t, omega_t);
const ::Eigen::Matrix<double, 2, 1> U_ff =
- Dynamics::FF_U(R.block<4, 1>(0, 0), omega_t, alpha_t);
+ dynamics_->FF_U(R.block<4, 1>(0, 0), omega_t, alpha_t);
*U = U_ff + K * (R - X);
}
@@ -494,11 +503,11 @@
U_unsaturated_.setZero();
} else {
const ::Eigen::Matrix<double, 6, 1> R =
- trajectory_->R(theta_, ::Eigen::Matrix<double, 2, 1>::Zero());
+ Trajectory::R(theta_, ::Eigen::Matrix<double, 2, 1>::Zero());
- U_ff_ = Dynamics::FF_U(X.block<4, 1>(0, 0),
- ::Eigen::Matrix<double, 2, 1>::Zero(),
- ::Eigen::Matrix<double, 2, 1>::Zero());
+ U_ff_ = dynamics_->FF_U(
+ X.block<4, 1>(0, 0), ::Eigen::Matrix<double, 2, 1>::Zero(),
+ ::Eigen::Matrix<double, 2, 1>::Zero());
const ::Eigen::Matrix<double, 2, 6> K = K_at_state(X, U_ff_);
U_ = U_unsaturated_ = U_ff_ + K * (R - X);
@@ -558,7 +567,7 @@
const ::Eigen::Matrix<double, 6, 1> R = trajectory_->R(theta_t, omega_t);
- U_ff_ = Dynamics::FF_U(R.block<4, 1>(0, 0), omega_t, alpha_t);
+ U_ff_ = dynamics_->FF_U(R.block<4, 1>(0, 0), omega_t, alpha_t);
const ::Eigen::Matrix<double, 2, 6> K = K_at_state(X, U_ff_);
U_ = U_unsaturated_ = U_ff_ + K * (R - X);
@@ -613,6 +622,5 @@
}
} // namespace arm
-} // namespace superstructure
} // namespace control_loops
-} // namespace y2018
+} // namespace frc971
diff --git a/y2018/control_loops/superstructure/arm/trajectory.h b/frc971/control_loops/double_jointed_arm/trajectory.h
similarity index 93%
rename from y2018/control_loops/superstructure/arm/trajectory.h
rename to frc971/control_loops/double_jointed_arm/trajectory.h
index a3a4246..a194a03 100644
--- a/y2018/control_loops/superstructure/arm/trajectory.h
+++ b/frc971/control_loops/double_jointed_arm/trajectory.h
@@ -1,5 +1,5 @@
-#ifndef Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_TRAJECTORY_H_
-#define Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_TRAJECTORY_H_
+#ifndef FRC971_CONTROL_LOOPS_DOUBLE_JOINTED_ARM_TRAJECTORY_H_
+#define FRC971_CONTROL_LOOPS_DOUBLE_JOINTED_ARM_TRAJECTORY_H_
#include <array>
#include <initializer_list>
@@ -7,10 +7,10 @@
#include <vector>
#include "Eigen/Dense"
+#include "frc971/control_loops/double_jointed_arm/dynamics.h"
-namespace y2018 {
+namespace frc971 {
namespace control_loops {
-namespace superstructure {
namespace arm {
// This class represents a path in theta0, theta1 space. It also returns the
@@ -90,8 +90,10 @@
public:
// Constructs a trajectory (but doesn't calculate it) given a path and a step
// size.
- Trajectory(::std::unique_ptr<const Path> path, double gridsize)
- : path_(::std::move(path)),
+ Trajectory(const Dynamics *dynamics, ::std::unique_ptr<const Path> path,
+ double gridsize)
+ : dynamics_(dynamics),
+ path_(::std::move(path)),
num_plan_points_(
static_cast<size_t>(::std::ceil(path_->length() / gridsize) + 1)),
step_size_(path_->length() /
@@ -220,7 +222,8 @@
double GetDAcceleration(double distance) const {
return GetDAcceleration(distance, max_dvelocity_);
}
- double GetDAcceleration(double distance, const ::std::vector<double> &plan) const {
+ double GetDAcceleration(double distance,
+ const ::std::vector<double> &plan) const {
::std::pair<size_t, size_t> indices = IndicesForDistance(distance);
const double v0 = plan[indices.first];
const double v1 = plan[indices.second];
@@ -276,6 +279,7 @@
const Path &path() const { return *path_; }
+
private:
friend class testing::TrajectoryTest_IndicesForDistanceTest_Test;
@@ -298,6 +302,8 @@
return ::std::pair<size_t, size_t>(lower_index, lower_index + 1);
}
+ const Dynamics *dynamics_;
+
// The path to follow.
::std::unique_ptr<const Path> path_;
// The number of points in the plan.
@@ -315,14 +321,16 @@
// This class tracks the current goal along trajectories and paths.
class TrajectoryFollower {
public:
- TrajectoryFollower(const ::Eigen::Matrix<double, 2, 1> &theta)
- : trajectory_(nullptr), theta_(theta) {
+ TrajectoryFollower(const Dynamics *dynamics,
+ const ::Eigen::Matrix<double, 2, 1> &theta)
+ : dynamics_(dynamics), trajectory_(nullptr), theta_(theta) {
omega_.setZero();
last_K_.setZero();
Reset();
}
- TrajectoryFollower(Trajectory *const trajectory) : trajectory_(trajectory) {
+ TrajectoryFollower(const Dynamics *dynamics, Trajectory *const trajectory)
+ : dynamics_(dynamics), trajectory_(trajectory) {
last_K_.setZero();
Reset();
}
@@ -405,6 +413,7 @@
int failed_solutions() const { return failed_solutions_; }
private:
+ const Dynamics *dynamics_;
// The trajectory plan.
const Trajectory *trajectory_ = nullptr;
@@ -431,8 +440,7 @@
};
} // namespace arm
-} // namespace superstructure
} // namespace control_loops
-} // namespace y2018
+} // namespace frc971
-#endif // Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_TRAJECTORY_H_
+#endif // FRC971_CONTROL_LOOPS_DOUBLE_JOINTED_ARM_TRAJECTORY_H_
diff --git a/y2018/control_loops/superstructure/arm/trajectory_test.cc b/frc971/control_loops/double_jointed_arm/trajectory_test.cc
similarity index 86%
rename from y2018/control_loops/superstructure/arm/trajectory_test.cc
rename to frc971/control_loops/double_jointed_arm/trajectory_test.cc
index b91c5a4..e700282 100644
--- a/y2018/control_loops/superstructure/arm/trajectory_test.cc
+++ b/frc971/control_loops/double_jointed_arm/trajectory_test.cc
@@ -1,13 +1,13 @@
-#include "y2018/control_loops/superstructure/arm/trajectory.h"
+#include "frc971/control_loops/double_jointed_arm/trajectory.h"
+#include "frc971/control_loops/double_jointed_arm/test_constants.h"
+#include "frc971/control_loops/double_jointed_arm/demo_path.h"
+#include "frc971/control_loops/double_jointed_arm/dynamics.h"
+#include "frc971/control_loops/double_jointed_arm/ekf.h"
#include "gtest/gtest.h"
-#include "y2018/control_loops/superstructure/arm/demo_path.h"
-#include "y2018/control_loops/superstructure/arm/dynamics.h"
-#include "y2018/control_loops/superstructure/arm/ekf.h"
-namespace y2018 {
+namespace frc971 {
namespace control_loops {
-namespace superstructure {
namespace arm {
namespace testing {
@@ -75,20 +75,22 @@
(::Eigen::Matrix<double, 2, 1>() << 3.0, 0.0).finished()));
}
-
-// Tests that we can compute the indices of the plan for a given distance correctly.
+// Tests that we can compute the indices of the plan for a given distance
+// correctly.
TEST(TrajectoryTest, IndicesForDistanceTest) {
// Start with a stupid simple plan.
Path p({{{0.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
{{1.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
{{2.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
{{3.0, 0.0, 1.0, 0.0, 0.0, 0.0}}});
- Trajectory t(::std::unique_ptr<Path>(new Path(p)), 0.1);
+ Dynamics dynamics(kArmConstants);
+ Trajectory t(&dynamics, ::std::unique_ptr<Path>(new Path(p)), 0.1);
// 0 - 3.0 every 0.1 should be 31 points.
EXPECT_EQ(t.num_plan_points(), 31);
- // Verify that something centered in a grid cell returns the points on either side.
+ // Verify that something centered in a grid cell returns the points on either
+ // side.
EXPECT_EQ(::std::make_pair(static_cast<size_t>(0), static_cast<size_t>(1)),
t.IndicesForDistance(0.05));
EXPECT_EQ(::std::make_pair(static_cast<size_t>(1), static_cast<size_t>(2)),
@@ -145,17 +147,21 @@
EXPECT_NEAR(path->length(), reversed_path->length(), 1e-6);
for (double d = 0; d < path->length(); d += 0.01) {
- EXPECT_TRUE(path->Theta(d).isApprox(reversed_path->Theta(path->length() - d)));
- EXPECT_TRUE(path->Omega(d).isApprox(-reversed_path->Omega(path->length() - d)));
- EXPECT_TRUE(path->Alpha(d).isApprox(reversed_path->Alpha(path->length() - d)));
+ EXPECT_TRUE(
+ path->Theta(d).isApprox(reversed_path->Theta(path->length() - d)));
+ EXPECT_TRUE(
+ path->Omega(d).isApprox(-reversed_path->Omega(path->length() - d)));
+ EXPECT_TRUE(
+ path->Alpha(d).isApprox(reversed_path->Alpha(path->length() - d)));
}
}
// Tests that we can follow a path. Look at :trajectory_plot if you want to see
// the path.
TEST(TrajectoryTest, RunTrajectory) {
+ Dynamics dynamics(kArmConstants);
::std::unique_ptr<Path> path = MakeDemoPath();
- Trajectory trajectory(::std::move(path), 0.001);
+ Trajectory trajectory(&dynamics, ::std::move(path), 0.001);
constexpr double kAlpha0Max = 40.0;
constexpr double kAlpha1Max = 60.0;
@@ -174,16 +180,17 @@
X << theta_t(0), 0.0, theta_t(1), 0.0;
}
- EKF arm_ekf;
+ EKF arm_ekf(&dynamics);
arm_ekf.Reset(X);
- TrajectoryFollower follower(&trajectory);
+ TrajectoryFollower follower(&dynamics, &trajectory);
constexpr double sim_dt = 0.00505;
while (t < 1.0) {
arm_ekf.Correct((::Eigen::Matrix<double, 2, 1>() << X(0), X(2)).finished(),
sim_dt);
follower.Update(arm_ekf.X_hat(), false, sim_dt, vmax, 12.0);
- X = Dynamics::UnboundedDiscreteDynamics(X, follower.U(), sim_dt);
+
+ X = dynamics.UnboundedDiscreteDynamics(X, follower.U(), sim_dt);
arm_ekf.Predict(follower.U(), sim_dt);
t += sim_dt;
}
@@ -204,6 +211,5 @@
} // namespace testing
} // namespace arm
-} // namespace superstructure
} // namespace control_loops
-} // namespace y2018
+} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index 13c8ade..86c1ec0 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -1,9 +1,9 @@
+load("//tools/build_rules:js.bzl", "ts_project")
load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
load("//aos:config.bzl", "aos_config")
load("//tools/build_rules:select.bzl", "cpu_select")
load("//aos:flatbuffers.bzl", "cc_static_flatbuffer")
-load("@npm//@bazel/typescript:index.bzl", "ts_library")
package(default_visibility = ["//visibility:public"])
@@ -786,7 +786,7 @@
],
)
-ts_library(
+ts_project(
name = "down_estimator_plotter",
srcs = ["down_estimator_plotter.ts"],
target_compatible_with = ["@platforms//os:linux"],
@@ -798,7 +798,7 @@
],
)
-ts_library(
+ts_project(
name = "spline_plotter",
srcs = ["spline_plotter.ts"],
target_compatible_with = ["@platforms//os:linux"],
@@ -809,7 +809,7 @@
],
)
-ts_library(
+ts_project(
name = "drivetrain_plotter",
srcs = ["drivetrain_plotter.ts"],
target_compatible_with = ["@platforms//os:linux"],
@@ -821,7 +821,7 @@
],
)
-ts_library(
+ts_project(
name = "robot_state_plotter",
srcs = ["robot_state_plotter.ts"],
target_compatible_with = ["@platforms//os:linux"],
diff --git a/frc971/image_streamer/www/BUILD b/frc971/image_streamer/www/BUILD
index 908c5b4..f808a73 100644
--- a/frc971/image_streamer/www/BUILD
+++ b/frc971/image_streamer/www/BUILD
@@ -1,5 +1,4 @@
-load("@npm//@bazel/typescript:index.bzl", "ts_library")
-load("//tools/build_rules:js.bzl", "rollup_bundle")
+load("//tools/build_rules:js.bzl", "rollup_bundle", "ts_project")
load("//frc971/downloader:downloader.bzl", "aos_downloader_dir")
package(default_visibility = ["//visibility:public"])
@@ -12,7 +11,7 @@
]),
)
-ts_library(
+ts_project(
name = "proxy",
srcs = [
"proxy.ts",
@@ -23,7 +22,7 @@
],
)
-ts_library(
+ts_project(
name = "main",
srcs = [
"main.ts",
diff --git a/frc971/wpilib/BUILD b/frc971/wpilib/BUILD
index eb0571c..ca3dd0c 100644
--- a/frc971/wpilib/BUILD
+++ b/frc971/wpilib/BUILD
@@ -1,4 +1,4 @@
-load("@npm//@bazel/typescript:index.bzl", "ts_library")
+load("//tools/build_rules:js.bzl", "ts_project")
load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
load("//aos:config.bzl", "aos_config")
@@ -469,7 +469,7 @@
],
)
-ts_library(
+ts_project(
name = "imu_plot_utils",
srcs = ["imu_plot_utils.ts"],
target_compatible_with = ["@platforms//os:linux"],
@@ -484,7 +484,7 @@
],
)
-ts_library(
+ts_project(
name = "imu_plotter",
srcs = ["imu_plotter.ts"],
target_compatible_with = ["@platforms//os:linux"],
diff --git a/scouting/scouting_test.ts b/scouting/scouting_test.ts
index 8623fa1..cbeffc1 100644
--- a/scouting/scouting_test.ts
+++ b/scouting/scouting_test.ts
@@ -327,7 +327,7 @@
// Navigate to Notes Page.
await loadPage();
await element(by.cssContainingText('.nav-link', 'Notes')).click();
- expect(await element(by.id('page-title')).getText()).toEqual('Notes');
+ expect(await getHeadingText()).toEqual('Notes');
// Add first team.
await setTextboxByIdTo('team_number_notes', '1234');
@@ -336,7 +336,7 @@
// Add note and select keyword for first team.
expect(await element(by.id('team-key-1')).getText()).toEqual('1234');
await element(by.id('text-input-1')).sendKeys('Good Driving');
- await element(by.id('Good Driving_0')).click();
+ await element(by.id('good_driving_0')).click();
// Navigate to add team selection and add another team.
await element(by.id('add-team-button')).click();
@@ -346,7 +346,7 @@
// Add note and select keyword for second team.
expect(await element(by.id('team-key-2')).getText()).toEqual('1235');
await element(by.id('text-input-2')).sendKeys('Bad Driving');
- await element(by.id('Bad Driving_1')).click();
+ await element(by.id('bad_driving_1')).click();
// Submit Notes.
await element(by.buttonText('Submit')).click();
@@ -359,7 +359,7 @@
// Navigate to Notes Page.
await loadPage();
await element(by.cssContainingText('.nav-link', 'Notes')).click();
- expect(await element(by.id('page-title')).getText()).toEqual('Notes');
+ expect(await getHeadingText()).toEqual('Notes');
// Add first team.
await setTextboxByIdTo('team_number_notes', '1234');
@@ -395,9 +395,7 @@
// Navigate to Driver Ranking Page.
await loadPage();
await element(by.cssContainingText('.nav-link', 'Driver Ranking')).click();
- expect(await element(by.id('page-title')).getText()).toEqual(
- 'Driver Ranking'
- );
+ expect(await getHeadingText()).toEqual('Driver Ranking');
// Input match and team numbers.
await setTextboxByIdTo('match_number_selection', '11');
diff --git a/scouting/testing/BUILD b/scouting/testing/BUILD
index 7e26763..7f0b0dd 100644
--- a/scouting/testing/BUILD
+++ b/scouting/testing/BUILD
@@ -8,6 +8,7 @@
"//scouting",
"//scouting/db/testdb_server",
"//scouting/scraping:test_data",
+ "@rules_python//python/runfiles",
],
visibility = ["//visibility:public"],
)
diff --git a/scouting/testing/scouting_test_servers.py b/scouting/testing/scouting_test_servers.py
index 9df23c1..d1e4e32 100644
--- a/scouting/testing/scouting_test_servers.py
+++ b/scouting/testing/scouting_test_servers.py
@@ -18,6 +18,10 @@
import time
from typing import List
+from rules_python.python.runfiles import runfiles
+
+RUNFILES = runfiles.Create()
+
def wait_for_server(port: int):
"""Waits for the server at the specified port to respond to TCP connections."""
@@ -59,15 +63,22 @@
tba_api_dir = tmpdir / "api" / "v3" / "event" / f"{year}{event_code}"
tba_api_dir.mkdir(parents=True, exist_ok=True)
(tba_api_dir / "matches").write_text(
- Path(f"scouting/scraping/test_data/{year}_{event_code}.json").
- read_text())
+ Path(
+ RUNFILES.Rlocation(
+ f"org_frc971/scouting/scraping/test_data/{year}_{event_code}.json"
+ )).read_text())
class Runner:
"""Helps manage the services we need for testing the scouting app."""
- def start(self, port: int):
- """Starts the services needed for testing the scouting app."""
+ def start(self, port: int, notify_fd: int = 0):
+ """Starts the services needed for testing the scouting app.
+
+ if notify_fd is set to a non-zero value, the string "READY" is written
+ to that file descriptor once everything is set up.
+ """
+
self.tmpdir = Path(os.environ["TEST_TMPDIR"]) / "servers"
self.tmpdir.mkdir(exist_ok=True)
@@ -76,12 +87,15 @@
# The database needs to be running and addressable before the scouting
# webserver can start.
- self.testdb_server = subprocess.Popen(
- ["scouting/db/testdb_server/testdb_server_/testdb_server"])
+ self.testdb_server = subprocess.Popen([
+ RUNFILES.Rlocation(
+ "org_frc971/scouting/db/testdb_server/testdb_server_/testdb_server"
+ )
+ ])
wait_for_server(5432)
self.webserver = subprocess.Popen([
- "scouting/scouting",
+ RUNFILES.Rlocation("org_frc971/scouting/scouting"),
f"--port={port}",
f"--db_config={db_config}",
f"--tba_config={tba_config}",
@@ -99,6 +113,10 @@
wait_for_server(7000)
wait_for_server(port)
+ if notify_fd:
+ with os.fdopen(notify_fd, "w") as file:
+ file.write("READY")
+
def stop(self):
"""Stops the services needed for testing the scouting app."""
servers = (self.webserver, self.testdb_server, self.fake_tba_api)
@@ -127,10 +145,17 @@
parser.add_argument("--port",
type=int,
help="The port for the actual web server.")
+ parser.add_argument(
+ "--notify_fd",
+ type=int,
+ default=0,
+ help=("If non-zero, indicates a file descriptor to which 'READY' is "
+ "written when everything has started up."),
+ )
args = parser.parse_args(argv[1:])
runner = Runner()
- runner.start(args.port)
+ runner.start(args.port, args.notify_fd)
# Wait until we're asked to shut down via CTRL-C or SIGTERM.
signal.signal(signal.SIGINT, discard_signal)
diff --git a/scouting/www/counter_button/package.json b/scouting/www/counter_button/package.json
new file mode 100644
index 0000000..61eb83b
--- /dev/null
+++ b/scouting/www/counter_button/package.json
@@ -0,0 +1,4 @@
+{
+ "name": "@org_frc971/scouting/www/counter_button",
+ "private": true
+}
diff --git a/scouting/www/driver_ranking/driver_ranking.ng.html b/scouting/www/driver_ranking/driver_ranking.ng.html
index 452359c..1fa1bc8 100644
--- a/scouting/www/driver_ranking/driver_ranking.ng.html
+++ b/scouting/www/driver_ranking/driver_ranking.ng.html
@@ -1,4 +1,6 @@
-<h2 id="page-title">Driver Ranking</h2>
+<div class="header">
+ <h2>Driver Ranking</h2>
+</div>
<ng-container [ngSwitch]="section">
<div *ngSwitchCase="'TeamSelection'">
diff --git a/scouting/www/driver_ranking/package.json b/scouting/www/driver_ranking/package.json
new file mode 100644
index 0000000..06472cf
--- /dev/null
+++ b/scouting/www/driver_ranking/package.json
@@ -0,0 +1,7 @@
+{
+ "name": "@org_frc971/scouting/www/driver_ranking",
+ "private": true,
+ "dependencies": {
+ "@angular/forms": "15.0.1"
+ }
+}
diff --git a/scouting/www/entry/package.json b/scouting/www/entry/package.json
new file mode 100644
index 0000000..5ecf57a
--- /dev/null
+++ b/scouting/www/entry/package.json
@@ -0,0 +1,8 @@
+{
+ "name": "@org_frc971/scouting/www/entry",
+ "private": true,
+ "dependencies": {
+ "@org_frc971/scouting/www/counter_button": "workspace:*",
+ "@angular/forms": "15.0.1"
+ }
+}
diff --git a/scouting/www/import_match_list/package.json b/scouting/www/import_match_list/package.json
new file mode 100644
index 0000000..d80b0dc
--- /dev/null
+++ b/scouting/www/import_match_list/package.json
@@ -0,0 +1,7 @@
+{
+ "name": "@org_frc971/scouting/www/import_match_list",
+ "private": true,
+ "dependencies": {
+ "@angular/forms": "15.0.1"
+ }
+}
diff --git a/scouting/www/match_list/package.json b/scouting/www/match_list/package.json
new file mode 100644
index 0000000..3a02094
--- /dev/null
+++ b/scouting/www/match_list/package.json
@@ -0,0 +1,8 @@
+{
+ "name": "@org_frc971/scouting/www/match_list",
+ "private": true,
+ "dependencies": {
+ "@org_frc971/scouting/www/rpc": "workspace:*",
+ "@angular/forms": "15.0.1"
+ }
+}
diff --git a/scouting/www/notes/notes.component.ts b/scouting/www/notes/notes.component.ts
index bcf6890..177ad44 100644
--- a/scouting/www/notes/notes.component.ts
+++ b/scouting/www/notes/notes.component.ts
@@ -175,4 +175,8 @@
this.errorMessage = '';
this.section = 'TeamSelection';
}
+
+ labelToId(label: String): String {
+ return label.replaceAll(' ', '_').toLowerCase();
+ }
}
diff --git a/scouting/www/notes/notes.ng.html b/scouting/www/notes/notes.ng.html
index b51a7ce..cb5e8ef 100644
--- a/scouting/www/notes/notes.ng.html
+++ b/scouting/www/notes/notes.ng.html
@@ -1,4 +1,6 @@
-<h2 id="page-title">Notes</h2>
+<div class="header">
+ <h2>Notes</h2>
+</div>
<ng-container [ngSwitch]="section">
<div *ngSwitchCase="'TeamSelection'">
@@ -58,7 +60,7 @@
class="form-check-input"
[(ngModel)]="newData[i]['keywordsData'][key]"
type="checkbox"
- id="{{KEYWORD_CHECKBOX_LABELS[key]}}_{{i}}"
+ id="{{labelToId(KEYWORD_CHECKBOX_LABELS[key])}}_{{i}}"
name="{{KEYWORD_CHECKBOX_LABELS[key]}}"
/>
<label
@@ -81,7 +83,7 @@
class="form-check-input"
[(ngModel)]="newData[i]['keywordsData'][key]"
type="checkbox"
- id="{{KEYWORD_CHECKBOX_LABELS[key]}}"
+ id="{{labelToId(KEYWORD_CHECKBOX_LABELS[key])}}"
name="{{KEYWORD_CHECKBOX_LABELS[key]}}"
/>
<label
diff --git a/scouting/www/notes/package.json b/scouting/www/notes/package.json
new file mode 100644
index 0000000..8cbeb94
--- /dev/null
+++ b/scouting/www/notes/package.json
@@ -0,0 +1,7 @@
+{
+ "name": "@org_frc971/scouting/www/notes",
+ "private": true,
+ "dependencies": {
+ "@angular/forms": "15.0.1"
+ }
+}
diff --git a/scouting/www/package.json b/scouting/www/package.json
new file mode 100644
index 0000000..3ab0035
--- /dev/null
+++ b/scouting/www/package.json
@@ -0,0 +1,4 @@
+{
+ "private": true,
+ "dependencies": {}
+}
diff --git a/scouting/www/rpc/package.json b/scouting/www/rpc/package.json
new file mode 100644
index 0000000..6d1369a
--- /dev/null
+++ b/scouting/www/rpc/package.json
@@ -0,0 +1,4 @@
+{
+ "name": "@org_frc971/scouting/www/rpc",
+ "private": true
+}
diff --git a/scouting/www/shift_schedule/package.json b/scouting/www/shift_schedule/package.json
new file mode 100644
index 0000000..1d71623
--- /dev/null
+++ b/scouting/www/shift_schedule/package.json
@@ -0,0 +1,7 @@
+{
+ "name": "@org_frc971/scouting/www/shift_schedule",
+ "private": true,
+ "dependencies": {
+ "@angular/forms": "15.0.1"
+ }
+}
diff --git a/tools/build_rules/js.bzl b/tools/build_rules/js.bzl
index 5510266..d334764 100644
--- a/tools/build_rules/js.bzl
+++ b/tools/build_rules/js.bzl
@@ -3,7 +3,15 @@
load("@npm//@bazel/terser:index.bzl", "terser_minified")
load("@bazel_skylib//lib:paths.bzl", "paths")
load("@npm//@bazel/protractor:index.bzl", "protractor_web_test_suite")
-load("@npm//@bazel/typescript:index.bzl", "ts_project")
+load("@npm//@bazel/typescript:index.bzl", upstream_ts_library = "ts_library", upstream_ts_project = "ts_project")
+
+def ts_project(**kwargs):
+ """A trivial wrapper to prepare for the rules_js migration.
+
+ The intent is to change his macro to wrap the new rules_js ts_project
+ implementation.
+ """
+ upstream_ts_library(**kwargs)
def rollup_bundle(name, deps, visibility = None, **kwargs):
"""Calls the upstream rollup_bundle() and exposes a .min.js file.
@@ -72,7 +80,7 @@
See the documentation for more information:
https://bazelbuild.github.io/rules_nodejs/Protractor.html#protractor_web_test_suite
"""
- ts_project(
+ upstream_ts_project(
name = name + "__lib",
srcs = srcs,
testonly = 1,
diff --git a/tsconfig.json b/tsconfig.json
index aed3439..16fe795 100644
--- a/tsconfig.json
+++ b/tsconfig.json
@@ -3,8 +3,8 @@
"experimentalDecorators": true,
"strict": false,
"noImplicitAny": false,
- "target": "es6",
- "lib": ["es6", "dom", "dom.iterable"],
+ "target": "es2021",
+ "lib": ["es2021", "dom", "dom.iterable"],
"moduleResolution": "node"
},
"bazelOptions": {
diff --git a/y2018/BUILD b/y2018/BUILD
index ac59493..8feaac4 100644
--- a/y2018/BUILD
+++ b/y2018/BUILD
@@ -57,9 +57,10 @@
"//aos/network:team_number",
"//aos/stl_mutex",
"//frc971:constants",
+ "//frc971/control_loops/double_jointed_arm:dynamics",
"//frc971/shooter_interpolation:interpolation",
"//y2018/control_loops/drivetrain:polydrivetrain_plants",
- "//y2018/control_loops/superstructure/arm:dynamics",
+ "//y2018/control_loops/superstructure/arm:arm_constants",
"//y2018/control_loops/superstructure/intake:intake_plants",
"@com_github_google_glog//:glog",
],
diff --git a/y2018/constants.h b/y2018/constants.h
index 16f1f8c..2a6f0ac 100644
--- a/y2018/constants.h
+++ b/y2018/constants.h
@@ -6,7 +6,8 @@
#include "frc971/constants.h"
#include "y2018/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "y2018/control_loops/superstructure/arm/dynamics.h"
+#include "frc971/control_loops/double_jointed_arm/dynamics.h"
+#include "y2018/control_loops/superstructure/arm/arm_constants.h"
#include "y2018/control_loops/superstructure/intake/intake_plant.h"
namespace y2018 {
@@ -49,8 +50,8 @@
return (12.0 / 60.0) * (18.0 / 84.0);
}
static constexpr double kMaxProximalEncoderPulsesPerSecond() {
- return control_loops::superstructure::arm::Dynamics::kFreeSpeed /
- (2.0 * M_PI) / control_loops::superstructure::arm::Dynamics::kG1 /
+ return control_loops::superstructure::arm::kArmConstants.free_speed /
+ (2.0 * M_PI) / control_loops::superstructure::arm::kArmConstants.g1 /
kProximalEncoderRatio() * kProximalEncoderCountsPerRevolution();
}
static constexpr double kProximalPotRatio() { return (12.0 / 60.0); }
@@ -58,8 +59,8 @@
static constexpr double kDistalEncoderCountsPerRevolution() { return 4096.0; }
static constexpr double kDistalEncoderRatio() { return (12.0 / 60.0); }
static constexpr double kMaxDistalEncoderPulsesPerSecond() {
- return control_loops::superstructure::arm::Dynamics::kFreeSpeed /
- (2.0 * M_PI) / control_loops::superstructure::arm::Dynamics::kG2 /
+ return control_loops::superstructure::arm::kArmConstants.free_speed /
+ (2.0 * M_PI) / control_loops::superstructure::arm::kArmConstants.g2 /
kDistalEncoderRatio() * kProximalEncoderCountsPerRevolution();
}
static constexpr double kDistalPotRatio() {
diff --git a/y2018/control_loops/python/graph_codegen.py b/y2018/control_loops/python/graph_codegen.py
index e7f8ce1..be267de 100644
--- a/y2018/control_loops/python/graph_codegen.py
+++ b/y2018/control_loops/python/graph_codegen.py
@@ -27,12 +27,12 @@
cc_file.append(" %s," % (alpha_unitizer))
if reverse:
cc_file.append(
- " Trajectory(Path::Reversed(%s()), 0.005));"
+ " Trajectory(dynamics, Path::Reversed(%s()), 0.005));"
% (path_function_name(str(name))))
else:
cc_file.append(
- " Trajectory(%s(), 0.005));" %
- (path_function_name(str(name))))
+ " Trajectory(dynamics, %s(), 0.005));"
+ % (path_function_name(str(name))))
start_index = None
end_index = None
@@ -68,9 +68,14 @@
cc_file.append("#include <memory>")
cc_file.append("")
cc_file.append(
- "#include \"y2018/control_loops/superstructure/arm/trajectory.h\"")
+ "#include \"frc971/control_loops/double_jointed_arm/trajectory.h\"")
cc_file.append(
- "#include \"y2018/control_loops/superstructure/arm/graph.h\"")
+ "#include \"frc971/control_loops/double_jointed_arm/graph.h\"")
+
+ cc_file.append("using frc971::control_loops::arm::Trajectory;")
+ cc_file.append("using frc971::control_loops::arm::Path;")
+ cc_file.append("using frc971::control_loops::arm::SearchGraph;")
+
cc_file.append("")
cc_file.append("namespace y2018 {")
cc_file.append("namespace control_loops {")
@@ -86,15 +91,21 @@
h_file.append("#include <memory>")
h_file.append("")
h_file.append(
- "#include \"y2018/control_loops/superstructure/arm/trajectory.h\"")
+ "#include \"frc971/control_loops/double_jointed_arm/trajectory.h\"")
h_file.append(
- "#include \"y2018/control_loops/superstructure/arm/graph.h\"")
+ "#include \"frc971/control_loops/double_jointed_arm/graph.h\"")
+ h_file.append(
+ "#include \"frc971/control_loops/double_jointed_arm/dynamics.h\"")
h_file.append("")
h_file.append("namespace y2018 {")
h_file.append("namespace control_loops {")
h_file.append("namespace superstructure {")
h_file.append("namespace arm {")
+ h_file.append("using frc971::control_loops::arm::Trajectory;")
+ h_file.append("using frc971::control_loops::arm::Path;")
+ h_file.append("using frc971::control_loops::arm::SearchGraph;")
+
h_file.append("")
h_file.append("struct TrajectoryAndParams {")
h_file.append(" TrajectoryAndParams(double new_vmax,")
@@ -182,11 +193,13 @@
h_file.append("")
h_file.append("// Builds a search graph.")
h_file.append("SearchGraph MakeSearchGraph("
+ "const frc971::control_loops::arm::Dynamics *dynamics, "
"::std::vector<TrajectoryAndParams> *trajectories,")
h_file.append(" "
"const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer,")
h_file.append(" double vmax);")
cc_file.append("SearchGraph MakeSearchGraph("
+ "const frc971::control_loops::arm::Dynamics *dynamics, "
"::std::vector<TrajectoryAndParams> *trajectories,")
cc_file.append(" "
"const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer,")
diff --git a/y2018/control_loops/superstructure/BUILD b/y2018/control_loops/superstructure/BUILD
index 567810d..a3cb808 100644
--- a/y2018/control_loops/superstructure/BUILD
+++ b/y2018/control_loops/superstructure/BUILD
@@ -94,6 +94,7 @@
"//frc971/control_loops:control_loop_test",
"//frc971/control_loops:position_sensor_sim",
"//frc971/control_loops:team_number_test_environment",
+ "//y2018/control_loops/superstructure/arm:arm_constants",
"//y2018/control_loops/superstructure/intake:intake_plants",
],
)
diff --git a/y2018/control_loops/superstructure/arm/BUILD b/y2018/control_loops/superstructure/arm/BUILD
index f90cc92..9f969ff 100644
--- a/y2018/control_loops/superstructure/arm/BUILD
+++ b/y2018/control_loops/superstructure/arm/BUILD
@@ -1,128 +1,4 @@
cc_library(
- name = "trajectory",
- srcs = [
- "trajectory.cc",
- ],
- hdrs = [
- "trajectory.h",
- ],
- target_compatible_with = ["@platforms//os:linux"],
- visibility = ["//visibility:public"],
- deps = [
- ":dynamics",
- "//aos/logging",
- "//frc971/control_loops:dlqr",
- "//frc971/control_loops:jacobian",
- "@org_tuxfamily_eigen//:eigen",
- ],
-)
-
-cc_test(
- name = "trajectory_test",
- srcs = [
- "trajectory_test.cc",
- ],
- target_compatible_with = ["@platforms//os:linux"],
- deps = [
- ":demo_path",
- ":dynamics",
- ":ekf",
- ":trajectory",
- "//aos/testing:googletest",
- "@org_tuxfamily_eigen//:eigen",
- ],
-)
-
-cc_library(
- name = "dynamics",
- srcs = [
- "dynamics.cc",
- ],
- hdrs = [
- "dynamics.h",
- ],
- target_compatible_with = ["@platforms//os:linux"],
- visibility = ["//visibility:public"],
- deps = [
- "//frc971/control_loops:runge_kutta",
- "@com_github_gflags_gflags//:gflags",
- "@org_tuxfamily_eigen//:eigen",
- ],
-)
-
-cc_library(
- name = "demo_path",
- srcs = [
- "demo_path.cc",
- ],
- hdrs = ["demo_path.h"],
- target_compatible_with = ["@platforms//os:linux"],
- deps = [":trajectory"],
-)
-
-cc_test(
- name = "dynamics_test",
- srcs = [
- "dynamics_test.cc",
- ],
- target_compatible_with = ["@platforms//os:linux"],
- deps = [
- ":dynamics",
- "//aos/testing:googletest",
- ],
-)
-
-cc_binary(
- name = "trajectory_plot",
- srcs = [
- "trajectory_plot.cc",
- ],
- target_compatible_with = ["@platforms//cpu:x86_64"],
- deps = [
- ":ekf",
- ":generated_graph",
- ":trajectory",
- "//frc971/analysis:in_process_plotter",
- "@com_github_gflags_gflags//:gflags",
- "@org_tuxfamily_eigen//:eigen",
- ],
-)
-
-cc_library(
- name = "ekf",
- srcs = [
- "ekf.cc",
- ],
- hdrs = [
- "ekf.h",
- ],
- target_compatible_with = ["@platforms//os:linux"],
- visibility = ["//visibility:public"],
- deps = [
- ":dynamics",
- "//frc971/control_loops:jacobian",
- "@org_tuxfamily_eigen//:eigen",
- ],
-)
-
-cc_library(
- name = "graph",
- srcs = ["graph.cc"],
- hdrs = ["graph.h"],
- target_compatible_with = ["@platforms//os:linux"],
-)
-
-cc_test(
- name = "graph_test",
- srcs = ["graph_test.cc"],
- target_compatible_with = ["@platforms//os:linux"],
- deps = [
- ":graph",
- "//aos/testing:googletest",
- ],
-)
-
-cc_library(
name = "arm",
srcs = [
"arm.cc",
@@ -133,15 +9,16 @@
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
deps = [
- ":demo_path",
- ":ekf",
":generated_graph",
- ":graph",
- ":trajectory",
+ "//frc971/control_loops/double_jointed_arm:demo_path",
+ "//frc971/control_loops/double_jointed_arm:ekf",
+ "//frc971/control_loops/double_jointed_arm:graph",
+ "//frc971/control_loops/double_jointed_arm:trajectory",
"//frc971/zeroing",
"//y2018:constants",
"//y2018/control_loops/superstructure:superstructure_position_fbs",
"//y2018/control_loops/superstructure:superstructure_status_fbs",
+ "//y2018/control_loops/superstructure/arm:arm_constants",
],
)
@@ -170,7 +47,35 @@
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
deps = [
- ":graph",
- ":trajectory",
+ ":arm_constants",
+ "//frc971/control_loops/double_jointed_arm:graph",
+ "//frc971/control_loops/double_jointed_arm:trajectory",
+ ],
+)
+
+cc_library(
+ name = "arm_constants",
+ hdrs = ["arm_constants.h"],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops/double_jointed_arm:dynamics",
+ ],
+)
+
+cc_binary(
+ name = "trajectory_plot",
+ srcs = [
+ "trajectory_plot.cc",
+ ],
+ target_compatible_with = ["@platforms//cpu:x86_64"],
+ deps = [
+ ":arm_constants",
+ ":generated_graph",
+ "//frc971/analysis:in_process_plotter",
+ "//frc971/control_loops/double_jointed_arm:ekf",
+ "//frc971/control_loops/double_jointed_arm:trajectory",
+ "@com_github_gflags_gflags//:gflags",
+ "@org_tuxfamily_eigen//:eigen",
],
)
diff --git a/y2018/control_loops/superstructure/arm/arm.cc b/y2018/control_loops/superstructure/arm/arm.cc
index 1c29157..ab5deea 100644
--- a/y2018/control_loops/superstructure/arm/arm.cc
+++ b/y2018/control_loops/superstructure/arm/arm.cc
@@ -5,8 +5,9 @@
#include "aos/logging/logging.h"
#include "y2018/constants.h"
-#include "y2018/control_loops/superstructure/arm/demo_path.h"
-#include "y2018/control_loops/superstructure/arm/dynamics.h"
+#include "frc971/control_loops/double_jointed_arm/demo_path.h"
+#include "frc971/control_loops/double_jointed_arm/dynamics.h"
+#include "y2018/control_loops/superstructure/arm/arm_constants.h"
#include "y2018/control_loops/superstructure/arm/generated_graph.h"
namespace y2018 {
@@ -29,9 +30,12 @@
alpha_unitizer_((::Eigen::Matrix<double, 2, 2>() << 1.0 / kAlpha0Max(),
0.0, 0.0, 1.0 / kAlpha1Max())
.finished()),
- search_graph_(MakeSearchGraph(&trajectories_, alpha_unitizer_, kVMax())),
+ dynamics_(kArmConstants),
+ search_graph_(MakeSearchGraph(&dynamics_, &trajectories_, alpha_unitizer_,
+ kVMax())),
+ arm_ekf_(&dynamics_),
// Go to the start of the first trajectory.
- follower_(ReadyAboveBoxPoint()),
+ follower_(&dynamics_, ReadyAboveBoxPoint()),
points_(PointList()) {
int i = 0;
for (const auto &trajectory : trajectories_) {
diff --git a/y2018/control_loops/superstructure/arm/arm.h b/y2018/control_loops/superstructure/arm/arm.h
index 7ceb001..51d6c4d 100644
--- a/y2018/control_loops/superstructure/arm/arm.h
+++ b/y2018/control_loops/superstructure/arm/arm.h
@@ -4,14 +4,17 @@
#include "aos/time/time.h"
#include "frc971/zeroing/zeroing.h"
#include "y2018/constants.h"
-#include "y2018/control_loops/superstructure/arm/dynamics.h"
-#include "y2018/control_loops/superstructure/arm/ekf.h"
+#include "frc971/control_loops/double_jointed_arm/dynamics.h"
+#include "frc971/control_loops/double_jointed_arm/ekf.h"
#include "y2018/control_loops/superstructure/arm/generated_graph.h"
-#include "y2018/control_loops/superstructure/arm/graph.h"
-#include "y2018/control_loops/superstructure/arm/trajectory.h"
+#include "frc971/control_loops/double_jointed_arm/graph.h"
+#include "frc971/control_loops/double_jointed_arm/trajectory.h"
#include "y2018/control_loops/superstructure/superstructure_position_generated.h"
#include "y2018/control_loops/superstructure/superstructure_status_generated.h"
+using frc971::control_loops::arm::TrajectoryFollower;
+using frc971::control_loops::arm::EKF;
+
namespace y2018 {
namespace control_loops {
namespace superstructure {
@@ -114,6 +117,8 @@
double vmax_ = kVMax();
+ frc971::control_loops::arm::Dynamics dynamics_;
+
::std::vector<TrajectoryAndParams> trajectories_;
SearchGraph search_graph_;
diff --git a/y2018/control_loops/superstructure/arm/arm_constants.h b/y2018/control_loops/superstructure/arm/arm_constants.h
new file mode 100644
index 0000000..9ac7020
--- /dev/null
+++ b/y2018/control_loops/superstructure/arm/arm_constants.h
@@ -0,0 +1,53 @@
+#ifndef Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_ARM_CONSTANTS_H_
+#define Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_ARM_CONSTANTS_H_
+
+#include "frc971/control_loops/double_jointed_arm/dynamics.h"
+
+namespace y2018 {
+namespace control_loops {
+namespace superstructure {
+namespace arm {
+
+constexpr double kEfficiencyTweak = 0.95;
+constexpr double kStallTorque = 1.41 * kEfficiencyTweak;
+constexpr double kFreeSpeed = (5840.0 / 60.0) * 2.0 * M_PI;
+constexpr double kStallCurrent = 89.0;
+
+constexpr ::frc971::control_loops::arm::ArmConstants kArmConstants = {
+ .l1 = 46.25 * 0.0254,
+ .l2 = 41.80 * 0.0254,
+ .m1 = 9.34 / 2.2,
+ .m2 = 9.77 / 2.2,
+
+ // Moment of inertia of the joints in kg m^2
+ .j1 = 2957.05 * 0.0002932545454545454,
+ .j2 = 2824.70 * 0.0002932545454545454,
+
+ // Radius of the center of mass of the joints in meters.
+ .r1 = 21.64 * 0.0254,
+ .r2 = 26.70 * 0.0254,
+
+ // Gear ratios for the two joints.
+ .g1 = 140.0,
+ .g2 = 90.0,
+
+ // MiniCIM motor constants.
+ .efficiency_tweak = kEfficiencyTweak,
+ .stall_torque = kStallTorque,
+ .free_speed = kFreeSpeed,
+ .stall_current = kStallCurrent,
+ .resistance = 12.0 / kStallCurrent,
+ .Kv = kFreeSpeed / 12.0,
+ .Kt = kStallTorque / kStallCurrent,
+
+ // Number of motors on the distal joint.
+ .num_distal_motors = 2.0,
+};
+
+
+} // namespace arm
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2018
+
+#endif // Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_ARM_CONSTANTS_H_
diff --git a/y2018/control_loops/superstructure/arm/demo_path.h b/y2018/control_loops/superstructure/arm/demo_path.h
deleted file mode 100644
index 0c18103..0000000
--- a/y2018/control_loops/superstructure/arm/demo_path.h
+++ /dev/null
@@ -1,21 +0,0 @@
-#ifndef Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_DEMO_PATH_H_
-#define Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_DEMO_PATH_H_
-
-#include <memory>
-
-#include "y2018/control_loops/superstructure/arm/trajectory.h"
-
-namespace y2018 {
-namespace control_loops {
-namespace superstructure {
-namespace arm {
-
-::std::unique_ptr<Path> MakeDemoPath();
-::std::unique_ptr<Path> MakeReversedDemoPath();
-
-} // namespace arm
-} // namespace superstructure
-} // namespace control_loops
-} // namespace y2018
-
-#endif // Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_DEMO_PATH_H_
diff --git a/y2018/control_loops/superstructure/arm/dynamics.cc b/y2018/control_loops/superstructure/arm/dynamics.cc
deleted file mode 100644
index 02a2861..0000000
--- a/y2018/control_loops/superstructure/arm/dynamics.cc
+++ /dev/null
@@ -1,35 +0,0 @@
-#include "y2018/control_loops/superstructure/arm/dynamics.h"
-
-DEFINE_bool(gravity, true, "If true, enable gravity.");
-
-namespace y2018 {
-namespace control_loops {
-namespace superstructure {
-namespace arm {
-
-const ::Eigen::Matrix<double, 2, 2> Dynamics::K3 =
- (::Eigen::Matrix<double, 2, 2>()
- << Dynamics::kG1 * Dynamics::Kt / Dynamics::kResistance,
- 0.0, 0.0, Dynamics::kG2 *Dynamics::kNumDistalMotors *Dynamics::Kt /
- Dynamics::kResistance)
- .finished();
-
-const ::Eigen::Matrix<double, 2, 2> Dynamics::K3_inverse = K3.inverse();
-
-const ::Eigen::Matrix<double, 2, 2> Dynamics::K4 =
- (::Eigen::Matrix<double, 2, 2>()
- << Dynamics::kG1 * Dynamics::kG1 * Dynamics::Kt /
- (Dynamics::Kv * Dynamics::kResistance),
- 0.0, 0.0,
- Dynamics::kG2 *Dynamics::kG2 *Dynamics::Kt *Dynamics::kNumDistalMotors /
- (Dynamics::Kv * Dynamics::kResistance))
- .finished();
-
-constexpr double Dynamics::kAlpha;
-constexpr double Dynamics::kBeta;
-constexpr double Dynamics::kGamma;
-
-} // namespace arm
-} // namespace superstructure
-} // namespace control_loops
-} // namespace y2018
diff --git a/y2018/control_loops/superstructure/arm/dynamics.h b/y2018/control_loops/superstructure/arm/dynamics.h
deleted file mode 100644
index b1ebc97..0000000
--- a/y2018/control_loops/superstructure/arm/dynamics.h
+++ /dev/null
@@ -1,189 +0,0 @@
-#ifndef Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_DYNAMICS_H_
-#define Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_DYNAMICS_H_
-
-#include "Eigen/Dense"
-
-#include "frc971/control_loops/runge_kutta.h"
-#include "gflags/gflags.h"
-
-DECLARE_bool(gravity);
-
-namespace y2018 {
-namespace control_loops {
-namespace superstructure {
-namespace arm {
-
-// This class captures the dynamics of our system. It doesn't actually need to
-// store state yet, so everything can be constexpr and/or static.
-class Dynamics {
- public:
- // Below, 1 refers to the proximal joint, and 2 refers to the distal joint.
- // Length of the joints in meters.
- static constexpr double kL1 = 46.25 * 0.0254;
- static constexpr double kL2 = 41.80 * 0.0254;
-
- // Mass of the joints in kilograms.
- static constexpr double kM1 = 9.34 / 2.2;
- static constexpr double kM2 = 9.77 / 2.2;
-
- // Moment of inertia of the joints in kg m^2
- static constexpr double kJ1 = 2957.05 * 0.0002932545454545454;
- static constexpr double kJ2 = 2824.70 * 0.0002932545454545454;
-
- // Radius of the center of mass of the joints in meters.
- static constexpr double r1 = 21.64 * 0.0254;
- static constexpr double r2 = 26.70 * 0.0254;
-
- // Gear ratios for the two joints.
- static constexpr double kG1 = 140.0;
- static constexpr double kG2 = 90.0;
-
- // MiniCIM motor constants.
- static constexpr double kEfficiencyTweak = 0.95;
- static constexpr double kStallTorque = 1.41 * kEfficiencyTweak;
- static constexpr double kFreeSpeed = (5840.0 / 60.0) * 2.0 * M_PI;
- static constexpr double kStallCurrent = 89.0;
- static constexpr double kResistance = 12.0 / kStallCurrent;
- static constexpr double Kv = kFreeSpeed / 12.0;
- static constexpr double Kt = kStallTorque / kStallCurrent;
-
- // Number of motors on the distal joint.
- static constexpr double kNumDistalMotors = 2.0;
-
- static constexpr double kAlpha = kJ1 + r1 * r1 * kM1 + kL1 * kL1 * kM2;
- static constexpr double kBeta = kL1 * r2 * kM2;
- static constexpr double kGamma = kJ2 + r2 * r2 * kM2;
-
- // K3, K4 matricies described below.
- static const ::Eigen::Matrix<double, 2, 2> K3;
- static const ::Eigen::Matrix<double, 2, 2> K3_inverse;
- static const ::Eigen::Matrix<double, 2, 2> K4;
-
- // Generates K1-2 for the arm ODE.
- // K1 * d^2 theta / dt^2 + K2 * d theta / dt = K3 * V - K4 * d theta/dt
- // These matricies are missing the velocity factor for K2[1, 0], and K2[0, 1].
- // You probbaly want MatriciesForState.
- static void NormilizedMatriciesForState(
- const ::Eigen::Matrix<double, 4, 1> &X,
- ::Eigen::Matrix<double, 2, 2> *K1_result,
- ::Eigen::Matrix<double, 2, 2> *K2_result) {
- const double angle = X(0, 0) - X(2, 0);
- const double s = ::std::sin(angle);
- const double c = ::std::cos(angle);
- *K1_result << kAlpha, c * kBeta, c * kBeta, kGamma;
- *K2_result << 0.0, s * kBeta, -s * kBeta, 0.0;
- }
-
- // Generates K1-2 for the arm ODE.
- // K1 * d^2 theta / dt^2 + K2 * d theta / dt = K3 * V - K4 * d theta/dt
- static void MatriciesForState(const ::Eigen::Matrix<double, 4, 1> &X,
- ::Eigen::Matrix<double, 2, 2> *K1_result,
- ::Eigen::Matrix<double, 2, 2> *K2_result) {
- NormilizedMatriciesForState(X, K1_result, K2_result);
- (*K2_result)(1, 0) *= X(1, 0);
- (*K2_result)(0, 1) *= X(3, 0);
- }
-
- // TODO(austin): We may want a way to provide K1 and K2 to save CPU cycles.
-
- // Calculates the acceleration given the current state and control input.
- static const ::Eigen::Matrix<double, 4, 1> Acceleration(
- const ::Eigen::Matrix<double, 4, 1> &X,
- const ::Eigen::Matrix<double, 2, 1> &U) {
- ::Eigen::Matrix<double, 2, 2> K1;
- ::Eigen::Matrix<double, 2, 2> K2;
-
- MatriciesForState(X, &K1, &K2);
-
- const ::Eigen::Matrix<double, 2, 1> velocity =
- (::Eigen::Matrix<double, 2, 1>() << X(1, 0), X(3, 0)).finished();
-
- const ::Eigen::Matrix<double, 2, 1> torque = K3 * U - K4 * velocity;
- const ::Eigen::Matrix<double, 2, 1> gravity_torque = GravityTorque(X);
-
- const ::Eigen::Matrix<double, 2, 1> accel =
- K1.inverse() * (torque + gravity_torque - K2 * velocity);
-
- return (::Eigen::Matrix<double, 4, 1>() << X(1, 0), accel(0, 0), X(3, 0),
- accel(1, 0))
- .finished();
- }
-
- // Calculates the acceleration given the current augmented kalman filter state
- // and control input.
- static const ::Eigen::Matrix<double, 6, 1> EKFAcceleration(
- const ::Eigen::Matrix<double, 6, 1> &X,
- const ::Eigen::Matrix<double, 2, 1> &U) {
- ::Eigen::Matrix<double, 2, 2> K1;
- ::Eigen::Matrix<double, 2, 2> K2;
-
- MatriciesForState(X.block<4, 1>(0, 0), &K1, &K2);
-
- const ::Eigen::Matrix<double, 2, 1> velocity =
- (::Eigen::Matrix<double, 2, 1>() << X(1, 0), X(3, 0)).finished();
-
- const ::Eigen::Matrix<double, 2, 1> torque =
- K3 *
- (U +
- (::Eigen::Matrix<double, 2, 1>() << X(4, 0), X(5, 0)).finished()) -
- K4 * velocity;
- const ::Eigen::Matrix<double, 2, 1> gravity_torque =
- GravityTorque(X.block<4, 1>(0, 0));
-
- const ::Eigen::Matrix<double, 2, 1> accel =
- K1.inverse() * (torque + gravity_torque - K2 * velocity);
-
- return (::Eigen::Matrix<double, 6, 1>() << X(1, 0), accel(0, 0), X(3, 0),
- accel(1, 0), 0.0, 0.0)
- .finished();
- }
-
- // Calculates the voltage required to follow the trajectory. This requires
- // knowing the current state, desired angular velocity and acceleration.
- static const ::Eigen::Matrix<double, 2, 1> FF_U(
- const ::Eigen::Matrix<double, 4, 1> &X,
- const ::Eigen::Matrix<double, 2, 1> &omega_t,
- const ::Eigen::Matrix<double, 2, 1> &alpha_t) {
- ::Eigen::Matrix<double, 2, 2> K1;
- ::Eigen::Matrix<double, 2, 2> K2;
-
- MatriciesForState(X, &K1, &K2);
-
- const ::Eigen::Matrix<double, 2, 1> gravity_torque = GravityTorque(X);
-
- return K3_inverse *
- (K1 * alpha_t + K2 * omega_t + K4 * omega_t - gravity_torque);
- }
-
- static ::Eigen::Matrix<double, 2, 1> GravityTorque(
- const ::Eigen::Matrix<double, 4, 1> &X) {
- constexpr double kAccelDueToGravity = 9.8 * kEfficiencyTweak;
- return (::Eigen::Matrix<double, 2, 1>() << (r1 * kM1 + kL1 * kM2) *
- ::std::sin(X(0)) *
- kAccelDueToGravity,
- r2 * kM2 * ::std::sin(X(2)) * kAccelDueToGravity)
- .finished() *
- (FLAGS_gravity ? 1.0 : 0.0);
- }
-
- static const ::Eigen::Matrix<double, 4, 1> UnboundedDiscreteDynamics(
- const ::Eigen::Matrix<double, 4, 1> &X,
- const ::Eigen::Matrix<double, 2, 1> &U, double dt) {
- return ::frc971::control_loops::RungeKuttaU(Dynamics::Acceleration, X, U,
- dt);
- }
-
- static const ::Eigen::Matrix<double, 6, 1> UnboundedEKFDiscreteDynamics(
- const ::Eigen::Matrix<double, 6, 1> &X,
- const ::Eigen::Matrix<double, 2, 1> &U, double dt) {
- return ::frc971::control_loops::RungeKuttaU(Dynamics::EKFAcceleration, X, U,
- dt);
- }
-};
-
-} // namespace arm
-} // namespace superstructure
-} // namespace control_loops
-} // namespace y2018
-
-#endif // Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_DYNAMICS_H_
diff --git a/y2018/control_loops/superstructure/arm/dynamics_test.cc b/y2018/control_loops/superstructure/arm/dynamics_test.cc
deleted file mode 100644
index 23e5adc..0000000
--- a/y2018/control_loops/superstructure/arm/dynamics_test.cc
+++ /dev/null
@@ -1,52 +0,0 @@
-#include "y2018/control_loops/superstructure/arm/dynamics.h"
-
-#include "gtest/gtest.h"
-
-namespace y2018 {
-namespace control_loops {
-namespace superstructure {
-namespace arm {
-namespace testing {
-
-// Tests that zero inputs result in no acceleration and no motion.
-// This isn't all that rigerous, but it's a good start.
-TEST(DynamicsTest, Acceleration) {
- Dynamics dynamics;
-
- EXPECT_TRUE(dynamics
- .Acceleration(::Eigen::Matrix<double, 4, 1>::Zero(),
- ::Eigen::Matrix<double, 2, 1>::Zero())
- .isApprox(::Eigen::Matrix<double, 4, 1>::Zero()));
-
- EXPECT_TRUE(dynamics
- .UnboundedDiscreteDynamics(
- ::Eigen::Matrix<double, 4, 1>::Zero(),
- ::Eigen::Matrix<double, 2, 1>::Zero(), 0.1)
- .isApprox(::Eigen::Matrix<double, 4, 1>::Zero()));
-
- const ::Eigen::Matrix<double, 4, 1> X =
- (::Eigen::Matrix<double, 4, 1>() << M_PI / 2.0, 0.0, 0.0, 0.0).finished();
-
- ::std::cout << dynamics.FF_U(X, ::Eigen::Matrix<double, 2, 1>::Zero(),
- ::Eigen::Matrix<double, 2, 1>::Zero())
- << ::std::endl;
-
- ::std::cout << dynamics.UnboundedDiscreteDynamics(
- X, dynamics.FF_U(X, ::Eigen::Matrix<double, 2, 1>::Zero(),
- ::Eigen::Matrix<double, 2, 1>::Zero()),
- 0.01)
- << ::std::endl;
-
- EXPECT_TRUE(dynamics
- .UnboundedDiscreteDynamics(
- X, dynamics.FF_U(X, ::Eigen::Matrix<double, 2, 1>::Zero(),
- ::Eigen::Matrix<double, 2, 1>::Zero()),
- 0.01)
- .isApprox(X));
-}
-
-} // namespace testing
-} // namespace arm
-} // namespace superstructure
-} // namespace control_loops
-} // namespace y2018
diff --git a/y2018/control_loops/superstructure/arm/trajectory_plot.cc b/y2018/control_loops/superstructure/arm/trajectory_plot.cc
index 10b39bc..0cee664 100644
--- a/y2018/control_loops/superstructure/arm/trajectory_plot.cc
+++ b/y2018/control_loops/superstructure/arm/trajectory_plot.cc
@@ -1,10 +1,11 @@
#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"
#include "gflags/gflags.h"
-#include "y2018/control_loops/superstructure/arm/dynamics.h"
-#include "y2018/control_loops/superstructure/arm/ekf.h"
+#include "y2018/control_loops/superstructure/arm/arm_constants.h"
#include "y2018/control_loops/superstructure/arm/generated_graph.h"
-#include "y2018/control_loops/superstructure/arm/trajectory.h"
DEFINE_bool(forwards, true, "If true, run the forwards simulation.");
DEFINE_bool(plot, true, "If true, plot");
@@ -16,10 +17,12 @@
namespace arm {
void Main() {
- Trajectory trajectory(FLAGS_forwards
- ? MakeNeutralToFrontHighPath()
- : Path::Reversed(MakeNeutralToFrontHighPath()),
- 0.001);
+ frc971::control_loops::arm::Dynamics dynamics(kArmConstants);
+ frc971::control_loops::arm::Trajectory trajectory(
+ &dynamics,
+ FLAGS_forwards ? MakeNeutralToFrontHighPath()
+ : Path::Reversed(MakeNeutralToFrontHighPath()),
+ 0.001);
constexpr double kAlpha0Max = 30.0;
constexpr double kAlpha1Max = 50.0;
@@ -96,7 +99,7 @@
const ::Eigen::Matrix<double, 6, 1> R = trajectory.R(theta_t, omega_t);
const ::Eigen::Matrix<double, 2, 1> U =
- Dynamics::FF_U(R.block<4, 1>(0, 0), omega_t, alpha_t)
+ dynamics.FF_U(R.block<4, 1>(0, 0), omega_t, alpha_t)
.array()
.max(-20)
.min(20);
@@ -118,7 +121,7 @@
const ::Eigen::Matrix<double, 6, 1> R = trajectory.R(theta_t, omega_t);
const ::Eigen::Matrix<double, 2, 1> U =
- Dynamics::FF_U(R.block<4, 1>(0, 0), omega_t, alpha_t)
+ dynamics.FF_U(R.block<4, 1>(0, 0), omega_t, alpha_t)
.array()
.max(-20)
.min(20);
@@ -140,7 +143,7 @@
const ::Eigen::Matrix<double, 6, 1> R = trajectory.R(theta_t, omega_t);
const ::Eigen::Matrix<double, 2, 1> U =
- Dynamics::FF_U(R.block<4, 1>(0, 0), omega_t, alpha_t)
+ dynamics.FF_U(R.block<4, 1>(0, 0), omega_t, alpha_t)
.array()
.max(-20)
.min(20);
@@ -156,7 +159,8 @@
X << theta_t(0), 0.0, theta_t(1), 0.0;
}
- TrajectoryFollower follower(&trajectory);
+ frc971::control_loops::arm::TrajectoryFollower follower(&dynamics,
+ &trajectory);
::std::vector<double> t_array;
::std::vector<double> theta0_goal_t_array;
@@ -187,7 +191,7 @@
::std::vector<double> torque0_hat_t_array;
::std::vector<double> torque1_hat_t_array;
- EKF arm_ekf;
+ frc971::control_loops::arm::EKF arm_ekf(&dynamics);
arm_ekf.Reset(X);
::std::cout << "Reset P: " << arm_ekf.P_reset() << ::std::endl;
::std::cout << "Stabilized P: " << arm_ekf.P_half_converged() << ::std::endl;
@@ -236,9 +240,9 @@
actual_U(0) += 1.0;
const ::Eigen::Matrix<double, 4, 1> xdot =
- Dynamics::Acceleration(X, actual_U);
+ dynamics.Acceleration(X, actual_U);
- X = Dynamics::UnboundedDiscreteDynamics(X, actual_U, sim_dt);
+ X = dynamics.UnboundedDiscreteDynamics(X, actual_U, sim_dt);
arm_ekf.Predict(follower.U(), sim_dt);
alpha0_t_array.push_back(xdot(1));
@@ -270,10 +274,14 @@
plotter.AddLine(distance_array, alpha0_array, "alpha0");
plotter.AddLine(distance_array, alpha1_array, "alpha1");
- plotter.AddLine(integrated_distance, integrated_theta0_array, "integrated theta0");
- plotter.AddLine(integrated_distance, integrated_theta1_array, "integrated theta1");
- plotter.AddLine(integrated_distance, integrated_omega0_array, "integrated omega0");
- plotter.AddLine(integrated_distance, integrated_omega1_array, "integrated omega1");
+ plotter.AddLine(integrated_distance, integrated_theta0_array,
+ "integrated theta0");
+ plotter.AddLine(integrated_distance, integrated_theta1_array,
+ "integrated theta1");
+ plotter.AddLine(integrated_distance, integrated_omega0_array,
+ "integrated omega0");
+ plotter.AddLine(integrated_distance, integrated_omega1_array,
+ "integrated omega1");
plotter.Publish();
plotter.AddFigure();
diff --git a/y2018/control_loops/superstructure/superstructure_lib_test.cc b/y2018/control_loops/superstructure/superstructure_lib_test.cc
index 285c8c3..54342c5 100644
--- a/y2018/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2018/control_loops/superstructure/superstructure_lib_test.cc
@@ -8,7 +8,8 @@
#include "frc971/control_loops/team_number_test_environment.h"
#include "gtest/gtest.h"
#include "y2018/constants.h"
-#include "y2018/control_loops/superstructure/arm/dynamics.h"
+#include "frc971/control_loops/double_jointed_arm/dynamics.h"
+#include "y2018/control_loops/superstructure/arm/arm_constants.h"
#include "y2018/control_loops/superstructure/arm/generated_graph.h"
#include "y2018/control_loops/superstructure/intake/intake_plant.h"
#include "y2018/control_loops/superstructure/superstructure.h"
@@ -129,7 +130,8 @@
constants::Values::kProximalEncoderRatio()),
distal_zeroing_constants_(distal_zeroing_constants),
distal_pot_encoder_(M_PI * 2.0 *
- constants::Values::kDistalEncoderRatio()) {
+ constants::Values::kDistalEncoderRatio()),
+ dynamics_(arm::kArmConstants) {
X_.setZero();
}
@@ -174,7 +176,7 @@
AOS_CHECK_LE(::std::abs(U(1)), voltage_check);
if (release_arm_brake) {
- X_ = arm::Dynamics::UnboundedDiscreteDynamics(X_, U, 0.00505);
+ X_ = dynamics_.UnboundedDiscreteDynamics(X_, U, 0.00505);
} else {
// Well, the brake shouldn't stop both joints, but this will get the tests
// to pass.
@@ -197,6 +199,8 @@
const ::frc971::constants::PotAndAbsoluteEncoderZeroingConstants
distal_zeroing_constants_;
PositionSensorSimulator distal_pot_encoder_;
+
+ ::frc971::control_loops::arm::Dynamics dynamics_;
};
class SuperstructureSimulation {
diff --git a/y2019/vision/server/BUILD b/y2019/vision/server/BUILD
index 93172b4..b0b7674 100644
--- a/y2019/vision/server/BUILD
+++ b/y2019/vision/server/BUILD
@@ -1,9 +1,9 @@
+load("//tools/build_rules:js.bzl", "ts_project")
load("//aos/seasocks:gen_embedded.bzl", "gen_embedded")
load("@com_google_protobuf//:protobuf.bzl", "cc_proto_library")
load("//frc971/downloader:downloader.bzl", "aos_downloader_dir")
-load("@npm//@bazel/typescript:index.bzl", "ts_library")
-ts_library(
+ts_project(
name = "demo",
srcs = [
"demo.ts",
diff --git a/y2019/vision/server/www/BUILD b/y2019/vision/server/www/BUILD
index 7c40eaa..2e8be80 100644
--- a/y2019/vision/server/www/BUILD
+++ b/y2019/vision/server/www/BUILD
@@ -1,5 +1,4 @@
-load("@npm//@bazel/typescript:index.bzl", "ts_library")
-load("//tools/build_rules:js.bzl", "rollup_bundle")
+load("//tools/build_rules:js.bzl", "rollup_bundle", "ts_project")
package(default_visibility = ["//visibility:public"])
@@ -10,7 +9,7 @@
]),
)
-ts_library(
+ts_project(
name = "visualizer",
srcs = glob([
"*.ts",
diff --git a/y2020/control_loops/drivetrain/BUILD b/y2020/control_loops/drivetrain/BUILD
index 0455d0f..fa153f0 100644
--- a/y2020/control_loops/drivetrain/BUILD
+++ b/y2020/control_loops/drivetrain/BUILD
@@ -1,7 +1,7 @@
+load("//tools/build_rules:js.bzl", "ts_project")
load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
load("//aos:config.bzl", "aos_config")
-load("@npm//@bazel/typescript:index.bzl", "ts_library")
genrule(
name = "genrule_drivetrain",
@@ -216,7 +216,7 @@
],
)
-ts_library(
+ts_project(
name = "localizer_plotter",
srcs = ["localizer_plotter.ts"],
target_compatible_with = ["@platforms//os:linux"],
diff --git a/y2020/control_loops/superstructure/BUILD b/y2020/control_loops/superstructure/BUILD
index d3f303f..d1f20a2 100644
--- a/y2020/control_loops/superstructure/BUILD
+++ b/y2020/control_loops/superstructure/BUILD
@@ -1,6 +1,6 @@
+load("//tools/build_rules:js.bzl", "ts_project")
load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
-load("@npm//@bazel/typescript:index.bzl", "ts_library")
package(default_visibility = ["//visibility:public"])
@@ -136,7 +136,7 @@
],
)
-ts_library(
+ts_project(
name = "turret_plotter",
srcs = ["turret_plotter.ts"],
target_compatible_with = ["@platforms//os:linux"],
@@ -152,7 +152,7 @@
],
)
-ts_library(
+ts_project(
name = "finisher_plotter",
srcs = ["finisher_plotter.ts"],
target_compatible_with = ["@platforms//os:linux"],
@@ -163,7 +163,7 @@
],
)
-ts_library(
+ts_project(
name = "accelerator_plotter",
srcs = ["accelerator_plotter.ts"],
target_compatible_with = ["@platforms//os:linux"],
@@ -174,7 +174,7 @@
],
)
-ts_library(
+ts_project(
name = "hood_plotter",
srcs = ["hood_plotter.ts"],
target_compatible_with = ["@platforms//os:linux"],
diff --git a/y2020/www/BUILD b/y2020/www/BUILD
index 8467994..0c05900 100644
--- a/y2020/www/BUILD
+++ b/y2020/www/BUILD
@@ -1,8 +1,7 @@
-load("@npm//@bazel/typescript:index.bzl", "ts_library")
-load("//tools/build_rules:js.bzl", "rollup_bundle")
+load("//tools/build_rules:js.bzl", "rollup_bundle", "ts_project")
load("//frc971/downloader:downloader.bzl", "aos_downloader_dir")
-ts_library(
+ts_project(
name = "camera_main",
srcs = [
"camera_main.ts",
@@ -21,7 +20,7 @@
],
)
-ts_library(
+ts_project(
name = "field_main",
srcs = [
"constants.ts",
diff --git a/y2021_bot3/control_loops/superstructure/BUILD b/y2021_bot3/control_loops/superstructure/BUILD
index f5b5b3b..be18cbe 100644
--- a/y2021_bot3/control_loops/superstructure/BUILD
+++ b/y2021_bot3/control_loops/superstructure/BUILD
@@ -1,5 +1,5 @@
+load("//tools/build_rules:js.bzl", "ts_project")
load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
-load("@npm//@bazel/typescript:index.bzl", "ts_library")
package(default_visibility = ["//visibility:public"])
@@ -104,7 +104,7 @@
],
)
-ts_library(
+ts_project(
name = "superstructure_plotter",
srcs = ["superstructure_plotter.ts"],
target_compatible_with = ["@platforms//os:linux"],
diff --git a/y2022/control_loops/superstructure/BUILD b/y2022/control_loops/superstructure/BUILD
index 3bc1297..a085049 100644
--- a/y2022/control_loops/superstructure/BUILD
+++ b/y2022/control_loops/superstructure/BUILD
@@ -1,6 +1,6 @@
+load("//tools/build_rules:js.bzl", "ts_project")
load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
-load("@npm//@bazel/typescript:index.bzl", "ts_library")
package(default_visibility = ["//visibility:public"])
@@ -200,7 +200,7 @@
],
)
-ts_library(
+ts_project(
name = "superstructure_plotter",
srcs = ["superstructure_plotter.ts"],
target_compatible_with = ["@platforms//os:linux"],
@@ -211,7 +211,7 @@
],
)
-ts_library(
+ts_project(
name = "catapult_plotter",
srcs = ["catapult_plotter.ts"],
target_compatible_with = ["@platforms//os:linux"],
@@ -222,7 +222,7 @@
],
)
-ts_library(
+ts_project(
name = "intake_plotter",
srcs = ["intake_plotter.ts"],
target_compatible_with = ["@platforms//os:linux"],
@@ -233,7 +233,7 @@
],
)
-ts_library(
+ts_project(
name = "turret_plotter",
srcs = ["turret_plotter.ts"],
target_compatible_with = ["@platforms//os:linux"],
@@ -244,7 +244,7 @@
],
)
-ts_library(
+ts_project(
name = "climber_plotter",
srcs = ["climber_plotter.ts"],
target_compatible_with = ["@platforms//os:linux"],
diff --git a/y2022/localizer/BUILD b/y2022/localizer/BUILD
index 2a0a9fe..1d45516 100644
--- a/y2022/localizer/BUILD
+++ b/y2022/localizer/BUILD
@@ -1,9 +1,9 @@
+load("//tools/build_rules:js.bzl", "ts_project")
load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
load("//aos:flatbuffers.bzl", "cc_static_flatbuffer")
-load("@npm//@bazel/typescript:index.bzl", "ts_library")
-ts_library(
+ts_project(
name = "localizer_plotter",
srcs = ["localizer_plotter.ts"],
target_compatible_with = ["@platforms//os:linux"],
diff --git a/y2022/vision/BUILD b/y2022/vision/BUILD
index 31644e4..8d8a43c 100644
--- a/y2022/vision/BUILD
+++ b/y2022/vision/BUILD
@@ -1,8 +1,8 @@
+load("//tools/build_rules:js.bzl", "ts_project")
load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
-load("@npm//@bazel/typescript:index.bzl", "ts_library")
-ts_library(
+ts_project(
name = "vision_plotter",
srcs = ["vision_plotter.ts"],
target_compatible_with = ["@platforms//os:linux"],
diff --git a/y2022/www/BUILD b/y2022/www/BUILD
index 957045d..386ca90 100644
--- a/y2022/www/BUILD
+++ b/y2022/www/BUILD
@@ -1,5 +1,4 @@
-load("@npm//@bazel/typescript:index.bzl", "ts_library")
-load("//tools/build_rules:js.bzl", "rollup_bundle")
+load("//tools/build_rules:js.bzl", "rollup_bundle", "ts_project")
load("//frc971/downloader:downloader.bzl", "aos_downloader_dir")
filegroup(
@@ -12,7 +11,7 @@
visibility = ["//visibility:public"],
)
-ts_library(
+ts_project(
name = "field_main",
srcs = [
"constants.ts",
diff --git a/y2023/BUILD b/y2023/BUILD
index c060688..32965ae 100644
--- a/y2023/BUILD
+++ b/y2023/BUILD
@@ -3,6 +3,35 @@
load("//tools/build_rules:template.bzl", "jinja2_template")
robot_downloader(
+ binaries = [
+ "//aos/network:web_proxy_main",
+ "//aos/events/logging:log_cat",
+ ],
+ data = [
+ ":aos_config",
+ "@ctre_phoenix_api_cpp_athena//:shared_libraries",
+ "@ctre_phoenix_cci_athena//:shared_libraries",
+ "@ctre_phoenixpro_api_cpp_athena//:shared_libraries",
+ "@ctre_phoenixpro_tools_athena//:shared_libraries",
+ ],
+ dirs = [
+ "//y2023/www:www_files",
+ ],
+ start_binaries = [
+ "//aos/events/logging:logger_main",
+ "//aos/network:web_proxy_main",
+ ":joystick_reader",
+ ":wpilib_interface",
+ "//aos/network:message_bridge_client",
+ "//aos/network:message_bridge_server",
+ "//y2023/control_loops/drivetrain:drivetrain",
+ "//y2023/control_loops/drivetrain:trajectory_generator",
+ "//y2023/control_loops/superstructure:superstructure",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+)
+
+robot_downloader(
name = "pi_download",
binaries = [
"//frc971/vision:intrinsics_calibration",
@@ -148,6 +177,7 @@
"//aos/network:timestamp_fbs",
"//y2019/control_loops/drivetrain:target_selector_fbs",
"//y2023/control_loops/superstructure:superstructure_goal_fbs",
+ "//y2023/control_loops/drivetrain:drivetrain_can_position_fbs",
"//y2023/control_loops/superstructure:superstructure_output_fbs",
"//y2023/control_loops/superstructure:superstructure_position_fbs",
"//y2023/control_loops/superstructure:superstructure_status_fbs",
@@ -204,6 +234,7 @@
":constants",
"//aos:init",
"//aos:math",
+ "//aos/containers:sized_array",
"//aos/events:shm_event_loop",
"//aos/logging",
"//aos/stl_mutex",
@@ -230,7 +261,9 @@
"//frc971/wpilib:wpilib_interface",
"//frc971/wpilib:wpilib_robot_base",
"//third_party:phoenix",
+ "//third_party:phoenixpro",
"//third_party:wpilib",
+ "//y2023/control_loops/drivetrain:drivetrain_can_position_fbs",
"//y2023/control_loops/superstructure:superstructure_output_fbs",
"//y2023/control_loops/superstructure:superstructure_position_fbs",
],
diff --git a/y2023/constants.h b/y2023/constants.h
index 03a6cae..edcb794 100644
--- a/y2023/constants.h
+++ b/y2023/constants.h
@@ -20,6 +20,10 @@
struct Values {
static const int kZeroingSampleSize = 200;
+ static const int kDrivetrainWriterPriority = 35;
+ static const int kDrivetrainTxPriority = 36;
+ static const int kDrivetrainRxPriority = 36;
+
static constexpr double kDrivetrainCyclesPerRevolution() { return 512.0; }
static constexpr double kDrivetrainEncoderCountsPerRevolution() {
return kDrivetrainCyclesPerRevolution() * 4;
@@ -32,6 +36,9 @@
kDrivetrainEncoderCountsPerRevolution();
}
+ static constexpr double kDrivetrainSupplyCurrentLimit() { return 35.0; }
+ static constexpr double kDrivetrainStatorCurrentLimit() { return 40.0; }
+
static double DrivetrainEncoderToMeters(int32_t in) {
return ((static_cast<double>(in) /
kDrivetrainEncoderCountsPerRevolution()) *
@@ -39,6 +46,12 @@
kDrivetrainEncoderRatio() * control_loops::drivetrain::kWheelRadius;
}
+ static double DrivetrainCANEncoderToMeters(double rotations) {
+ return (rotations * (2.0 * M_PI)) *
+ control_loops::drivetrain::kHighOutputRatio *
+ control_loops::drivetrain::kWheelRadius;
+ }
+
struct PotConstants {
::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
::frc971::zeroing::RelativeEncoderZeroingEstimator>
diff --git a/y2023/control_loops/drivetrain/BUILD b/y2023/control_loops/drivetrain/BUILD
index 8a06d00..ac00e2b 100644
--- a/y2023/control_loops/drivetrain/BUILD
+++ b/y2023/control_loops/drivetrain/BUILD
@@ -1,4 +1,5 @@
load("//aos:config.bzl", "aos_config")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
genrule(
name = "genrule_drivetrain",
@@ -111,3 +112,12 @@
"//frc971/control_loops/drivetrain:trajectory_generator",
],
)
+
+flatbuffer_cc_library(
+ name = "drivetrain_can_position_fbs",
+ srcs = [
+ "drivetrain_can_position.fbs",
+ ],
+ gen_reflections = 1,
+ visibility = ["//visibility:public"],
+)
diff --git a/y2023/control_loops/drivetrain/drivetrain_can_position.fbs b/y2023/control_loops/drivetrain/drivetrain_can_position.fbs
new file mode 100644
index 0000000..10d7c9a
--- /dev/null
+++ b/y2023/control_loops/drivetrain/drivetrain_can_position.fbs
@@ -0,0 +1,39 @@
+namespace y2023.control_loops.drivetrain;
+
+table CANFalcon {
+ // The CAN id of the falcon
+ id:int (id: 0);
+
+ // In Amps
+ supply_current:float (id: 1);
+
+ // In Amps
+ // Stator current where positive current means torque is applied in
+ // the motor's forward direction as determined by its Inverted setting.
+ torque_current:float (id: 2);
+
+ // In Volts
+ supply_voltage:float (id: 3);
+
+ // In degrees Celsius
+ device_temp:float (id: 4);
+
+ // In meters traveled on the drivetrain
+ position:float (id: 5);
+}
+
+// CAN readings from the CAN sensor reader loop
+table CANPosition {
+ falcons: [CANFalcon] (id: 0);
+
+ // The timestamp of the measurement on the canivore clock in nanoseconds
+ // This will have less jitter than the
+ // timestamp of the message being sent out.
+ timestamp:int64 (id: 1);
+
+ // The ctre::phoenix::StatusCode of the measurement
+ // Should be OK = 0
+ status:int (id: 2);
+}
+
+root_type CANPosition;
diff --git a/y2023/control_loops/drivetrain/drivetrain_main.cc b/y2023/control_loops/drivetrain/drivetrain_main.cc
index cf8aa8a..0817b3d 100644
--- a/y2023/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2023/control_loops/drivetrain/drivetrain_main.cc
@@ -19,6 +19,9 @@
std::make_unique<::frc971::control_loops::drivetrain::DeadReckonEkf>(
&event_loop,
::y2023::control_loops::drivetrain::GetDrivetrainConfig());
+ std::unique_ptr<DrivetrainLoop> drivetrain = std::make_unique<DrivetrainLoop>(
+ y2023::control_loops::drivetrain::GetDrivetrainConfig(), &event_loop,
+ localizer.get());
event_loop.Run();
diff --git a/y2023/control_loops/python/graph_edit.py b/y2023/control_loops/python/graph_edit.py
index 448103b..3ef9d07 100644
--- a/y2023/control_loops/python/graph_edit.py
+++ b/y2023/control_loops/python/graph_edit.py
@@ -11,17 +11,30 @@
gi.require_version('Gtk', '3.0')
from gi.repository import Gdk, Gtk
import cairo
-from graph_tools import XYSegment, AngleSegment, to_theta, to_xy, alpha_blend, draw_lines
-from graph_tools import back_to_xy_loop, subdivide_theta, to_theta_loop, px
+from graph_tools import XYSegment, AngleSegment, to_theta, to_xy, alpha_blend
+from graph_tools import back_to_xy_loop, subdivide_theta, to_theta_loop
from graph_tools import l1, l2, joint_center
import graph_paths
-from frc971.control_loops.python.basic_window import quit_main_loop, set_color
+from frc971.control_loops.python.basic_window import quit_main_loop, set_color, OverrideMatrix, identity
import shapely
from shapely.geometry import Polygon
+def px(cr):
+ return OverrideMatrix(cr, identity)
+
+
+# Draw lines to cr + stroke.
+def draw_lines(cr, lines):
+ cr.move_to(lines[0][0], lines[0][1])
+ for pt in lines[1:]:
+ cr.line_to(pt[0], pt[1])
+ with px(cr):
+ cr.stroke()
+
+
def draw_px_cross(cr, length_px):
"""Draws a cross with fixed dimensions in pixel space."""
with px(cr):
diff --git a/y2023/control_loops/python/graph_paths.py b/y2023/control_loops/python/graph_paths.py
index da0ad4f..6d8f489 100644
--- a/y2023/control_loops/python/graph_paths.py
+++ b/y2023/control_loops/python/graph_paths.py
@@ -17,6 +17,12 @@
circular_index=-1)
cone_perch_pos = to_theta_with_circular_index(1.0, 2.0, circular_index=-1)
+points = []
+front_points = []
+back_points = []
+unnamed_segments = []
+named_segments = []
+
segments = [
ThetaSplineSegment(neutral, neutral_to_cone_1, neutral_to_cone_2, cone_pos,
"NeutralToCone"),
diff --git a/y2023/control_loops/python/graph_tools.py b/y2023/control_loops/python/graph_tools.py
index 3ddfb37..dafa294 100644
--- a/y2023/control_loops/python/graph_tools.py
+++ b/y2023/control_loops/python/graph_tools.py
@@ -1,7 +1,4 @@
import numpy
-import cairo
-
-from frc971.control_loops.python.basic_window import OverrideMatrix, identity
# joint_center in x-y space.
joint_center = (-0.299, 0.299)
@@ -16,10 +13,6 @@
theta_end_circle_size = 0.07
-def px(cr):
- return OverrideMatrix(cr, identity)
-
-
# Convert from x-y coordinates to theta coordinates.
# orientation is a bool. This orientation is circular_index mod 2.
# where circular_index is the circular index, or the position in the
@@ -215,13 +208,11 @@
return (a[0], a[1], d1[0], d1[1], accel[0], accel[1])
-# Draw lines to cr + stroke.
+# Draw a list of lines to a cairo context.
def draw_lines(cr, lines):
cr.move_to(lines[0][0], lines[0][1])
for pt in lines[1:]:
cr.line_to(pt[0], pt[1])
- with px(cr):
- cr.stroke()
# Segment in angle space.
diff --git a/y2023/control_loops/superstructure/BUILD b/y2023/control_loops/superstructure/BUILD
index 5246b3e..718a575 100644
--- a/y2023/control_loops/superstructure/BUILD
+++ b/y2023/control_loops/superstructure/BUILD
@@ -1,6 +1,6 @@
+load("//tools/build_rules:js.bzl", "ts_project")
load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
-load("@npm//@bazel/typescript:index.bzl", "ts_library")
package(default_visibility = ["//visibility:public"])
@@ -133,7 +133,7 @@
],
)
-ts_library(
+ts_project(
name = "superstructure_plotter",
srcs = ["superstructure_plotter.ts"],
target_compatible_with = ["@platforms//os:linux"],
diff --git a/y2023/wpilib_interface.cc b/y2023/wpilib_interface.cc
index 77ff5af..690d4bf 100644
--- a/y2023/wpilib_interface.cc
+++ b/y2023/wpilib_interface.cc
@@ -23,6 +23,7 @@
#undef ERROR
#include "aos/commonmath.h"
+#include "aos/containers/sized_array.h"
#include "aos/events/event_loop.h"
#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
@@ -32,8 +33,10 @@
#include "aos/util/log_interval.h"
#include "aos/util/phased_loop.h"
#include "aos/util/wrapping_counter.h"
+#include "ctre/phoenix/cci/Diagnostics_CCI.h"
#include "ctre/phoenix/motorcontrol/can/TalonFX.h"
#include "ctre/phoenix/motorcontrol/can/TalonSRX.h"
+#include "ctre/phoenixpro/TalonFX.hpp"
#include "frc971/autonomous/auto_mode_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
#include "frc971/input/robot_state_generated.h"
@@ -51,12 +54,18 @@
#include "frc971/wpilib/sensor_reader.h"
#include "frc971/wpilib/wpilib_robot_base.h"
#include "y2023/constants.h"
+#include "y2023/control_loops/drivetrain/drivetrain_can_position_generated.h"
#include "y2023/control_loops/superstructure/superstructure_output_generated.h"
#include "y2023/control_loops/superstructure/superstructure_position_generated.h"
+DEFINE_bool(ctre_diag_server, false,
+ "If true, enable the diagnostics server for interacting with "
+ "devices on the CAN bus using Phoenix Tuner");
+
using ::aos::monotonic_clock;
using ::y2023::constants::Values;
namespace superstructure = ::y2023::control_loops::superstructure;
+namespace drivetrain = ::y2023::control_loops::drivetrain;
namespace chrono = ::std::chrono;
using std::make_unique;
@@ -122,6 +131,16 @@
autonomous_modes_.at(i) = ::std::move(sensor);
}
+ void set_heading_input(::std::unique_ptr<frc::DigitalInput> sensor) {
+ imu_heading_input_ = ::std::move(sensor);
+ imu_heading_reader_.set_input(imu_heading_input_.get());
+ }
+
+ void set_yaw_rate_input(::std::unique_ptr<frc::DigitalInput> sensor) {
+ imu_yaw_rate_input_ = ::std::move(sensor);
+ imu_yaw_rate_reader_.set_input(imu_yaw_rate_input_.get());
+ }
+
void RunIteration() override {
superstructure_reading_->Set(true);
{ auto builder = superstructure_position_sender_.MakeBuilder(); }
@@ -220,6 +239,8 @@
std::array<std::unique_ptr<frc::DigitalInput>, 2> autonomous_modes_;
+ std::unique_ptr<frc::DigitalInput> imu_heading_input_, imu_yaw_rate_input_;
+
frc971::wpilib::DMAPulseWidthReader imu_heading_reader_, imu_yaw_rate_reader_;
};
@@ -256,6 +277,326 @@
}
};
+static constexpr int kCANFalconCount = 6;
+static constexpr int kCANSignalsCount = 3;
+static constexpr units::frequency::hertz_t kCANUpdateFreqHz = 200_Hz;
+
+class Falcon {
+ public:
+ Falcon(int device_id, std::string canbus,
+ aos::SizedArray<ctre::phoenixpro::BaseStatusSignalValue *,
+ kCANFalconCount * kCANSignalsCount> *signals)
+ : talon_(device_id, canbus),
+ device_id_(device_id),
+ device_temp_(talon_.GetDeviceTemp()),
+ supply_voltage_(talon_.GetSupplyVoltage()),
+ supply_current_(talon_.GetSupplyCurrent()),
+ torque_current_(talon_.GetTorqueCurrent()),
+ position_(talon_.GetPosition()) {
+ // device temp is not timesynced so don't add it to the list of signals
+ device_temp_.SetUpdateFrequency(kCANUpdateFreqHz);
+
+ CHECK_EQ(kCANSignalsCount, 3);
+ CHECK_NOTNULL(signals);
+ CHECK_LE(signals->size() + 3u, signals->capacity());
+
+ supply_voltage_.SetUpdateFrequency(kCANUpdateFreqHz);
+ signals->push_back(&supply_voltage_);
+
+ supply_current_.SetUpdateFrequency(kCANUpdateFreqHz);
+ signals->push_back(&supply_current_);
+
+ torque_current_.SetUpdateFrequency(kCANUpdateFreqHz);
+ signals->push_back(&torque_current_);
+
+ position_.SetUpdateFrequency(kCANUpdateFreqHz);
+ signals->push_back(&position_);
+ }
+
+ void WriteConfigs(ctre::phoenixpro::signals::InvertedValue invert) {
+ inverted_ = invert;
+
+ ctre::phoenixpro::configs::CurrentLimitsConfigs current_limits;
+ current_limits.StatorCurrentLimit =
+ constants::Values::kDrivetrainStatorCurrentLimit();
+ current_limits.StatorCurrentLimitEnable = true;
+ current_limits.SupplyCurrentLimit =
+ constants::Values::kDrivetrainSupplyCurrentLimit();
+ current_limits.SupplyCurrentLimitEnable = true;
+
+ ctre::phoenixpro::configs::MotorOutputConfigs output_configs;
+ output_configs.NeutralMode =
+ ctre::phoenixpro::signals::NeutralModeValue::Brake;
+ output_configs.DutyCycleNeutralDeadband = 0;
+
+ output_configs.Inverted = inverted_;
+
+ ctre::phoenixpro::configs::TalonFXConfiguration configuration;
+ configuration.CurrentLimits = current_limits;
+ configuration.MotorOutput = output_configs;
+
+ ctre::phoenix::StatusCode status =
+ talon_.GetConfigurator().Apply(configuration);
+ if (!status.IsOK()) {
+ AOS_LOG(ERROR, "Failed to set falcon configuration: %s: %s",
+ status.GetName(), status.GetDescription());
+ }
+ }
+
+ ctre::phoenixpro::hardware::TalonFX *talon() { return &talon_; }
+
+ flatbuffers::Offset<drivetrain::CANFalcon> WritePosition(
+ flatbuffers::FlatBufferBuilder *fbb) {
+ drivetrain::CANFalcon::Builder builder(*fbb);
+ builder.add_id(device_id_);
+ builder.add_device_temp(device_temp_.GetValue().value());
+ builder.add_supply_voltage(supply_voltage_.GetValue().value());
+ builder.add_supply_current(supply_current_.GetValue().value());
+ builder.add_torque_current(torque_current_.GetValue().value());
+
+ double invert =
+ (inverted_ ==
+ ctre::phoenixpro::signals::InvertedValue::Clockwise_Positive
+ ? 1
+ : -1);
+
+ builder.add_position(constants::Values::DrivetrainCANEncoderToMeters(
+ position_.GetValue().value()) *
+ invert);
+
+ return builder.Finish();
+ }
+
+ // returns the monotonic timestamp of the latest timesynced reading in the
+ // timebase of the the syncronized CAN bus clock.
+ int64_t GetTimestamp() {
+ std::chrono::nanoseconds latest_timestamp =
+ torque_current_.GetTimestamp().GetTime();
+
+ return latest_timestamp.count();
+ }
+
+ void RefreshNontimesyncedSignals() { device_temp_.Refresh(); };
+
+ private:
+ ctre::phoenixpro::hardware::TalonFX talon_;
+ int device_id_;
+
+ ctre::phoenixpro::signals::InvertedValue inverted_;
+
+ ctre::phoenixpro::StatusSignalValue<units::temperature::celsius_t>
+ device_temp_;
+ ctre::phoenixpro::StatusSignalValue<units::voltage::volt_t> supply_voltage_;
+ ctre::phoenixpro::StatusSignalValue<units::current::ampere_t> supply_current_,
+ torque_current_;
+ ctre::phoenixpro::StatusSignalValue<units::angle::turn_t> position_;
+};
+
+class CANSensorReader {
+ public:
+ CANSensorReader(aos::EventLoop *event_loop)
+ : event_loop_(event_loop),
+ can_position_sender_(
+ event_loop->MakeSender<drivetrain::CANPosition>("/drivetrain")) {
+ event_loop->SetRuntimeRealtimePriority(35);
+ timer_handler_ = event_loop->AddTimer([this]() { Loop(); });
+ timer_handler_->set_name("CANSensorReader Loop");
+
+ event_loop->OnRun([this]() {
+ timer_handler_->Setup(event_loop_->monotonic_now(), 1 / kCANUpdateFreqHz);
+ });
+ }
+
+ aos::SizedArray<ctre::phoenixpro::BaseStatusSignalValue *,
+ kCANFalconCount * kCANSignalsCount>
+ *get_signals_registry() {
+ return &signals_;
+ };
+
+ void set_falcons(std::shared_ptr<Falcon> right_front,
+ std::shared_ptr<Falcon> right_back,
+ std::shared_ptr<Falcon> right_under,
+ std::shared_ptr<Falcon> left_front,
+ std::shared_ptr<Falcon> left_back,
+ std::shared_ptr<Falcon> left_under) {
+ right_front_ = std::move(right_front);
+ right_back_ = std::move(right_back);
+ right_under_ = std::move(right_under);
+ left_front_ = std::move(left_front);
+ left_back_ = std::move(left_back);
+ left_under_ = std::move(left_under);
+ }
+
+ private:
+ void Loop() {
+ CHECK_EQ(signals_.size(), 18u);
+ ctre::phoenix::StatusCode status =
+ ctre::phoenixpro::BaseStatusSignalValue::WaitForAll(
+ 2000_ms,
+ {signals_[0], signals_[1], signals_[2], signals_[3], signals_[4],
+ signals_[5], signals_[6], signals_[7], signals_[8], signals_[9],
+ signals_[10], signals_[11], signals_[12], signals_[13],
+ signals_[14], signals_[15], signals_[16], signals_[17]});
+
+ if (!status.IsOK()) {
+ AOS_LOG(ERROR, "Failed to read signals from falcons: %s: %s",
+ status.GetName(), status.GetDescription());
+ }
+
+ auto builder = can_position_sender_.MakeBuilder();
+
+ for (auto falcon : {right_front_, right_back_, right_under_, left_front_,
+ left_back_, left_under_}) {
+ falcon->RefreshNontimesyncedSignals();
+ }
+
+ aos::SizedArray<flatbuffers::Offset<drivetrain::CANFalcon>, kCANFalconCount>
+ falcons;
+
+ for (auto falcon : {right_front_, right_back_, right_under_, left_front_,
+ left_back_, left_under_}) {
+ falcons.push_back(falcon->WritePosition(builder.fbb()));
+ }
+
+ auto falcons_list =
+ builder.fbb()->CreateVector<flatbuffers::Offset<drivetrain::CANFalcon>>(
+ falcons);
+
+ drivetrain::CANPosition::Builder can_position_builder =
+ builder.MakeBuilder<drivetrain::CANPosition>();
+
+ can_position_builder.add_falcons(falcons_list);
+ can_position_builder.add_timestamp(right_front_->GetTimestamp());
+ can_position_builder.add_status(static_cast<int>(status));
+
+ builder.CheckOk(builder.Send(can_position_builder.Finish()));
+ }
+
+ aos::EventLoop *event_loop_;
+
+ aos::SizedArray<ctre::phoenixpro::BaseStatusSignalValue *,
+ kCANFalconCount * kCANSignalsCount>
+ signals_;
+ aos::Sender<drivetrain::CANPosition> can_position_sender_;
+
+ std::shared_ptr<Falcon> right_front_, right_back_, right_under_, left_front_,
+ left_back_, left_under_;
+
+ // Pointer to the timer handler used to modify the wakeup.
+ ::aos::TimerHandler *timer_handler_;
+};
+
+class DrivetrainWriter : public ::frc971::wpilib::LoopOutputHandler<
+ ::frc971::control_loops::drivetrain::Output> {
+ public:
+ DrivetrainWriter(::aos::EventLoop *event_loop)
+ : ::frc971::wpilib::LoopOutputHandler<
+ ::frc971::control_loops::drivetrain::Output>(event_loop,
+ "/drivetrain") {
+ event_loop->SetRuntimeRealtimePriority(
+ constants::Values::kDrivetrainWriterPriority);
+
+ if (!FLAGS_ctre_diag_server) {
+ c_Phoenix_Diagnostics_SetSecondsToStart(-1);
+ c_Phoenix_Diagnostics_Dispose();
+ }
+
+ ctre::phoenix::platform::can::CANComm_SetRxSchedPriority(
+ constants::Values::kDrivetrainRxPriority, true, "Drivetrain Bus");
+ ctre::phoenix::platform::can::CANComm_SetTxSchedPriority(
+ constants::Values::kDrivetrainTxPriority, true, "Drivetrain Bus");
+
+ event_loop->OnRun([this]() { WriteConfigs(); });
+ }
+
+ void set_falcons(std::shared_ptr<Falcon> right_front,
+ std::shared_ptr<Falcon> right_back,
+ std::shared_ptr<Falcon> right_under,
+ std::shared_ptr<Falcon> left_front,
+ std::shared_ptr<Falcon> left_back,
+ std::shared_ptr<Falcon> left_under) {
+ right_front_ = std::move(right_front);
+ right_back_ = std::move(right_back);
+ right_under_ = std::move(right_under);
+ left_front_ = std::move(left_front);
+ left_back_ = std::move(left_back);
+ left_under_ = std::move(left_under);
+ }
+
+ void set_right_inverted(ctre::phoenixpro::signals::InvertedValue invert) {
+ right_inverted_ = invert;
+ }
+
+ void set_left_inverted(ctre::phoenixpro::signals::InvertedValue invert) {
+ left_inverted_ = invert;
+ }
+
+ private:
+ void WriteConfigs() {
+ for (auto falcon :
+ {right_front_.get(), right_back_.get(), right_under_.get()}) {
+ falcon->WriteConfigs(right_inverted_);
+ }
+
+ for (auto falcon :
+ {left_front_.get(), left_back_.get(), left_under_.get()}) {
+ falcon->WriteConfigs(left_inverted_);
+ }
+ }
+
+ void Write(
+ const ::frc971::control_loops::drivetrain::Output &output) override {
+ ctre::phoenixpro::controls::DutyCycleOut left_control(
+ SafeSpeed(output.left_voltage()));
+ left_control.UpdateFreqHz = 0_Hz;
+ left_control.EnableFOC = true;
+
+ ctre::phoenixpro::controls::DutyCycleOut right_control(
+ SafeSpeed(output.right_voltage()));
+ right_control.UpdateFreqHz = 0_Hz;
+ right_control.EnableFOC = true;
+
+ for (auto falcon :
+ {left_front_.get(), left_back_.get(), left_under_.get()}) {
+ ctre::phoenix::StatusCode status =
+ falcon->talon()->SetControl(left_control);
+
+ if (!status.IsOK()) {
+ AOS_LOG(ERROR, "Failed to write control to falcon: %s: %s",
+ status.GetName(), status.GetDescription());
+ }
+ }
+
+ for (auto falcon :
+ {right_front_.get(), right_back_.get(), right_under_.get()}) {
+ ctre::phoenix::StatusCode status =
+ falcon->talon()->SetControl(right_control);
+
+ if (!status.IsOK()) {
+ AOS_LOG(ERROR, "Failed to write control to falcon: %s: %s",
+ status.GetName(), status.GetDescription());
+ }
+ }
+ }
+
+ void Stop() override {
+ AOS_LOG(WARNING, "drivetrain output too old\n");
+ ctre::phoenixpro::controls::NeutralOut stop_command;
+ for (auto falcon :
+ {right_front_.get(), right_back_.get(), right_under_.get(),
+ left_front_.get(), left_back_.get(), left_under_.get()}) {
+ falcon->talon()->SetControl(stop_command);
+ }
+ }
+
+ double SafeSpeed(double voltage) {
+ return (::aos::Clip(voltage, -kMaxBringupPower, kMaxBringupPower) / 12.0);
+ }
+
+ ctre::phoenixpro::signals::InvertedValue left_inverted_, right_inverted_;
+ std::shared_ptr<Falcon> right_front_, right_back_, right_under_, left_front_,
+ left_back_, left_under_;
+};
class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
public:
@@ -272,6 +613,11 @@
aos::configuration::ReadConfig("aos_config.json");
// Thread 1.
+ ::aos::ShmEventLoop joystick_sender_event_loop(&config.message());
+ ::frc971::wpilib::JoystickSender joystick_sender(
+ &joystick_sender_event_loop);
+ AddLoop(&joystick_sender_event_loop);
+
// Thread 2.
::aos::ShmEventLoop pdp_fetcher_event_loop(&config.message());
::frc971::wpilib::PDPFetcher pdp_fetcher(&pdp_fetcher_event_loop);
@@ -288,11 +634,43 @@
sensor_reader.set_drivetrain_right_encoder(make_encoder(0));
sensor_reader.set_superstructure_reading(superstructure_reading);
+ sensor_reader.set_heading_input(make_unique<frc::DigitalInput>(9));
+ sensor_reader.set_yaw_rate_input(make_unique<frc::DigitalInput>(8));
+
AddLoop(&sensor_reader_event_loop);
// Thread 4.
+ ::aos::ShmEventLoop can_sensor_reader_event_loop(&config.message());
+ CANSensorReader can_sensor_reader(&can_sensor_reader_event_loop);
+
+ std::shared_ptr<Falcon> right_front = std::make_shared<Falcon>(
+ 1, "Drivetrain Bus", can_sensor_reader.get_signals_registry());
+ std::shared_ptr<Falcon> right_back = std::make_shared<Falcon>(
+ 2, "Drivetrain Bus", can_sensor_reader.get_signals_registry());
+ std::shared_ptr<Falcon> right_under = std::make_shared<Falcon>(
+ 3, "Drivetrain Bus", can_sensor_reader.get_signals_registry());
+ std::shared_ptr<Falcon> left_front = std::make_shared<Falcon>(
+ 4, "Drivetrain Bus", can_sensor_reader.get_signals_registry());
+ std::shared_ptr<Falcon> left_back = std::make_shared<Falcon>(
+ 5, "Drivetrain Bus", can_sensor_reader.get_signals_registry());
+ std::shared_ptr<Falcon> left_under = std::make_shared<Falcon>(
+ 6, "Drivetrain Bus", can_sensor_reader.get_signals_registry());
+
+ can_sensor_reader.set_falcons(right_front, right_back, right_under,
+ left_front, left_back, left_under);
+
+ AddLoop(&can_sensor_reader_event_loop);
+
+ // Thread 5.
::aos::ShmEventLoop output_event_loop(&config.message());
- ::frc971::wpilib::DrivetrainWriter drivetrain_writer(&output_event_loop);
+ DrivetrainWriter drivetrain_writer(&output_event_loop);
+
+ drivetrain_writer.set_falcons(right_front, right_back, right_under,
+ left_front, left_back, left_under);
+ drivetrain_writer.set_right_inverted(
+ ctre::phoenixpro::signals::InvertedValue::Clockwise_Positive);
+ drivetrain_writer.set_left_inverted(
+ ctre::phoenixpro::signals::InvertedValue::CounterClockwise_Positive);
SuperstructureWriter superstructure_writer(&output_event_loop);
diff --git a/y2023/www/BUILD b/y2023/www/BUILD
index 5dae3c6..539fa6d 100644
--- a/y2023/www/BUILD
+++ b/y2023/www/BUILD
@@ -1,5 +1,4 @@
-load("@npm//@bazel/typescript:index.bzl", "ts_library")
-load("//tools/build_rules:js.bzl", "rollup_bundle")
+load("//tools/build_rules:js.bzl", "rollup_bundle", "ts_project")
load("//frc971/downloader:downloader.bzl", "aos_downloader_dir")
filegroup(
@@ -12,7 +11,7 @@
visibility = ["//visibility:public"],
)
-ts_library(
+ts_project(
name = "field_main",
srcs = [
"constants.ts",
diff --git a/y2023/y2023_roborio.json b/y2023/y2023_roborio.json
index 572aadc..e12d612 100644
--- a/y2023/y2023_roborio.json
+++ b/y2023/y2023_roborio.json
@@ -287,6 +287,14 @@
},
{
"name": "/drivetrain",
+ "type": "y2023.control_loops.drivetrain.CANPosition",
+ "source_node": "roborio",
+ "frequency": 220,
+ "num_senders": 2,
+ "max_size": 400
+ },
+ {
+ "name": "/drivetrain",
"type": "frc971.sensors.GyroReading",
"source_node": "roborio",
"frequency": 200,
@@ -490,7 +498,7 @@
]
},
{
- "name": "web_proxy",
+ "name": "roborio_web_proxy",
"executable_name": "web_proxy_main",
"args": ["--min_ice_port=5800", "--max_ice_port=5810"],
"nodes": [
@@ -506,7 +514,7 @@
]
},
{
- "name": "message_bridge_server",
+ "name": "roborio_message_bridge_server",
"executable_name": "message_bridge_server",
"args": ["--rt_priority=16"],
"nodes": [